Skip to content

Fix ST mode #3

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
nils-van-zuijlen wants to merge 1 commit into
base: main
Choose a base branch
from
+72 −45
Open

Fix ST mode #3

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
69 changes: 45 additions & 24 deletions ServoDriver/CONNECT.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
}
Expand All @@ -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);
}
}

Expand Down Expand Up @@ -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
}


Expand All @@ -197,6 +217,7 @@ void webCtrlServer(){
case 1:activeCtrl(cmdI);break;
case 9:searchCmd = true;break;
}
server.send(204);
});

// Start server
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -316,4 +337,4 @@ void espNowInit(){
MAC_ADDRESS = WiFi.macAddress();
Serial.print("MAC:");
Serial.println(WiFi.macAddress());
}
}
42 changes: 21 additions & 21 deletions ServoDriver/STSCTRL.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@


// === SC Servo ===
#define CTRL_SC_SERVO
#ifdef CTRL_SC_SERVO
SCSCL st;
float ServoDigitalRange = 1023.0;
float ServoAngleRange = 210.0;
Expand All @@ -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];
Expand Down Expand Up @@ -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);
}
Expand All @@ -140,4 +140,4 @@ void servoStop(byte servoID){

void servoTorque(byte servoID, u8 enableCMD){
st.EnableTorque(servoID, enableCMD);
}
}
6 changes: 6 additions & 0 deletions ServoDriver/ServoDriver.ino
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down