Dateien hochladen nach ''

FullArmControl.ino

@@ -0,0 +1,204 @@
+
+#include <AccelStepper.h>
+
+// Define the pins:
+// Stepper 0
+#define STEP_PIN0 2
+#define DIR_PIN0 30
+#define ENABLE_PIN0 31
+
+// Stepper 1
+#define STEP_PIN1 3
+#define DIR_PIN1 32
+#define ENABLE_PIN1 33
+
+// Stepper 2
+#define STEP_PIN2 4
+#define DIR_PIN2 34
+#define ENABLE_PIN2 35
+
+// Stepper 3
+#define STEP_PIN3 5
+#define DIR_PIN3 36
+#define ENABLE_PIN3 37
+
+
+
+// Define the steps per revolution of thr stepper motor
+const float STEPS_PER_REVOLUTION = 200.0;
+float i = 20;  //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){}
+
+  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
+    move(steps);
+
+    // Enable the motor
+    digitalWrite(ENABLE_PIN3, LOW);
+
+    // Run the stepper until it reaches the target position
+    while (distanceToGo() != 0) {
+      run();
+    }
+
+    // Disable the motor
+    //digitalWrite(ENABLE_PIN3, HIGH);
+  }
+};
+myStepper stepper3(AccelStepper::DRIVER, STEP_PIN3, DIR_PIN3);
+
+const byte numChars = 32;
+char receivedChars[numChars];
+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;
+
+boolean newData = false;
+
+//============
+
+void setup() {
+  Serial.begin(9600);
+  
+  // Set the maximum speed and acceleration (you can adjust these values)
+  stepper3.setMaxSpeed(400);
+  stepper3.setAcceleration(400.0);
+
+  // Set the enable pin as an output and disable the motor by default
+  pinMode(ENABLE_PIN3, OUTPUT);
+  digitalWrite(ENABLE_PIN3, LOW);
+
+  // Set the motor direction (0 for clockwise, 1 for counter-clockwise)
+  stepper3.setPinsInverted(false, false, true);  // Adjust the last parameter if needed
+}
+
+//============
+
+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();
+    newData = false;
+
+    //check received values
+    Serial.println(moveToAngels[3]);
+    
+    stepper3.rotateStepper(moveToAngels[3] * i);
+    
+    
+    //reset recieved values
+    moveToAngels[0] = 0;
+    moveToAngels[1] = 0;
+    moveToAngels[2] = 0;
+    moveToAngels[3] = 0;
+    isCalibrated = 0;
+  }
+}
+
+//============
+
+void recvWithStartEndMarkers() {
+  static boolean recvInProgress = false;
+  static byte ndx = 0;
+  char startMarker = '<';
+  char endMarker = '>';
+  char rc;
+
+  while (Serial.available() > 0 && newData == false) {
+    rc = Serial.read();
+    Serial.print(rc);
+
+    if (recvInProgress == true) {
+      if (rc != endMarker) {
+        receivedChars[ndx] = rc;
+        ndx++;
+        if (ndx >= numChars) {
+          ndx = numChars - 1;
+        }
+      } else {
+        receivedChars[ndx] = '\0';  // terminate the string
+        recvInProgress = false;
+        ndx = 0;
+        newData = true;
+      }
+    }
+
+    else if (rc == startMarker) {
+      recvInProgress = true;
+    }
+  }
+}
+
+//============
+
+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
+
+  strtokIndx = strtok(NULL, ",");  // this continues where the previous call left off
+  moveToAngels[0] = atoi(strtokIndx);
+
+  strtokIndx = strtok(NULL, ",");
+  moveToAngels[1] = atoi(strtokIndx);  // convert this part to a float
+
+  strtokIndx = strtok(NULL, ",");
+  moveToAngels[2] = atof(strtokIndx);  // convert this part to a float
+
+  strtokIndx = strtok(NULL, ",");
+  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.print("Stepper1 ");
+  Serial.println(moveToAngels[0]);
+  Serial.print("Stepper2 ");
+  Serial.println(moveToAngels[1]);
+  Serial.print("Stepper3 ");
+  Serial.println(moveToAngels[2]);
+  Serial.print("Stepper4 ");
+  Serial.println(moveToAngels[3]);
+}