'FullArmControl.ino' ändern

FullArmControl.ino

@@ -6,33 +6,37 @@
 #define STEP_PIN0 2
 #define DIR_PIN0 30
 #define ENABLE_PIN0 31
+#define SENSOR_PIN0 32
 
 // Stepper 1
 #define STEP_PIN1 3
-#define DIR_PIN1 32
-#define ENABLE_PIN1 33
+#define DIR_PIN1 33
+#define ENABLE_PIN1 34
+#define SENSOR_PIN1 35
 
 // Stepper 2
 #define STEP_PIN2 4
-#define DIR_PIN2 34
-#define ENABLE_PIN2 35
+#define DIR_PIN2 36
+#define ENABLE_PIN2 37
+#define SENSOR_PIN2 38
 
 // Stepper 3
 #define STEP_PIN3 5
-#define DIR_PIN3 36
-#define ENABLE_PIN3 37
+#define DIR_PIN3 39
+#define ENABLE_PIN3 40
+#define SENSOR_PIN3 41
 
 
 
 // Define the steps per revolution of thr stepper motor
 const float STEPS_PER_REVOLUTION = 200.0;
-float i = 20;  //Gear Transmission
+float i = 12;  //Gear Transmission
 
-// Create myStepper Class and inherrt AccelStepper
-class myStepper: public AccelStepper{
-  public: 
-  myStepper(uint8_t interface = AccelStepper::FULL4WIRE, uint8_t pin1 = 2, uint8_t pin2 = 3, uint8_t pin3 = 4, uint8_t pin4 = 5, bool enable = true)
-    :AccelStepper(interface, pin1, pin2, pin3, pin4, enable){}
+// Create myStepper Class and inherit AccelStepper
+class myStepper : public AccelStepper {
+public:
+  myStepper(uint8_t interface = AccelStepper::FULL4WIRE, uint8_t pin1 = 2, uint8_t pin2 = 3, uint8_t sensorPin = 6, uint8_t enablePin = 7)
+    : AccelStepper(interface, pin1, pin2, 9, 10, true), sensorPin(sensorPin), enablePin(enablePin) {}
 
   void rotateStepper(float degrees) {
     // Calculate the number of steps based on the desired angle
@@ -41,19 +45,36 @@ class myStepper: public AccelStepper{
     // Move the stepper to the specified number of steps
     move(steps);
 
-    // Enable the motor
-    digitalWrite(ENABLE_PIN3, LOW);
+    // Disable the motor
+    digitalWrite(enablePin, LOW);
 
     // Run the stepper until it reaches the target position
     while (distanceToGo() != 0) {
       run();
     }
+    Serial.println(distanceToGo());
 
-    // Disable the motor
-    //digitalWrite(ENABLE_PIN3, HIGH);
+    // Enable the motor
+    digitalWrite(enablePin, LOW);
   }
+
+  void calibrateStepper() {
+    while (digitalRead(sensorPin) == HIGH) {
+      rotateStepper(1);
+      delay(1);
+    }
+  }
+
+private:
+  int sensorPin;
+  int enablePin;
 };
-myStepper stepper3(AccelStepper::DRIVER, STEP_PIN3, DIR_PIN3);
+
+myStepper stepper0(AccelStepper::DRIVER, STEP_PIN0, DIR_PIN0, SENSOR_PIN0, ENABLE_PIN0);
+myStepper stepper1(AccelStepper::DRIVER, STEP_PIN1, DIR_PIN1, SENSOR_PIN1, ENABLE_PIN1);
+myStepper stepper2(AccelStepper::DRIVER, STEP_PIN2, DIR_PIN2, SENSOR_PIN2, ENABLE_PIN2);
+myStepper stepper3(AccelStepper::DRIVER, STEP_PIN3, DIR_PIN3, SENSOR_PIN3, ENABLE_PIN3);
+
 
 const byte numChars = 32;
 char receivedChars[numChars];
@@ -61,51 +82,91 @@ char tempChars[numChars];  // temporary array for use when parsing
 
 // variables to hold the parsed data
 float moveToAngels[4] = { 0, 0, 0, 0 };
-int isCalibrated = 0;
+int calibrateArm = 0;
 
 boolean newData = false;
 
 //============
 
 void setup() {
+
   Serial.begin(9600);
-  
+
   // Set the maximum speed and acceleration (you can adjust these values)
+  stepper0.setMaxSpeed(400);
+  stepper0.setAcceleration(400.0);
+
+  stepper1.setMaxSpeed(400);
+  stepper1.setAcceleration(400.0);
+
+  stepper2.setMaxSpeed(400);
+  stepper2.setAcceleration(400.0);
+
   stepper3.setMaxSpeed(400);
   stepper3.setAcceleration(400.0);
 
   // Set the enable pin as an output and disable the motor by default
+  pinMode(ENABLE_PIN0, OUTPUT);
+  digitalWrite(ENABLE_PIN0, LOW);
+  pinMode(SENSOR_PIN0, INPUT);
+
+  pinMode(ENABLE_PIN1, OUTPUT);
+  digitalWrite(ENABLE_PIN1, LOW);
+  pinMode(SENSOR_PIN1, INPUT);
+
+
+  pinMode(ENABLE_PIN2, OUTPUT);
+  digitalWrite(ENABLE_PIN2, LOW);
+  pinMode(SENSOR_PIN2, INPUT);
+
+
   pinMode(ENABLE_PIN3, OUTPUT);
   digitalWrite(ENABLE_PIN3, LOW);
+  pinMode(SENSOR_PIN3, INPUT);
+
 
   // Set the motor direction (0 for clockwise, 1 for counter-clockwise)
-  stepper3.setPinsInverted(false, false, true);  // Adjust the last parameter if needed
+  stepper0.setPinsInverted(false, false, true);
+  stepper1.setPinsInverted(false, false, true);
+  stepper2.setPinsInverted(false, false, true);
+  stepper3.setPinsInverted(false, false, true);
 }
 
 //============
 
 void loop() {
+
   recvWithStartEndMarkers();
   if (newData == true) {
     strcpy(tempChars, receivedChars);
     // this temporary copy is necessary to protect the original data
     //   because strtok() used in parseData() replaces the commas with \0
     parseData();
-    //showParsedData();
+    showParsedData();
     newData = false;
 
     //check received values
     Serial.println(moveToAngels[3]);
-    
-    stepper3.rotateStepper(moveToAngels[3] * i);
-    
-    
+    if (calibrateArm == 1) {
+
+      stepper0.calibrateStepper();
+      stepper1.calibrateStepper();
+      stepper2.calibrateStepper();
+      stepper3.calibrateStepper();
+
+    } else {
+      stepper0.rotateStepper(moveToAngels[0] * i);
+      stepper1.rotateStepper(moveToAngels[1] * i);
+      stepper2.rotateStepper(moveToAngels[2] * i);
+      stepper3.rotateStepper(moveToAngels[3] * i);
+    }
+
     //reset recieved values
     moveToAngels[0] = 0;
     moveToAngels[1] = 0;
     moveToAngels[2] = 0;
     moveToAngels[3] = 0;
-    isCalibrated = 0;
+    calibrateArm = 0;
   }
 }
 
@@ -150,7 +211,7 @@ void parseData() {  // split the data into its parts
   char* strtokIndx;  // this is used by strtok() as an index
 
   strtokIndx = strtok(tempChars, ",");  // get the first part
-  isCalibrated = atoi(strtokIndx);      // convert this part to an integer
+  calibrateArm = atoi(strtokIndx);      // convert this part to an integer
 
   strtokIndx = strtok(NULL, ",");  // this continues where the previous call left off
   moveToAngels[0] = atoi(strtokIndx);
@@ -165,34 +226,13 @@ void parseData() {  // split the data into its parts
   moveToAngels[3] = atof(strtokIndx);  // convert this part to a float
 }
 
-//============
-
-/* void rotateStepper(float degrees) {
-  // Calculate the number of steps based on the desired angle
-  float steps = degrees * (STEPS_PER_REVOLUTION / 360.0);
-
-  // Move the stepper to the specified number of steps
-  stepper.move(steps);
-
-  // Enable the motor
-  digitalWrite(ENABLE_PIN1, LOW);
-
-  // Run the stepper until it reaches the target position
-  while (stepper.distanceToGo() != 0) {
-    stepper.run();
-  }
-
-  // Disable the motor
-  //digitalWrite(ENABLE_PIN1, HIGH);
-} 
-**/
 
 //============
 
 // For debugging purposes
 void showParsedData() {
   Serial.print("Calibration:  ");
-  Serial.println(isCalibrated);
+  Serial.println(calibrateArm);
   Serial.print("Stepper1 ");
   Serial.println(moveToAngels[0]);
   Serial.print("Stepper2 ");