mirror of https://github.com/arendst/Tasmota.git
works fine...
This commit is contained in:
parent
e516262904
commit
4e1480d0f0
|
@ -1,18 +1,18 @@
|
||||||
/* This library is free software; you can redistribute it and/or
|
/*
|
||||||
* modify it under the terms of the GNU Lesser General Public
|
This library is free software: you can redistribute it and/or modify
|
||||||
* License as published by the Free Software Foundation; either
|
it under the terms of the GNU General Public License as published by
|
||||||
* version 2.1 of the License, or (at your option) any later version.
|
the Free Software Foundation, either version 3 of the License, or
|
||||||
*
|
(at your option) any later version.
|
||||||
* This library is distributed in the hope that it will be useful,
|
|
||||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
This library is distributed in the hope that it will be useful,
|
||||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||||
* Lesser General Public License for more details.
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||||
*
|
GNU General Public License for more details.
|
||||||
* You should have received a copy of the GNU Lesser General Public
|
|
||||||
* License along with this library; if not, write to the Free Software
|
You should have received a copy of the GNU General Public License
|
||||||
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||||
*
|
|
||||||
* Drives a bipolar motor, controlled by A4988 stepper driver circuit
|
Drives a bipolar motor, controlled by A4988 stepper driver circuit
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include "Arduino.h"
|
#include "Arduino.h"
|
||||||
|
@ -96,7 +96,7 @@ void A4988_Stepper::adjustMicrosteps() {
|
||||||
}
|
}
|
||||||
|
|
||||||
void A4988_Stepper::adjustDelay(void) {
|
void A4988_Stepper::adjustDelay(void) {
|
||||||
motor_delay = 60L * 1000L * 1000L / motor_SPR / motor_RPM / motor_MIS;
|
motor_delay = 60L * 1000L * 1000L / motor_SPR / motor_RPM / motor_MIS/2;
|
||||||
}
|
}
|
||||||
|
|
||||||
void A4988_Stepper::setMIS(short oneToSixteen) {
|
void A4988_Stepper::setMIS(short oneToSixteen) {
|
||||||
|
@ -147,8 +147,8 @@ void A4988_Stepper::doMove(long howManySteps)
|
||||||
if (now - last_time >= motor_delay) {
|
if (now - last_time >= motor_delay) {
|
||||||
digitalWrite(motor_stp_pin, lastStepWasHigh?LOW:HIGH);
|
digitalWrite(motor_stp_pin, lastStepWasHigh?LOW:HIGH);
|
||||||
lastStepWasHigh = !lastStepWasHigh;
|
lastStepWasHigh = !lastStepWasHigh;
|
||||||
// remeber step-time if last signal was HIGH we can pull low after 50ms as only HIGH actually moves the stepper
|
// remeber step-time
|
||||||
last_time = lastStepWasHigh?now-50:now;
|
last_time = now;
|
||||||
if (!lastStepWasHigh) steps_togo--; // same here - only HIGH moves, if pulled LOW step is completed...
|
if (!lastStepWasHigh) steps_togo--; // same here - only HIGH moves, if pulled LOW step is completed...
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -1,20 +1,19 @@
|
||||||
/* This library is free software; you can redistribute it and/or
|
/*
|
||||||
* modify it under the terms of the GNU Lesser General Public
|
This library is free software: you can redistribute it and/or modify
|
||||||
* License as published by the Free Software Foundation; either
|
it under the terms of the GNU General Public License as published by
|
||||||
* version 2.1 of the License, or (at your option) any later version.
|
the Free Software Foundation, either version 3 of the License, or
|
||||||
*
|
(at your option) any later version.
|
||||||
* This library is distributed in the hope that it will be useful,
|
|
||||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
This library is distributed in the hope that it will be useful,
|
||||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||||
* Lesser General Public License for more details.
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||||
*
|
GNU General Public License for more details.
|
||||||
* You should have received a copy of the GNU Lesser General Public
|
|
||||||
* License along with this library; if not, write to the Free Software
|
You should have received a copy of the GNU General Public License
|
||||||
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||||
*
|
|
||||||
* Drives a bipolar motor, controlled by A4988 stepper driver circuit
|
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
|
||||||
#ifndef A4988_Stepper_h
|
#ifndef A4988_Stepper_h
|
||||||
#define A4988_Stepper_h
|
#define A4988_Stepper_h
|
||||||
|
|
||||||
|
|
|
@ -218,7 +218,8 @@ upload_speed = 115200
|
||||||
upload_resetmethod = nodemcu
|
upload_resetmethod = nodemcu
|
||||||
|
|
||||||
; *** Upload Serial reset method for Wemos and NodeMCU
|
; *** Upload Serial reset method for Wemos and NodeMCU
|
||||||
upload_port = COM5
|
; upload_port = COM5
|
||||||
|
upload_port = /dev/cu.wchusbserial1410
|
||||||
extra_scripts = pio/strip-floats.py
|
extra_scripts = pio/strip-floats.py
|
||||||
|
|
||||||
; *** Upload file to OTA server using SCP
|
; *** Upload file to OTA server using SCP
|
||||||
|
|
|
@ -460,8 +460,6 @@
|
||||||
// Commands xdrv_25_A4988_Stepper.ino
|
// Commands xdrv_25_A4988_Stepper.ino
|
||||||
#ifdef USE_A4988_Stepper
|
#ifdef USE_A4988_Stepper
|
||||||
#define D_CMND_MOTOR "MOTOR"
|
#define D_CMND_MOTOR "MOTOR"
|
||||||
#define D_JSON_MOTOR_COMMAND "Command"
|
|
||||||
#define D_JSON_MOTOR_VALUE "Value"
|
|
||||||
#define D_JSON_MOTOR_MOVE "doMove"
|
#define D_JSON_MOTOR_MOVE "doMove"
|
||||||
#define D_JSON_MOTOR_ROTATE "doRotate"
|
#define D_JSON_MOTOR_ROTATE "doRotate"
|
||||||
#define D_JSON_MOTOR_TURN "doTurn"
|
#define D_JSON_MOTOR_TURN "doTurn"
|
||||||
|
|
|
@ -57,13 +57,13 @@
|
||||||
#define BOOT_LOOP_OFFSET 1 // [SetOption36] Number of boot loops before starting restoring defaults (0 = disable, 1..200 = boot loops offset)
|
#define BOOT_LOOP_OFFSET 1 // [SetOption36] Number of boot loops before starting restoring defaults (0 = disable, 1..200 = boot loops offset)
|
||||||
|
|
||||||
// -- Wifi ----------------------------------------
|
// -- Wifi ----------------------------------------
|
||||||
#define WIFI_IP_ADDRESS "0.0.0.0" // [IpAddress1] Set to 0.0.0.0 for using DHCP or enter a static IP address
|
#define WIFI_IP_ADDRESS "172.17." // [IpAddress1] Set to 0.0.0.0 for using DHCP or enter a static IP address
|
||||||
#define WIFI_GATEWAY "192.168.1.1" // [IpAddress2] If not using DHCP set Gateway IP address
|
#define WIFI_GATEWAY "172.17.0.1" // [IpAddress2] If not using DHCP set Gateway IP address
|
||||||
#define WIFI_SUBNETMASK "255.255.255.0" // [IpAddress3] If not using DHCP set Network mask
|
#define WIFI_SUBNETMASK "255.255.192.0" // [IpAddress3] If not using DHCP set Network mask
|
||||||
#define WIFI_DNS "192.168.1.1" // [IpAddress4] If not using DHCP set DNS IP address (might be equal to WIFI_GATEWAY)
|
#define WIFI_DNS "172.17.0.1" // [IpAddress4] If not using DHCP set DNS IP address (might be equal to WIFI_GATEWAY)
|
||||||
|
|
||||||
#define STA_SSID1 "" // [Ssid1] Wifi SSID
|
#define STA_SSID1 "Tim" // [Ssid1] Wifi SSID
|
||||||
#define STA_PASS1 "" // [Password1] Wifi password
|
#define STA_PASS1 "qwer1234!" // [Password1] Wifi password
|
||||||
#define STA_SSID2 "" // [Ssid2] Optional alternate AP Wifi SSID
|
#define STA_SSID2 "" // [Ssid2] Optional alternate AP Wifi SSID
|
||||||
#define STA_PASS2 "" // [Password2] Optional alternate AP Wifi password
|
#define STA_PASS2 "" // [Password2] Optional alternate AP Wifi password
|
||||||
#define WIFI_CONFIG_TOOL WIFI_RETRY // [WifiConfig] Default tool if wifi fails to connect
|
#define WIFI_CONFIG_TOOL WIFI_RETRY // [WifiConfig] Default tool if wifi fails to connect
|
||||||
|
@ -527,7 +527,7 @@
|
||||||
#define USE_SM16716 // Add support for SM16716 RGB LED controller (+0k7 code)
|
#define USE_SM16716 // Add support for SM16716 RGB LED controller (+0k7 code)
|
||||||
|
|
||||||
//#define USE_HRE // Add support for Badger HR-E Water Meter (+1k4 code)
|
//#define USE_HRE // Add support for Badger HR-E Water Meter (+1k4 code)
|
||||||
#define USE_A4988_Stepper // Add support for A4988 Stepper-Motors-Driver-circuit (+12k7 code)
|
#define USE_A4988_Stepper // Add support for A4988 Stepper-Motors-Driver-circuit (+10k4 code)
|
||||||
|
|
||||||
/*********************************************************************************************\
|
/*********************************************************************************************\
|
||||||
* Debug features
|
* Debug features
|
||||||
|
|
|
@ -1,8 +1,6 @@
|
||||||
|
|
||||||
/*
|
/*
|
||||||
xsns_22_sr04.ino - SR04 ultrasonic sensor support for Sonoff-Tasmota
|
xdrv_25_A4988_Stepper.ino - A4988-StepMotorDriverCircuit- support for Sonoff-Tasmota
|
||||||
|
|
||||||
Copyright (C) 2019 Nuno Ferreira and Theo Arends
|
|
||||||
|
|
||||||
This program is free software: you can redistribute it and/or modify
|
This program is free software: you can redistribute it and/or modify
|
||||||
it under the terms of the GNU General Public License as published by
|
it under the terms of the GNU General Public License as published by
|
||||||
|
@ -22,7 +20,7 @@
|
||||||
#include <A4988_Stepper.h>
|
#include <A4988_Stepper.h>
|
||||||
#define XDRV_25 25
|
#define XDRV_25 25
|
||||||
|
|
||||||
enum A4988Errors { A4988_NO_ERROR, A4988_NO_JSON_COMMAND, A4988_INVALID_JSON};
|
enum A4988Errors { A4988_NO_ERROR, A4988_NO_JSON_COMMAND, A4988_INVALID_JSON, A4988_MOVE, A4988_ROTATE, A4988_TURN};
|
||||||
|
|
||||||
short A4988_dir_pin = pin[GPIO_MAX];
|
short A4988_dir_pin = pin[GPIO_MAX];
|
||||||
short A4988_stp_pin = pin[GPIO_MAX];
|
short A4988_stp_pin = pin[GPIO_MAX];
|
||||||
|
@ -59,53 +57,79 @@ void A4988Init(void)
|
||||||
, A4988_ms3_pin );
|
, A4988_ms3_pin );
|
||||||
}
|
}
|
||||||
|
|
||||||
const char kA4988Commands[] PROGMEM = "MOTOR|"
|
const char kA4988Commands[] PROGMEM = "|"
|
||||||
"doMove|doRotate|doTurn|setSPR|setRPM|setMIS";
|
"MOTOR";
|
||||||
|
|
||||||
void (* const A4988Command[])(void) PROGMEM = { &CmndMOTOR};
|
void (* const A4988Command[])(void) PROGMEM = { &CmndMOTOR};
|
||||||
|
|
||||||
uint32_t MOTORCmndJson(void)
|
uint32_t MOTORCmndJson(void)
|
||||||
{
|
{
|
||||||
// MOTOR {"Command":"doMove","Value":200}
|
// MOTOR {"doMove":200}
|
||||||
// MOTOR {"Command":"doRotate","Value":360}
|
// MOTOR {"doRotate":360}
|
||||||
// MOTOR {"Command":"doTurn","Value":1.0}
|
// MOTOR {"doTurn":1.0}
|
||||||
|
uint32_t returnValue =A4988_NO_JSON_COMMAND;
|
||||||
|
|
||||||
|
char parm_uc[12];
|
||||||
char dataBufUc[XdrvMailbox.data_len];
|
char dataBufUc[XdrvMailbox.data_len];
|
||||||
UpperCase(dataBufUc, XdrvMailbox.data);
|
UpperCase(dataBufUc, XdrvMailbox.data);
|
||||||
RemoveSpace(dataBufUc);
|
RemoveSpace(dataBufUc);
|
||||||
if (strlen(dataBufUc) < 8) { return A4988_INVALID_JSON; }
|
if (strlen(dataBufUc) < 8) { returnValue =A4988_INVALID_JSON; }
|
||||||
|
|
||||||
DynamicJsonBuffer jsonBuf;
|
DynamicJsonBuffer jsonBuf;
|
||||||
JsonObject &json = jsonBuf.parseObject(dataBufUc);
|
JsonObject &json = jsonBuf.parseObject(dataBufUc);
|
||||||
if (!json.success()) { return A4988_INVALID_JSON; }
|
if (json.success()) {
|
||||||
if (json.containsKey(D_JSON_MOTOR_MOVE )){
|
while (json.count()>0) {
|
||||||
long stepsPlease = 50;
|
UpperCase_P(parm_uc, PSTR(D_JSON_MOTOR_SPR));
|
||||||
stepsPlease = strtoul(json[D_JSON_MOTOR_MOVE],nullptr,10);
|
if (json.containsKey(parm_uc)){
|
||||||
myA4988->doMove(stepsPlease);
|
int howManySteps =strtoul(json[parm_uc],nullptr,10);
|
||||||
} else if (json.containsKey(D_JSON_MOTOR_ROTATE )){
|
|
||||||
long degrsPlease = 45;
|
|
||||||
degrsPlease = strtoul(json[D_JSON_MOTOR_ROTATE],nullptr,10);
|
|
||||||
myA4988->doRotate(degrsPlease);
|
|
||||||
} else if (json.containsKey(D_JSON_MOTOR_TURN )){
|
|
||||||
float turnsPlease = 0.25;
|
|
||||||
turnsPlease = strtod(json[D_JSON_MOTOR_TURN],nullptr);
|
|
||||||
myA4988->doTurn(turnsPlease);
|
|
||||||
} else if (json.containsKey(D_JSON_MOTOR_SPR )){
|
|
||||||
int howManySteps =strtoul(json[D_JSON_MOTOR_SPR],nullptr,10);
|
|
||||||
myA4988->setSPR(howManySteps);
|
myA4988->setSPR(howManySteps);
|
||||||
} else if (json.containsKey(D_JSON_MOTOR_RPM )){
|
returnValue = A4988_NO_ERROR;
|
||||||
int howManyRounds =strtoul(json[D_JSON_MOTOR_RPM],nullptr,10);
|
json.remove(parm_uc);
|
||||||
|
}
|
||||||
|
UpperCase_P(parm_uc, PSTR(D_JSON_MOTOR_RPM));
|
||||||
|
if (json.containsKey(parm_uc)){
|
||||||
|
int howManyRounds =strtoul(json[parm_uc],nullptr,10);
|
||||||
myA4988->setRPM(howManyRounds);
|
myA4988->setRPM(howManyRounds);
|
||||||
} else if (json.containsKey(D_JSON_MOTOR_MIS )){
|
returnValue = A4988_NO_ERROR;
|
||||||
short oneToSixteen =strtoul(json[D_JSON_MOTOR_MIS],nullptr,10);
|
json.remove(parm_uc);
|
||||||
|
}
|
||||||
|
UpperCase_P(parm_uc, PSTR(D_JSON_MOTOR_MIS));
|
||||||
|
if (json.containsKey(parm_uc)){
|
||||||
|
short oneToSixteen =strtoul(json[parm_uc],nullptr,10);
|
||||||
myA4988->setMIS(oneToSixteen);
|
myA4988->setMIS(oneToSixteen);
|
||||||
} else return A4988_NO_JSON_COMMAND;
|
returnValue = A4988_NO_ERROR;
|
||||||
return A4988_NO_ERROR;
|
json.remove(parm_uc);
|
||||||
|
}
|
||||||
|
UpperCase_P(parm_uc, PSTR(D_JSON_MOTOR_MOVE));
|
||||||
|
if (json.containsKey(parm_uc)){
|
||||||
|
long stepsPlease = strtoul(json[parm_uc],nullptr,10);
|
||||||
|
myA4988->doMove(stepsPlease);
|
||||||
|
returnValue = A4988_MOVE;
|
||||||
|
json.remove(parm_uc);
|
||||||
|
}
|
||||||
|
UpperCase_P(parm_uc, PSTR(D_JSON_MOTOR_ROTATE));
|
||||||
|
if (json.containsKey(parm_uc)){
|
||||||
|
long degrsPlease = strtoul(json[parm_uc],nullptr,10);
|
||||||
|
myA4988->doRotate(degrsPlease);
|
||||||
|
returnValue = A4988_ROTATE;
|
||||||
|
json.remove(parm_uc);
|
||||||
|
}
|
||||||
|
UpperCase_P(parm_uc, PSTR(D_JSON_MOTOR_TURN));
|
||||||
|
if (json.containsKey(parm_uc)){
|
||||||
|
float turnsPlease = strtod(json[parm_uc],nullptr);
|
||||||
|
myA4988->doTurn(turnsPlease);
|
||||||
|
returnValue = A4988_TURN;
|
||||||
|
json.remove(parm_uc);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
} else returnValue =A4988_INVALID_JSON;
|
||||||
|
return returnValue;
|
||||||
}
|
}
|
||||||
|
|
||||||
void CmndMOTOR(void){
|
void CmndMOTOR(void){
|
||||||
uint32_t error;
|
uint32_t error;
|
||||||
if (XdrvMailbox.data_len) {
|
if (XdrvMailbox.data_len) {
|
||||||
if (strstr(XdrvMailbox.data, "{") == nullptr) {
|
if (strstr(XdrvMailbox.data, "}") == nullptr) {
|
||||||
error = A4988_NO_JSON_COMMAND;
|
error = A4988_NO_JSON_COMMAND;
|
||||||
} else {
|
} else {
|
||||||
error = MOTORCmndJson();
|
error = MOTORCmndJson();
|
||||||
|
@ -117,7 +141,16 @@ void CmndMOTOR(void){
|
||||||
void A4988CmndResponse(uint32_t error){
|
void A4988CmndResponse(uint32_t error){
|
||||||
switch (error) {
|
switch (error) {
|
||||||
case A4988_NO_JSON_COMMAND:
|
case A4988_NO_JSON_COMMAND:
|
||||||
ResponseCmndChar(D_JSON_INVALID_JSON);
|
ResponseCmndChar(PSTR("Kein Commando!"));
|
||||||
|
break;
|
||||||
|
case A4988_MOVE:
|
||||||
|
ResponseCmndChar(PSTR("Stepping!"));
|
||||||
|
break;
|
||||||
|
case A4988_ROTATE:
|
||||||
|
ResponseCmndChar(PSTR("Rotating!"));
|
||||||
|
break;
|
||||||
|
case A4988_TURN:
|
||||||
|
ResponseCmndChar(PSTR("Turning!"));
|
||||||
break;
|
break;
|
||||||
default: // A4988_NO_ERROR
|
default: // A4988_NO_ERROR
|
||||||
ResponseCmndDone();
|
ResponseCmndDone();
|
||||||
|
|
Loading…
Reference in New Issue