The Classy Colonoscopy Magnetic Capsule Robotic Arms Navigation System

//MECH2636 Robotic Arm Motors Control Code
//Team 4
//Author: Ali Waseem

//Set up pins
int motorStepPin_1 = 2;     // pin for motor 1 step
int motorDirPin_1 = 3;      // pin for motor 1 direction
int motorEnablePin_1 = 4;   // pin for motor 1 enable
int motorStepPin_2 = 5;     // pin for motor 2 step
int motorDirPin_2 = 6;      // pin for motor 2 direction
int motorEnablePin_2 = 7;   // pin for motor 2 enable
int MSPin_1 = 9;            // pin for MS1
int MSPin_2 = 10;           // pin for MS2
int MSPin_3 = 11;           // pin for MS3
int blackButtonPin = A9;    // pin for black button
int greenButtonPin = A10;   // pin for green button
int potPin = A8;            // pin for rotary pot

//Set up variables
int motorStep_1 = 0;                    // Motor 1 step, 0 to 1
int motorStep_2 = 0;                    // Motor 2 step, 0 to 1
int motorDir_1 = 0;                     // Motor 1 direction, 0 to 1
int motorDir_2 = 0;                     // Motor 2 direction, 0 to 1
int currentStep_1 = 0;                  // Current number of motor 1 steps undertaken
int currentStep_2 = 0;                  // Current number of motor 2 steps undertaken
int desiredStep_1 = 0;                  // Desired number of motor 1 steps from trajectory
int desiredStep_2 = 0;                  // Desired number of motor 2 steps from trajectory
int currentPos = 0;                     // Current position in array
int lastPos = 0;                        // number of rows in array
int potValue;                           // Potentiometer value
float maxPot = 270;                     // Used to calculate potentiometer value
int stepdt = 10;                        // Loop delay time
float potBias;                          // Potentiometer bias -1 to +1
unsigned long timePassed = 0;           // Time in msec since button press
unsigned long timeStarted;              // Time in msec when button pressed
int buttonPressed = 0;                  // Which button pressed: 0 = neither, 1 = black, 2 = green

void setup() {
  // Setup Motor 1
  pinMode(motorDirPin_1, OUTPUT);       // Initiates Motor 1 Direction Channel pin
  pinMode(motorEnablePin_1, OUTPUT);    // Initiates Motor 1 Enable Channel pin
  pinMode(motorStepPin_1, OUTPUT);      // Initiates Motor 1 Step Channel pin
  // Setup Motor 2
  pinMode(motorDirPin_2, OUTPUT);       // Initiates Motor 2 Direction Channel pin
  pinMode(motorEnablePin_2, OUTPUT);    // Initiates Motor 2 Enable Channel pin
  pinMode(motorStepPin_2, OUTPUT);      // Initiates Motor 2 Step Channel pin
  // Setup microstepping
  pinMode(MSPin_1, OUTPUT);             // Initiates Microstepping 1 Channel pin
  pinMode(MSPin_2, OUTPUT);             // Initiates Microstepping 2 Channel pin
  pinMode(MSPin_3, OUTPUT);             // Initiates Microstepping 3 Channel pin
  // Setup Button box
  pinMode(blackButtonPin, INPUT);       // Initiates Black Button
  pinMode(greenButtonPin, INPUT);       // Initiates Green Button
  pinMode(potPin, INPUT);               // Initiates Potentiometer

  Serial.begin(9600);                   // Setup serial
}

// Main loop
void loop() {
  digitalWrite(motorEnablePin_1, HIGH);   // Engage the Brake for Motor 1
  digitalWrite(motorEnablePin_2, HIGH);   // Engage the Brake for Motor 2
  buttonPressed = 0;

  // Waits here until a button is pressed
  while (buttonPressed == 0) {
    if (analogRead(blackButtonPin) < 2) {
      buttonPressed = 1;
    }
    else if (analogRead(greenButtonPin) < 2) {
      buttonPressed = 2;
    }
    else {
      buttonPressed = 0;
    }
    delay(50);
  }

  // Initialise variables ready for the run loop
  potValue = analogRead(potPin);
  potBias = (potValue - 260) / maxPot;
  timeStarted = millis();
  timePassed = millis() - timeStarted;
  digitalWrite(motorEnablePin_1, LOW);    // Enable Motor 1
  digitalWrite(motorEnablePin_2, LOW);    // Enable Motor 2

  // ******** YOU SET THESE VALUES BELOW ******
  digitalWrite(MSPin_1, LOW);             // Set microstepping pins (LOW/HIGH) per step size
  digitalWrite(MSPin_2, HIGH);
  digitalWrite(MSPin_3, LOW);
  stepdt = 50;                            // Time in ms between each motor step (min 10)

  int trajectory[1611][2] = {               // Paste formatted trajectory here (integer motor step values)    
    
    // THERE WOULD BE 1611 ROWS OF TRAJECTORY DATA HERE, IN THE FORMAT:
    // {theta1_1, theta2_1},
    
    // THEY WERE LOADED FROM THE EXCEL FILE GENERATED BY THE MATLAB CODE, 
    // AND WOULD REPRESENT THE DESIRED ANGLE FOR EACH MOTOR AT EACH POINT IN THE TRAJECTORY.

    // TO AVOID CLUTTERING THE CODE BLOCK, THE TRAJECTORY DATA HAS BEEN OMITTED FROM THIS DISPLAY.

    // PLEASE FIND THEM in the FOLLOWING EXCEL FILE

  };

  lastPos = 8;        // Set to number of rows in trajectory
  currentPos = 0;
  currentStep_1 = 0;  // Reset step counters at start of run
  currentStep_2 = 0;

  // Run loop - continues until all trajectory points are reached
  while (currentPos < lastPos) {

    timePassed = millis() - timeStarted;

    desiredStep_1 = trajectory[currentPos][0];
    desiredStep_2 = trajectory[currentPos][1];

    // Step Motor 1 toward desired position
    if (currentStep_1 < desiredStep_1) {
      digitalWrite(motorDirPin_1, HIGH);
      digitalWrite(motorStepPin_1, HIGH);
      delay(5);
      digitalWrite(motorStepPin_1, LOW);
      currentStep_1++;
    }
    else if (currentStep_1 > desiredStep_1) {
      digitalWrite(motorDirPin_1, LOW);
      digitalWrite(motorStepPin_1, HIGH);
      delay(5);
      digitalWrite(motorStepPin_1, LOW);
      currentStep_1--;
    }

    // Step Motor 2 toward desired position
    if (currentStep_2 < desiredStep_2) {
      digitalWrite(motorDirPin_2, HIGH);
      digitalWrite(motorStepPin_2, HIGH);
      delay(5);
      digitalWrite(motorStepPin_2, LOW);
      currentStep_2++;
    }
    else if (currentStep_2 > desiredStep_2) {
      digitalWrite(motorDirPin_2, LOW);
      digitalWrite(motorStepPin_2, HIGH);
      delay(5);
      digitalWrite(motorStepPin_2, LOW);
      currentStep_2--;
    }

    // Advance to next trajectory point once both motors reach their target
    if (currentStep_1 == desiredStep_1 && currentStep_2 == desiredStep_2) {
      currentPos++;
    }

    delay(stepdt);

    // Uncomment for debugging:
//    Serial.print(" M1 Desired: "); Serial.print(desiredStep_1);
//    Serial.print(" M1 Current: "); Serial.print(currentStep_1);
//    Serial.print(" M2 Desired: "); Serial.print(desiredStep_2);
//    Serial.print(" M2 Current: "); Serial.print(currentStep_2);
//    Serial.print(" Array Pos: ");  Serial.print(currentPos);
//    Serial.println();
  }

  delay(1000);
}