Refactor motor control logic and update configuration settings

python/config.json

@@ -10,7 +10,7 @@
         {
             "id": 2,
             "name": "shoulder",
-            "step_range": [0, 2838]
+            "step_range": [-1000, 1000]
         },
         {
             "id": 3,

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

src/.main.cpp

@@ -0,0 +1,78 @@
+#include <SPI.h>
+#include <Wire.h>
+#include <Adafruit_MotorShield.h>
+
+#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]);
+  }
+}

src/main.cpp

@@ -3,90 +3,86 @@
 #include <Adafruit_MotorShield.h>
 
 #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);
+  stepper1.setMaxSpeed(MOTORSPEED);
-  stepper2.setSpeed(motorSpeed);
+  stepper2.setMaxSpeed(MOTORSPEED);
-  stepper3.setSpeed(motorSpeed);
+  stepper3.setMaxSpeed(MOTORSPEED);
-  stepper4.setSpeed(motorSpeed);
+  stepper4.setMaxSpeed(MOTORSPEED);
-  stepper5.setSpeed(motorSpeed);
+  stepper5.setMaxSpeed(MOTORSPEED);
-  stepper6.setSpeed(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();
+  if (stepper1.distanceToGo() != 0) {
-  stepper2.runSpeedToPosition();
+    stepper1.runSpeedToPosition();
-  stepper3.runSpeedToPosition();
+  }
-  stepper4.runSpeedToPosition();
+  if (stepper2.distanceToGo() != 0) {
-  stepper5.runSpeedToPosition();
+    stepper2.runSpeedToPosition();
-  stepper6.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();
+  }
 }