From b1bac8fe66eb69652715c469c7f1d946a790ab5c Mon Sep 17 00:00:00 2001 From: Nils VAN ZUIJLEN Date: Thu, 25 Jul 2024 22:43:06 +0200 Subject: [PATCH] Fix ST mode --- ServoDriver/CONNECT.h | 69 ++++++++++++++++++++++++------------- ServoDriver/STSCTRL.h | 42 +++++++++++----------- ServoDriver/ServoDriver.ino | 6 ++++ 3 files changed, 72 insertions(+), 45 deletions(-) diff --git a/ServoDriver/CONNECT.h b/ServoDriver/CONNECT.h index e5b0c06..b0cf946 100644 --- a/ServoDriver/CONNECT.h +++ b/ServoDriver/CONNECT.h @@ -47,73 +47,93 @@ int rangeCtrl(int rawInput, int minInput, int maxInput){ void activeCtrl(int cmdInput){ switch(cmdInput){ + // Middle case 1:st.WritePosEx(listID[activeNumInList], ServoDigitalMiddle, activeServoSpeed, ServoInitACC);break; + // Stop case 2: if(modeRead[listID[activeNumInList]] == 0) { servoStop(listID[activeNumInList]); } else if(modeRead[listID[activeNumInList]] == 3){ - st.WritePos(listID[activeNumInList], 0, 0, 0); + st.WritePosEx(listID[activeNumInList], 0, 0, 0); } break; + // Release case 3:servoTorque(listID[activeNumInList],0);Torque_List[activeNumInList] = false;break; + // Torque case 4:servoTorque(listID[activeNumInList],1);Torque_List[activeNumInList] = true;break; + // Position + case 5: if(modeRead[listID[activeNumInList]] == 0){ - if(SERVO_TYPE_SELECT == 1){ +#ifdef CTRL_ST_SERVO st.WritePosEx(listID[activeNumInList], ServoDigitalRange - 1, activeServoSpeed, ServoInitACC); - } - else if(SERVO_TYPE_SELECT == 2){ +#endif +#ifdef CTRL_SC_SERVO st.WritePosEx(listID[activeNumInList], ServoDigitalRange - MAX_MIN_OFFSET, activeServoSpeed, ServoInitACC); - } +#endif } else if(modeRead[listID[activeNumInList]] == 3){ - if(SERVO_TYPE_SELECT == 1){ - st.WritePosEx(listID[activeNumInList], 10000, activeServoSpeed, ServoInitACC); - } - else if(SERVO_TYPE_SELECT == 2){ +#ifdef CTRL_ST_SERVO + st.WritePosEx(listID[activeNumInList], 10000, activeServoSpeed, ServoInitACC); +#endif +#ifdef CTRL_SC_SERVO st.WritePos(listID[activeNumInList], 0, rangeCtrl(activeServoSpeed,200,999), 0); - } +#endif } break; + // Position - case 6: if(modeRead[listID[activeNumInList]] == 0){ - if(SERVO_TYPE_SELECT == 1){ +#ifdef CTRL_ST_SERVO st.WritePosEx(listID[activeNumInList], 0, activeServoSpeed, ServoInitACC); - } - else if(SERVO_TYPE_SELECT == 2){ +#endif +#ifdef CTRL_SC_SERVO st.WritePosEx(listID[activeNumInList], MAX_MIN_OFFSET, activeServoSpeed, ServoInitACC); - } +#endif } - else if(modeRead[listID[activeNumInList]] == 3){ - if(SERVO_TYPE_SELECT == 1){ +#ifdef CTRL_ST_SERVO st.WritePosEx(listID[activeNumInList], -10000, activeServoSpeed, ServoInitACC); - } - else if(SERVO_TYPE_SELECT == 2){ +#endif +#ifdef CTRL_SC_SERVO st.WritePos(listID[activeNumInList], 0, rangeCtrl(activeServoSpeed,200,999)+1024, 0); - } +#endif } break; + // Speed + case 7:activeSpeed(100);break; + // Speed - case 8:activeSpeed(-100);break; + // ID to set + case 9:servotoSet += 1;if(servotoSet > 250){servotoSet = 0;}break; + // ID to set - case 10:servotoSet -= 1;if(servotoSet < 0){servotoSet = 0;}break; + // Set Middle position case 11:setMiddle(listID[activeNumInList]);break; + // Set Servo Mode case 12:setMode(listID[activeNumInList], 0);break; + // Set Motor mode case 13:setMode(listID[activeNumInList], 3);break; + // Start Serial Forwarding case 14:SERIAL_FORWARDING = true;break; + // End Serial Forwarding case 15:SERIAL_FORWARDING = false;break; + // Set New ID case 16:setID(listID[activeNumInList], servotoSet);break; + // Set role as Normal case 17:DEV_ROLE = 0;break; + // Set Role as Leader case 18:DEV_ROLE = 1;break; + // Set Role as Follower case 19:DEV_ROLE = 2;break; + // Rainbow On case 20:RAINBOW_STATUS = 1;break; + // Rainbow OFF case 21:RAINBOW_STATUS = 0;break; } } @@ -130,11 +150,11 @@ void handleID() { for(int i = 0; i< searchNum; i++){ IDmessage += String(listID[i]) + " "; } - server.send(200, "text/plane", IDmessage); + server.send(200, "text/plain", IDmessage); } else if(searchedStatus){ String IDmessage = "Searching..."; - server.send(200, "text/plane", IDmessage); + server.send(200, "text/plain", IDmessage); } } @@ -177,7 +197,7 @@ void handleSTS() { else{ stsValue += " FeedBack err"; } - server.send(200, "text/plane", stsValue); //Send ADC value only to client ajax request + server.send(200, "text/plain", stsValue); //Send ADC value only to client ajax request } @@ -197,6 +217,7 @@ void webCtrlServer(){ case 1:activeCtrl(cmdI);break; case 9:searchCmd = true;break; } + server.send(204); }); // Start server @@ -267,7 +288,7 @@ void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) { } -void OnDataRecv(const uint8_t * mac, const uint8_t *incomingData, int len) { +void OnDataRecv(const esp_now_recv_info * mac, const uint8_t *incomingData, int len) { if(DEV_ROLE == 2){ memcpy(&myData, incomingData, sizeof(myData)); myData.Spd_send = abs(myData.Spd_send); @@ -316,4 +337,4 @@ void espNowInit(){ MAC_ADDRESS = WiFi.macAddress(); Serial.print("MAC:"); Serial.println(WiFi.macAddress()); -} \ No newline at end of file +} diff --git a/ServoDriver/STSCTRL.h b/ServoDriver/STSCTRL.h index d68296b..1b1d8af 100644 --- a/ServoDriver/STSCTRL.h +++ b/ServoDriver/STSCTRL.h @@ -3,7 +3,7 @@ // === SC Servo === -#define CTRL_SC_SERVO +#ifdef CTRL_SC_SERVO SCSCL st; float ServoDigitalRange = 1023.0; float ServoAngleRange = 210.0; @@ -12,21 +12,21 @@ float ServoDigitalMiddle= 511.0; #define ServoMaxSpeed 1500 #define MaxSpeed_X 1500 #define ServoInitSpeed 1500 -int SERVO_TYPE_SELECT = 2; int MAX_MIN_OFFSET = 30; #define SMS_STS_ID SCSCL_ID +#endif // ifdef CTRL_SC_SERVO // === ST Servo === -// #define CTRL_ST_SERVO -// SMS_STS st; -// float ServoDigitalRange = 4095.0; -// float ServoAngleRange = 360.0; -// float ServoDigitalMiddle= 2047.0; -// #define ServoInitACC 100 -// #define ServoMaxSpeed 4000 -// #define MaxSpeed_X 4000 -// #define ServoInitSpeed 2000 -// int SERVO_TYPE_SELECT = 1 +#ifdef CTRL_ST_SERVO +SMS_STS st; +float ServoDigitalRange = 4095.0; +float ServoAngleRange = 360.0; +float ServoDigitalMiddle= 2047.0; +#define ServoInitACC 100 +#define ServoMaxSpeed 4000 +#define MaxSpeed_X 4000 +#define ServoInitSpeed 2000 +#endif // ifdef CTRL_ST_SERVO // set the servo ID list. byte ID_List[253]; @@ -95,29 +95,29 @@ void setMiddle(byte InputID){ void setMode(byte InputID, byte InputMode){ st.unLockEprom(InputID); if(InputMode == 0){ - if(SERVO_TYPE_SELECT == 1){ +#ifdef CTRL_ST_SERVO st.writeWord(InputID, 11, 4095); st.writeByte(InputID, SMS_STS_MODE, InputMode); - } - else if(SERVO_TYPE_SELECT == 2){ +#endif +#ifdef CTRL_SC_SERVO st.writeWord(InputID, SCSCL_MIN_ANGLE_LIMIT_L, 20); // st.writeWord(InputID, SCSCL_MIN_ANGLE_LIMIT_H, 5123); st.writeWord(InputID, SCSCL_MAX_ANGLE_LIMIT_L, 1003); // st.writeWord(InputID, SCSCL_MAX_ANGLE_LIMIT_H, 60240); - } +#endif } else if(InputMode == 3){ - if(SERVO_TYPE_SELECT == 1){ +#ifdef CTRL_ST_SERVO st.writeByte(InputID, SMS_STS_MODE, InputMode); st.writeWord(InputID, 11, 0); - } - else if(SERVO_TYPE_SELECT == 2){ +#endif +#ifdef CTRL_SC_SERVO st.writeWord(InputID, SCSCL_MIN_ANGLE_LIMIT_L, 0); // st.writeWord(InputID, SCSCL_MIN_ANGLE_LIMIT_H, 0); st.writeWord(InputID, SCSCL_MAX_ANGLE_LIMIT_L, 0); // st.writeWord(InputID, SCSCL_MAX_ANGLE_LIMIT_H, 0); - } +#endif } st.LockEprom(InputID); } @@ -140,4 +140,4 @@ void servoStop(byte servoID){ void servoTorque(byte servoID, u8 enableCMD){ st.EnableTorque(servoID, enableCMD); -} \ No newline at end of file +} diff --git a/ServoDriver/ServoDriver.ino b/ServoDriver/ServoDriver.ino index 45ea755..898b00d 100644 --- a/ServoDriver/ServoDriver.ino +++ b/ServoDriver/ServoDriver.ino @@ -20,6 +20,12 @@ typedef struct struct_message { // Create a struct_message called myData struct_message myData; +// Choose your servo type here +// Uncomment the line corresponding to your servo type +// You can only have one active at a time. +//#define CTRL_ST_SERVO // For ST servos +#define CTRL_SC_SERVO // For SC servos + // set the default role here. // 0 as normal mode. // 1 as leader, ctrl other device via ESP-NOW.