From edea4c076f4ef0e81778c3e2c217a73046e77af5 Mon Sep 17 00:00:00 2001 From: "guillaume.bonabau" Date: Wed, 18 Dec 2024 12:48:57 +0100 Subject: [PATCH] Refactor motor control logic and update configuration settings --- python/config.json | 2 +- python/sliders.py | 4 +- src/.main.cpp | 78 +++++++++++++++++++++++++++++++++++++ src/main.cpp | 97 ++++++++++++++++++++++++++++------------------ 4 files changed, 141 insertions(+), 40 deletions(-) create mode 100644 src/.main.cpp diff --git a/python/config.json b/python/config.json index d79f6c7..80e0cae 100644 --- a/python/config.json +++ b/python/config.json @@ -10,7 +10,7 @@ { "id": 2, "name": "shoulder", - "step_range": [0, 2838] + "step_range": [-1000, 1000] }, { "id": 3, diff --git a/python/sliders.py b/python/sliders.py index 9080ae0..e634f3b 100644 --- a/python/sliders.py +++ b/python/sliders.py @@ -6,8 +6,10 @@ import threading test_mode = False + + # Read the joint configuration from the config.json file -with open('config.json', 'r') as f: +with open('python/config.json', 'r') as f: config = json.load(f) # Configure the serial port diff --git a/src/.main.cpp b/src/.main.cpp new file mode 100644 index 0000000..4994c56 --- /dev/null +++ b/src/.main.cpp @@ -0,0 +1,78 @@ +#include +#include +#include + +#define STEPSBYREVOLUTION 48 +#define MOTORTYPE DOUBLE + +// Create the motor shield object with 0x60 address for the top shield +Adafruit_MotorShield AFMS_1 = Adafruit_MotorShield(0x60); +// Create the motor shield object with 0x61 address for the bottom shield +Adafruit_MotorShield AFMS_2 = Adafruit_MotorShield(0x61); +// Create the motor shield object with 0x63 address for the bottom shield +Adafruit_MotorShield AFMS_3 = Adafruit_MotorShield(0x63); + +// to shield 1, motor port #1 (M1 and M2) +Adafruit_StepperMotor *motor1 = AFMS_1.getStepper(48, 1); +// to shield 1, motor port #2 (M3 and M4) +Adafruit_StepperMotor *motor2 = AFMS_1.getStepper(48, 2); +// to shield 2, motor port #1 (M1 and M2) +Adafruit_StepperMotor *motor3 = AFMS_2.getStepper(48, 1); +// to shield 2, motor port #2 (M3 and M4) +Adafruit_StepperMotor *motor4 = AFMS_2.getStepper(48, 2); +// to shield 3, motor port #1 (M1 and M2) +Adafruit_StepperMotor *motor5 = AFMS_3.getStepper(48, 1); +// to shield 3, motor port #2 (M3 and M4) +Adafruit_StepperMotor *motor6 = AFMS_3.getStepper(48, 2); + +Adafruit_StepperMotor *motorArray[6] = {motor1, motor2, motor3, motor4, motor5, motor6}; + +void setup() { + Serial.begin(9600); + + AFMS_1.begin(); // Start the top shield + AFMS_2.begin(); // Start the bottom shield + AFMS_3.begin(); // Start the bottom shield + + // Set the speed of the motors + for (int i = 0; i < 6; i++) { + motorArray[i]->setSpeed(10); + } +} + +void moveMotor(Adafruit_StepperMotor *motor, int steps) { + if (steps > 0) { + motor->step(steps, FORWARD, MOTORTYPE); + } else if (steps < 0) { + motor->step(-steps, BACKWARD, MOTORTYPE); + } +} + +void loop() { + if (Serial.available() > 0) { + String data = Serial.readStringUntil('\n'); + int jointValues[6]; + int index = 0; + int start = 0; + int end = data.indexOf(','); + + while (end != -1 && index < 6) { + jointValues[index] = data.substring(start, end).toInt(); + start = end + 1; + end = data.indexOf(',', start); + index++; + } + if (index < 6) { + jointValues[index] = data.substring(start).toInt(); + Serial.println("Moving to position: D1: " + String(jointValues[0]) + " D2: " + String(jointValues[1]) + " D3: " + String(jointValues[2]) + " D4: " + String(jointValues[3]) + " D5: " + String(jointValues[4]) + " D6: " + String(jointValues[5])); + } + + // Move the motors to the target positions + moveMotor(motor1, jointValues[0]); + moveMotor(motor2, jointValues[1]); + moveMotor(motor3, jointValues[2]); + moveMotor(motor4, jointValues[3]); + moveMotor(motor5, jointValues[4]); + moveMotor(motor6, jointValues[5]); + } +} \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index fb22c51..90c8def 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -3,90 +3,86 @@ #include #define STEPSBYREVOLUTION 48 +#define MOTORTYPE SINGLE // Change to MICROSTEP for microstepping +#define MOTORSPEED 500 // Create the motor shield object with 0x60 address for the top shield Adafruit_MotorShield AFMS_1 = Adafruit_MotorShield(0x60); // Create the motor shield object with 0x61 address for the bottom shield Adafruit_MotorShield AFMS_2 = Adafruit_MotorShield(0x61); -// Create the motor shield object with 0x61 address for the bottom shield +// Create the motor shield object with 0x63 address for the bottom shield Adafruit_MotorShield AFMS_3 = Adafruit_MotorShield(0x63); - // to shield 1, motor port #1 (M1 and M2) Adafruit_StepperMotor *myMotor1 = AFMS_1.getStepper(48, 1); // you can change these to DOUBLE or INTERLEAVE or MICROSTEP! // wrappers for the 1st motor! void forwardstep1() { - myMotor1->onestep(FORWARD, SINGLE); + myMotor1->onestep(FORWARD, MOTORTYPE); } void backwardstep1() { - myMotor1->onestep(BACKWARD, SINGLE); + myMotor1->onestep(BACKWARD, MOTORTYPE); } - // to shield 1, motor port #2 (M3 and M4) Adafruit_StepperMotor *myMotor2 = AFMS_1.getStepper(48, 2); // you can change these to DOUBLE or INTERLEAVE or MICROSTEP! // wrappers for the 2nd motor! void forwardstep2() { - myMotor2->onestep(FORWARD, SINGLE); + myMotor2->onestep(FORWARD, MOTORTYPE); } void backwardstep2() { - myMotor2->onestep(BACKWARD, SINGLE); + myMotor2->onestep(BACKWARD, MOTORTYPE); } - // to shield 2, motor port #1 (M1 and M2) Adafruit_StepperMotor *myMotor3 = AFMS_2.getStepper(48, 1); // you can change these to DOUBLE or INTERLEAVE or MICROSTEP! // wrappers for the 3rd motor! void forwardstep3() { - myMotor3->onestep(FORWARD, SINGLE); + myMotor3->onestep(FORWARD, MOTORTYPE); } void backwardstep3() { - myMotor3->onestep(BACKWARD, SINGLE); + myMotor3->onestep(BACKWARD, MOTORTYPE); } - // to shield 2, motor port #2 (M3 and M4) Adafruit_StepperMotor *myMotor4 = AFMS_2.getStepper(48, 2); // you can change these to DOUBLE or INTERLEAVE or MICROSTEP! // wrappers for the 4th motor! void forwardstep4() { - myMotor4->onestep(FORWARD, SINGLE); + myMotor4->onestep(FORWARD, MOTORTYPE); } void backwardstep4() { - myMotor4->onestep(BACKWARD, SINGLE); + myMotor4->onestep(BACKWARD, MOTORTYPE); } - // to shield 3, motor port #1 (M1 and M2) Adafruit_StepperMotor *myMotor5 = AFMS_3.getStepper(48, 1); // you can change these to DOUBLE or INTERLEAVE or MICROSTEP! // wrappers for the 5th motor! void forwardstep5() { - myMotor5->onestep(FORWARD, SINGLE); + myMotor5->onestep(FORWARD, MOTORTYPE); } void backwardstep5() { - myMotor5->onestep(BACKWARD, SINGLE); + myMotor5->onestep(BACKWARD, MOTORTYPE); } - // to shield 3, motor port #2 (M3 and M4) Adafruit_StepperMotor *myMotor6 = AFMS_3.getStepper(48, 2); // you can change these to DOUBLE or INTERLEAVE or MICROSTEP! // wrappers for the 6th motor! void forwardstep6() { - myMotor6->onestep(FORWARD, SINGLE); + myMotor6->onestep(FORWARD, MOTORTYPE); } void backwardstep6() { - myMotor6->onestep(BACKWARD, SINGLE); + myMotor6->onestep(BACKWARD, MOTORTYPE); } // Now we'll wrap the 6 steppers in an AccelStepper object @@ -100,7 +96,6 @@ AccelStepper stepper6(forwardstep6, backwardstep6); unsigned long lastUpdate = 0; // Last time the position was updated const int updateInterval = 0; // Update interval in milliseconds int newTarget = 0; -int motorSpeed = 10; int potValue1 = 0; int potValue2 = 0; @@ -116,24 +111,37 @@ int targetPosition5 = 0; int targetPosition6 = 0; void setup() { - // put your setup code here, to run once: Serial.begin(9600); - AFMS_1.begin(); // Start the bottom shield + AFMS_1.begin(); // Start the top shield AFMS_2.begin(); // Start the bottom shield - AFMS_3.begin(); // Start the top shield + AFMS_3.begin(); // Start the bottom shield - stepper1.setSpeed(motorSpeed); - stepper2.setSpeed(motorSpeed); - stepper3.setSpeed(motorSpeed); - stepper4.setSpeed(motorSpeed); - stepper5.setSpeed(motorSpeed); - stepper6.setSpeed(motorSpeed); + stepper1.setMaxSpeed(MOTORSPEED); + stepper2.setMaxSpeed(MOTORSPEED); + stepper3.setMaxSpeed(MOTORSPEED); + stepper4.setMaxSpeed(MOTORSPEED); + stepper5.setMaxSpeed(MOTORSPEED); + stepper6.setMaxSpeed(MOTORSPEED); + stepper1.setAcceleration(MOTORSPEED); + stepper2.setAcceleration(MOTORSPEED); + stepper3.setAcceleration(MOTORSPEED); + stepper4.setAcceleration(MOTORSPEED); + stepper5.setAcceleration(MOTORSPEED); + stepper6.setAcceleration(MOTORSPEED); + + //Homing sequence moving +100 0 for all motors + // AccelStepper *motorArray[6] = {&stepper1, &stepper2, &stepper3, &stepper4, &stepper5, &stepper6}; + // for (int i = 0; i < 6; i++) { + // motorArray[i]->moveTo(100); + // motorArray[i]->runToPosition(); + // motorArray[i]->moveTo(0); + // motorArray[i]->runToPosition(); + // } } void loop() { - // put your main code here, to run repeatedly: if (Serial.available() > 0) { String data = Serial.readStringUntil('\n'); int jointValues[6]; @@ -157,13 +165,26 @@ void loop() { stepper4.moveTo(jointValues[3]); stepper5.moveTo(jointValues[4]); stepper6.moveTo(jointValues[5]); + + Serial.println("Moving to position: D1: " + String(jointValues[0]) + " D2: " + String(jointValues[1]) + " D3: " + String(jointValues[2]) + " D4: " + String(jointValues[3]) + " D5: " + String(jointValues[4]) + " D6: " + String(jointValues[5])); } - stepper1.runSpeedToPosition(); - stepper2.runSpeedToPosition(); - stepper3.runSpeedToPosition(); - stepper4.runSpeedToPosition(); - stepper5.runSpeedToPosition(); - stepper6.runSpeedToPosition(); - -} + if (stepper1.distanceToGo() != 0) { + stepper1.runSpeedToPosition(); + } + if (stepper2.distanceToGo() != 0) { + stepper2.runSpeedToPosition(); + } + if (stepper3.distanceToGo() != 0) { + stepper3.runSpeedToPosition(); + } + if (stepper4.distanceToGo() != 0) { + stepper4.runSpeedToPosition(); + } + if (stepper5.distanceToGo() != 0) { + stepper5.runSpeedToPosition(); + } + if (stepper6.distanceToGo() != 0) { + stepper6.runSpeedToPosition(); + } +} \ No newline at end of file