The Classy Colonoscopy Magnetic Capsule Robotic Arms Navigation System

Supplementary Digital Appendix for our System.

Members & Roles

CAD & 3D Design

Harry Marsden
Andy Wills

Mathematics & Ethics

Ebony Cope
Imogen Dolan
Jiaoxiao WU

Arduino & Poster

Ali Waseem

MATLAB, Website & Final Deployment

Yecheng WANG

1. 3D Design Walk Around Video

3D Design Walk Around Video

The Arms 1 length is 250 and Arms 2 length is 200.

2. Trajectory Simulation

MATLAB Kinematics Record

Real-time execution of the Equidistant Linear Resampling algorithm enforcing Elbow-down configuration. Based on our design, the offset of Motor in Colon Map is (-60,-60) and Arms' Length is (250,200). Current origin of the coordinate system is at the Motor.

Farthest Reachability Check

Verification of Inverse Kinematics at the extremity of the physical envelope (Max extension verification). The programme pinpointed the Unreachable Points in the workspace with red markers and skipped them for Physical Continuity.

3. Interpolation Methodology

Due to space constraints on the physical poster, the specific logic behind our Equidistant Linear Resampling and Interpolation for Constant Velocity Trajectory Generation is detailed below.

Spatial Discretization Strategy

Unlike previous segmented interpolation, our current algorithm computes the Euclidean distance dist = √[Δx² + Δy²] between each pair of the 87 original waypoints. It dynamically assigns a resampling density N = ⌈dist/Δs⌉ to maintain a uniform spatial resolution across the entire trajectory.

Constant Velocity Profile (Jerk Mitigation)

By maintaining a fixed spatial displacement between discrete steps, the system achieves a constant velocity profile when executed by the Arduino's fixed-frequency timer. This eliminates velocity spikes and minimizes mechanical jerk, which is critical for magnetic navigation.

4. Bill of Materials (BOM)

Part No.	Component Name	Category	Quantity
n/a	M3 Pan head 10mm	BS	10
n/a	M3 Pan head 16mm	BS	8
n/a	M3 Pan head 20mm	BS	5
n/a	M3 Pan head 25mm	BS	10
n/a	M3 Pan head 30mm	BS	4
n/a	M3 hex nut	BS	38
n/a	M3 washer	BS	2
9	ASBS 220mm arm piece 2	S	1
12	AR6 arm connection piece 60mm	S	1
10	AR6 arm connection piece 150mm	S	1
11	ASBS 400mm arm piece 1	S	2
15	motor 2 hold plate	S	1
1	Magic Pen Gripper	3D	1
2	Rod holder	3D	6
5	Motor housing lid	3D	1
6	Motor housing	3D	1
7	Motor 2 housing	3D	1
8	Motor 2 housing lid	3D	1
n/a	919D23 (belt)	G	1
n/a	919D12 (pulley)	G	2
n/a	GFM-0608-04 Push in bearing	G	4
n/a	clamp collar	G	1
n/a	Rigid Coupling	G	1

Category Legend: BS: Standard Fasteners (Bolts/Screws) S: Structural / Stock Parts 3D: Custom 3D Printed Components G: Gears, Bearings & Couplings

5. Codes Appendix

Four code artefacts are archived below. The MATLAB script generates the constant step-size trajectory. The initial Arduino control framework (Ali Waseem) is retained for the record, but was not used in the final competition. The assessed run was executed with run_SUCCESSFUL.ino — developed, deployed and parameter-tuned on the physical rig by Yecheng WANG. run-2fast.ino was a post-success attempt at a faster run time using a coarser trajectory and shorter step delay; it did not complete successfully and is archived for reference.

%% MECH2636 Robotic Arm Trajectory Simulation
% Team 4
% Author: Yecheng 'Geoffrey' WANG
% Version: 004 | Note: Global Constant Step-Size Interpolation (Velocity Optimized)
clear; clc; close all;

%% 1. Parameters
L1 = 250;           % Arm 1 Length in mm
L2 = 200;           % Arm 2 Length in mm
x_base_map = -60;   % Motor 1 X-offset in Colon Map
y_base_map = -60;   % Motor 1 Y-offset in Colon Map

% --- Gear Ratios ---
gear1 = 1.0;        % Arm 1 & Motor 1 ratio
gear2 = 1.0;        % Arm 2 & Motor 2 ratio

% --- Global Interpolation Parameter ---
% Target distance between each point in mm. 
% Smaller = Higher resolution/Slower speed. 
% Larger = Lower resolution/Faster speed.
target_step_size = 0.5; 

% --- Simulation Update Rate ---
sim_pause = 0.01; % (seconds)

%% 2. Data Loading & Constant Step-Size Interpolation
data = readtable('colon_coordinates.csv');
x_orig = data.x - x_base_map; 
y_orig = data.y - y_base_map;

% Process interpolation to maintain constant travel speed
x_robot = [];
y_robot = [];

for k = 1:length(x_orig)-1
    % Calculate the Euclidean distance of this segment
    dist_segment = sqrt((x_orig(k+1) - x_orig(k))^2 + (y_orig(k+1) - y_orig(k))^2);
    
    % Determine number of steps needed to maintain constant step size
    % Ensure at least 1 step even for very short segments
    num_steps = max(1, round(dist_segment / target_step_size));
    
    % Generate points for this segment
    segment_x = linspace(x_orig(k), x_orig(k+1), num_steps + 1);
    segment_y = linspace(y_orig(k), y_orig(k+1), num_steps + 1);
    
    % Append points (omitting the last point of segment to avoid duplicates)
    x_robot = [x_robot, segment_x(1:end-1)];
    y_robot = [y_robot, segment_y(1:end-1)];
end

% Append the very last point from the CSV
x_robot = [x_robot, x_orig(end)];
y_robot = [y_robot, y_orig(end)];

num_points = length(x_robot);
theta1_list = zeros(num_points, 1);
theta2_list = zeros(num_points, 1);
reachable = true(num_points, 1);

%% 3. IK Solver (Elbow-down)
for i = 1:num_points
    x = x_robot(i); y = y_robot(i);
    dist_sq = x^2 + y^2;
    
    % Reachability check
    if dist_sq > (L1 + L2)^2 || dist_sq < abs(L1 - L2)^2
        reachable(i) = false;
        theta1_list(i) = NaN;
        theta2_list(i) = NaN;
        continue;
    end
    
    cos_theta2 = (dist_sq - L1^2 - L2^2) / (2 * L1 * L2);
    
    % --- Elbow-down Configuration ---
    theta2 = acos(cos_theta2); 
    
    % Calculate theta1
    k1 = L1 + L2 * cos(theta2);
    k2 = L2 * sin(theta2);
    theta1 = atan2(y, x) - atan2(k2, k1);
    
    theta1_list(i) = theta1;
    theta2_list(i) = theta2;
end

%% 4. Data Export to XLSX
output_table = table((1:num_points)', x_robot', y_robot', theta1_list, theta2_list, ...
    'VariableNames', {'Point_ID', 'X_Robot_mm', 'Y_Robot_mm', 'Theta1_rad', 'Theta2_rad'});

filename = 'v4_ConstantStep_Trajectory_Output.xlsx';
writetable(output_table, filename);
fprintf('Success: %d points (with constant step size of %.2fmm) exported to %s\n', ...
        num_points, target_step_size, filename);

%% 5. Visualization
figure('Color', 'w', 'Name', 'Constant Step-Size Simulation (Elbow-down)','Position',[100,100,800,1200]);
grid on; hold on; axis equal;

% --- Adjustment 3: Fixed Axis Scale ---
xlim([-50, 320]); 
ylim([-100, 320]);

xlabel('X (mm) - Robot/Motor Map','FontSize',18);
ylabel('Y (mm) - Robot/Motor Map','FontSize',18);
set(gca,'FontSize',16);

% Plot original given points from CSV
plot(x_orig, y_orig, 'ro', 'MarkerSize', 5, 'DisplayName', 'Given 87 Keypoints');
% Plot interpolated path
plot(x_robot, y_robot, 'c.', 'MarkerSize', 2, 'DisplayName', 'Interpolation Points');

% Even if no points are unreachable, this creates the legend entry
h_unreach = plot(x_robot(~reachable), y_robot(~reachable), 'mx', 'MarkerSize', 10, ...
                 'LineWidth', 1.5, 'DisplayName', 'Unreachable Points');

h_arm = plot([0,0,0], [0,0,0], '-ok', 'LineWidth', 2, 'MarkerFaceColor', 'k', ...
             'DisplayName', 'Robotics Arms'); 

h_path = plot(NaN, NaN, 'g', 'LineWidth', 1.5, 'DisplayName', 'Actual Motion');
legend('Location', 'southwest','AutoUpdate','on','FontSize',18);

% Simulation Loop
for i = 1:num_points
    if ~reachable(i)
        continue; 
    end
    
    % Forward Kinematics for visualization
    x1 = L1 * cos(theta1_list(i));
    y1 = L1 * sin(theta1_list(i));
    x2 = x1 + L2 * cos(theta1_list(i) + theta2_list(i));
    y2 = y1 + L2 * sin(theta1_list(i) + theta2_list(i));
        
    % Update the green path showing progress
    set(h_path, 'XData', x_robot(1:i), 'YData', y_robot(1:i));

    % Update arm segments
    set(h_arm, 'XData', [0, x1, x2], 'YData', [0, y1, y2]);
    
    % Update Title
    title(sprintf('Elbow-Down | Point: %d/%d | Step: %.1fmm | Delay: %.3fs', ...
          i, num_points, target_step_size, sim_pause), 'FontSize',18);
    
    drawnow;
    pause(sim_pause); 
end

//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);
}

info: Full 1611 points' dataset is omitted above.
note: Initial control framework, retained for the record — the assessed final run used run_SUCCESSFUL.ino (see the Final Run tab). View Full Trajectory & Coordinates with their Analysis (.xlsx)

// MECH2636 Final Competition Run Code (SUCCESSFUL - the assessed, scored run)
// Team 4
// Author: Yecheng 'Geoffrey' WANG
// Deployed & parameter-tuned on the physical rig

// ========================================================================
// Must be included to store the 812-row super large array in Flash memory
#include <avr/pgmspace.h> // Flash memory
// ========================================================================

// 1. Pin Setup
// ==========Motor 1============
int motorStepPin_1 = 5;     
int motorDirPin_1 = 6;      
int motorEnablePin_1 = 7;   
// ---------Motor 2--------------
int motorStepPin_2 = 2;     
int motorDirPin_2 = 3;      
int motorEnablePin_2 = 4;   
// ===============================
int MSPin_1 = 9;            
int MSPin_2 = 10;           
int MSPin_3 = 11;           
int encPhaseA_1 = 18;       
int encPhaseB_1 = 19;       
int encPhaseA_2 = 20;       
int encPhaseB_2 = 21;       
int blackButtonPin = A9;    
int greenButtonPin = A10;   
int potPin = A8;            
 
// 2. Variables
volatile int encCount_1 = 0;            
volatile int encCount_2 = 0;            
int currentStep_1 = 0;                  
int currentStep_2 = 0;                  
int desiredStep_1 = 0;                  
int desiredStep_2 = 0;                  
int currentPos = 0;                     
int lastPos = 581; // total lines of points                       
int stepdt = 39;     // speed control - 40 for 400-600,30 for 800                
int buttonPressed = 0;                  

// =========================================================================
//  581 lines of {motor1, motor2} to [812][2], PROGMEM automatically Flash
//  + positive is ACW - anti clockwise
// =========================================================================
const int trajectory[581][2] PROGMEM = {
{0,0},
{1,0},
{2,0},
{3,0},
{4,-1},
{5,-1},
{7,-1},
{8,-1},
{9,-1},
{10,-1},

  // ... 568 intermediate rows omitted for display ...
  // The complete 581-row trajectory is included in the downloadable sketch below.

{53,-14},
{55,-16},
{57,-20}
};

// declare interrupt function
void encInc_1();
void encInc_2();

void setup() {
  pinMode(motorDirPin_1, OUTPUT);       
  pinMode(motorEnablePin_1, OUTPUT);    
  pinMode(motorStepPin_1, OUTPUT);      
  pinMode(motorDirPin_2 , OUTPUT);      
  pinMode(motorEnablePin_2, OUTPUT);    
  pinMode(motorStepPin_2, OUTPUT);      
  
  pinMode(MSPin_1, OUTPUT);             
  pinMode(MSPin_2, OUTPUT);             
  pinMode(MSPin_3, OUTPUT);             
  
  pinMode(encPhaseA_1, INPUT);         
  pinMode(encPhaseB_1, INPUT);         
  pinMode(encPhaseA_2, INPUT);         
  pinMode(encPhaseB_2, INPUT);         
  
  // Enable the internal pull-up resistor to prevent false triggering caused by floating button levels.
  pinMode(blackButtonPin, INPUT_PULLUP);      
  pinMode(greenButtonPin, INPUT_PULLUP);      
  pinMode(potPin, INPUT);              

  attachInterrupt(digitalPinToInterrupt(encPhaseA_1), encInc_1, CHANGE); 
  attachInterrupt(digitalPinToInterrupt(encPhaseB_1), encInc_1, CHANGE); 
  attachInterrupt(digitalPinToInterrupt(encPhaseA_2), encInc_2, CHANGE); 
  attachInterrupt(digitalPinToInterrupt(encPhaseB_2), encInc_2, CHANGE); 

  Serial.begin(115200); // Increase the baud rate to accommodate high-frequency data streams
  Serial.println("System Initialized. Waiting for input...");
}

void loop(){                              
  digitalWrite(motorEnablePin_1, HIGH);  // Cut off the power supply to allow manual resetting of the robotic arm to zero position.        
  digitalWrite(motorEnablePin_2, HIGH);          
  buttonPressed = 0;                         

  // Polling for button press
  while (buttonPressed == 0) {                 
    if (analogRead(blackButtonPin) < 100) {      
      buttonPressed = 1;                       
    }
    else if (analogRead(greenButtonPin) < 100) { 
      buttonPressed = 2;                       
    }
    delay(50);                               
  }
  
  encCount_1 = 0;                             
  encCount_2 = 0;                             
  
  digitalWrite(motorEnablePin_1, LOW);   // Lock the motor and prepare for operation 
  digitalWrite(motorEnablePin_2, LOW);          
   
  // Microstepping Setup 1/8 here
  digitalWrite(MSPin_1, LOW);                    
  digitalWrite(MSPin_2, HIGH);                    
  digitalWrite(MSPin_3, LOW);                    
   
  currentPos = 0;
  currentStep_1 = 0; 
  currentStep_2 = 0;
   
  Serial.println("Executing Trajectory...");

  // Synchronous Blocking State Machine
  while (currentPos < lastPos)  {     
    
    // use pgm_read_word to read target step from Flash memo
    desiredStep_1 = pgm_read_word(&(trajectory[currentPos][0]));   
    desiredStep_2 = pgm_read_word(&(trajectory[currentPos][1]));    
    
    // =========== Motor 1 (Base) Control ===========
    if (currentStep_1 < desiredStep_1) {             
      digitalWrite(motorDirPin_1, LOW);     // HIGH --> LOW       
      digitalWrite(motorStepPin_1, HIGH);           
      delayMicroseconds(500); 
      digitalWrite(motorStepPin_1, LOW); 
      currentStep_1++; 
    }
    else if (currentStep_1 > desiredStep_1) {
      digitalWrite(motorDirPin_1, HIGH);    // LOW --> HIGH        
      digitalWrite(motorStepPin_1, HIGH);           
      delayMicroseconds(500);
      digitalWrite(motorStepPin_1, LOW);  
      currentStep_1--; 
    }

    // =========== Motor 2 (Elbow) Control ===========
    if (currentStep_2 < desiredStep_2) {        
      digitalWrite(motorDirPin_2, LOW);     // HIGH --> LOW         
      digitalWrite(motorStepPin_2, HIGH);           
      delayMicroseconds(500);
      digitalWrite(motorStepPin_2, LOW); 
      currentStep_2++;  
    }
    else if (currentStep_2 > desiredStep_2) {
      digitalWrite(motorDirPin_2, HIGH);    // LOW --> HIGH        
      digitalWrite(motorStepPin_2, HIGH);           
      delayMicroseconds(500);
      digitalWrite(motorStepPin_2, LOW); 
      currentStep_2--;  
    }

    // Synchronization barrier 
    // Both motors must reach the current target before proceeding to the next point.
    if (currentStep_1 == desiredStep_1 && currentStep_2 == desiredStep_2) {  
      currentPos++;
    }
     
    delay(stepdt); // speed control
    
    // Reduce the frequency of debug logs to avoid slowdowns caused by blocking
    if (currentPos % 100 == 0) {
        Serial.print("Pos: "); Serial.print(currentPos);
        Serial.print(" | Enc1: "); Serial.print(encCount_1);
        Serial.print(" | Enc2: "); Serial.println(encCount_2);
    }
  }
  
  Serial.println("Run Complete.");
  delay(1000);
}

// ===============================================================================================
// Encoder interrupt function (for telemetry monitoring only, not involved in closed-loop control)
// ===============================================================================================
void encInc_1() {
   static bool prevPhaseA;
   static bool prevPhaseB;
   bool currentPhaseA = digitalRead(encPhaseA_1);                                       
   bool currentPhaseB = digitalRead(encPhaseB_1); 
   if (currentPhaseA != prevPhaseA) {                                              
     if (digitalRead(encPhaseA_1) == HIGH && digitalRead(encPhaseB_1) == HIGH) encCount_1 ++;
     else if (digitalRead(encPhaseA_1) == HIGH && digitalRead(encPhaseB_1) == LOW) encCount_1 --;
     else if (digitalRead(encPhaseA_1) == LOW && digitalRead(encPhaseB_1) == HIGH) encCount_1 --;
     else encCount_1 ++;
   }
   else if (currentPhaseB != prevPhaseB) {                                         
     if (digitalRead(encPhaseA_1) == HIGH && digitalRead(encPhaseB_1) == HIGH) encCount_1 --;
     else if (digitalRead(encPhaseA_1) == HIGH && digitalRead(encPhaseB_1) == LOW) encCount_1 ++;
     else if (digitalRead(encPhaseA_1) == LOW && digitalRead(encPhaseB_1) == HIGH) encCount_1 ++;
     else encCount_1 --;
   }
   prevPhaseA = currentPhaseA; 
   prevPhaseB = currentPhaseB;  
}

void encInc_2() {
   static bool prevPhaseA;
   static bool prevPhaseB;
   bool currentPhaseA = digitalRead(encPhaseA_2);                                       
   bool currentPhaseB = digitalRead(encPhaseB_2); 
   if (currentPhaseA != prevPhaseA) {                                              
     if (digitalRead(encPhaseA_2) == HIGH && digitalRead(encPhaseB_2) == HIGH) encCount_2 ++;
     else if (digitalRead(encPhaseA_2) == HIGH && digitalRead(encPhaseB_2) == LOW) encCount_2 --;
     else if (digitalRead(encPhaseA_2) == LOW && digitalRead(encPhaseB_2) == HIGH) encCount_2 --;
     else encCount_2 ++;
   }
   else if (currentPhaseB != prevPhaseB) {                                      
     if (digitalRead(encPhaseA_2) == HIGH && digitalRead(encPhaseB_2) == HIGH) encCount_2 --;
     else if (digitalRead(encPhaseA_2) == HIGH && digitalRead(encPhaseB_2) == LOW) encCount_2 ++;
     else if (digitalRead(encPhaseA_2) == LOW && digitalRead(encPhaseB_2) == HIGH) encCount_2 ++;
     else encCount_2 --;
   }
   prevPhaseA = currentPhaseA; 
   prevPhaseB = currentPhaseB;  
}

info: 568 of 581 trajectory rows omitted above — download the complete sketch below.
warn: Keep the two .ino sketches in separate folders — the Arduino IDE compiles every .ino in a folder together, causing duplicate pin-definition errors (see run/readme.txt). Download run_SUCCESSFUL.ino

// MECH2636 Post-Success Speed-Run Attempt (NOT successful - archived for reference)
// Team 4
// Author: Yecheng 'Geoffrey' WANG
// Coarser 438-point trajectory + reduced step delay for a faster run time

// ========================================================================
// Must be included to store the 812-row super large array in Flash memory
#include <avr/pgmspace.h> // Flash memory
// ========================================================================

// 1. Pin Setup
// ==========Motor 1============
int motorStepPin_1 = 5;     
int motorDirPin_1 = 6;      
int motorEnablePin_1 = 7;   
// ---------Motor 2--------------
int motorStepPin_2 = 2;     
int motorDirPin_2 = 3;      
int motorEnablePin_2 = 4;   
// ===============================
int MSPin_1 = 9;            
int MSPin_2 = 10;           
int MSPin_3 = 11;           
int encPhaseA_1 = 18;       
int encPhaseB_1 = 19;       
int encPhaseA_2 = 20;       
int encPhaseB_2 = 21;       
int blackButtonPin = A9;    
int greenButtonPin = A10;   
int potPin = A8;            
 
// 2. Variables
volatile int encCount_1 = 0;            
volatile int encCount_2 = 0;            
int currentStep_1 = 0;                  
int currentStep_2 = 0;                  
int desiredStep_1 = 0;                  
int desiredStep_2 = 0;                  
int currentPos = 0;                     
int lastPos = 438; // total lines of points                       
int stepdt = 30;     // speed control - 40 for 400-600,30 for 800                
int buttonPressed = 0;                  

// =========================================================================
//  581 lines of {motor1, motor2} to [812][2], PROGMEM automatically Flash
//  + positive is ACW - anti clockwise
// =========================================================================
const int trajectory[438][2] PROGMEM = {
{0,0},
{1,0},
{3,0},
{4,-1},
{6,-1},
{7,-1},
{9,-2},
{10,-2},
{12,-2},
{13,-2},

  // ... 425 intermediate rows omitted for display ...
  // The complete 438-row trajectory is included in the downloadable sketch below.

{53,-16},
{55,-18},
{57,-20}
};

// declare interrupt function
void encInc_1();
void encInc_2();

void setup() {
  pinMode(motorDirPin_1, OUTPUT);       
  pinMode(motorEnablePin_1, OUTPUT);    
  pinMode(motorStepPin_1, OUTPUT);      
  pinMode(motorDirPin_2 , OUTPUT);      
  pinMode(motorEnablePin_2, OUTPUT);    
  pinMode(motorStepPin_2, OUTPUT);      
  
  pinMode(MSPin_1, OUTPUT);             
  pinMode(MSPin_2, OUTPUT);             
  pinMode(MSPin_3, OUTPUT);             
  
  pinMode(encPhaseA_1, INPUT);         
  pinMode(encPhaseB_1, INPUT);         
  pinMode(encPhaseA_2, INPUT);         
  pinMode(encPhaseB_2, INPUT);         
  
  // Enable the internal pull-up resistor to prevent false triggering caused by floating button levels.
  pinMode(blackButtonPin, INPUT_PULLUP);      
  pinMode(greenButtonPin, INPUT_PULLUP);      
  pinMode(potPin, INPUT);              

  attachInterrupt(digitalPinToInterrupt(encPhaseA_1), encInc_1, CHANGE); 
  attachInterrupt(digitalPinToInterrupt(encPhaseB_1), encInc_1, CHANGE); 
  attachInterrupt(digitalPinToInterrupt(encPhaseA_2), encInc_2, CHANGE); 
  attachInterrupt(digitalPinToInterrupt(encPhaseB_2), encInc_2, CHANGE); 

  Serial.begin(115200); // Increase the baud rate to accommodate high-frequency data streams
  Serial.println("System Initialized. Waiting for input...");
}

void loop(){                              
  digitalWrite(motorEnablePin_1, HIGH);  // Cut off the power supply to allow manual resetting of the robotic arm to zero position.        
  digitalWrite(motorEnablePin_2, HIGH);          
  buttonPressed = 0;                         

  // Polling for button press
  while (buttonPressed == 0) {                 
    if (analogRead(blackButtonPin) < 100) {      
      buttonPressed = 1;                       
    }
    else if (analogRead(greenButtonPin) < 100) { 
      buttonPressed = 2;                       
    }
    delay(50);                               
  }
  
  encCount_1 = 0;                             
  encCount_2 = 0;                             
  
  digitalWrite(motorEnablePin_1, LOW);   // Lock the motor and prepare for operation 
  digitalWrite(motorEnablePin_2, LOW);          
   
  // Microstepping Setup 1/8 here
  digitalWrite(MSPin_1, LOW);                    
  digitalWrite(MSPin_2, HIGH);                    
  digitalWrite(MSPin_3, LOW);                    
   
  currentPos = 0;
  currentStep_1 = 0; 
  currentStep_2 = 0;
   
  Serial.println("Executing Trajectory...");

  // Synchronous Blocking State Machine
  while (currentPos < lastPos)  {     
    
    // use pgm_read_word to read target step from Flash memo
    desiredStep_1 = pgm_read_word(&(trajectory[currentPos][0]));   
    desiredStep_2 = pgm_read_word(&(trajectory[currentPos][1]));    
    
    // =========== Motor 1 (Base) Control ===========
    if (currentStep_1 < desiredStep_1) {             
      digitalWrite(motorDirPin_1, LOW);     // HIGH --> LOW       
      digitalWrite(motorStepPin_1, HIGH);           
      delayMicroseconds(500); 
      digitalWrite(motorStepPin_1, LOW); 
      currentStep_1++; 
    }
    else if (currentStep_1 > desiredStep_1) {
      digitalWrite(motorDirPin_1, HIGH);    // LOW --> HIGH        
      digitalWrite(motorStepPin_1, HIGH);           
      delayMicroseconds(500);
      digitalWrite(motorStepPin_1, LOW);  
      currentStep_1--; 
    }

    // =========== Motor 2 (Elbow) Control ===========
    if (currentStep_2 < desiredStep_2) {        
      digitalWrite(motorDirPin_2, LOW);     // HIGH --> LOW         
      digitalWrite(motorStepPin_2, HIGH);           
      delayMicroseconds(500);
      digitalWrite(motorStepPin_2, LOW); 
      currentStep_2++;  
    }
    else if (currentStep_2 > desiredStep_2) {
      digitalWrite(motorDirPin_2, HIGH);    // LOW --> HIGH        
      digitalWrite(motorStepPin_2, HIGH);           
      delayMicroseconds(500);
      digitalWrite(motorStepPin_2, LOW); 
      currentStep_2--;  
    }

    // Synchronization barrier 
    // Both motors must reach the current target before proceeding to the next point.
    if (currentStep_1 == desiredStep_1 && currentStep_2 == desiredStep_2) {  
      currentPos++;
    }
     
    delay(stepdt); // speed control
    
    // Reduce the frequency of debug logs to avoid slowdowns caused by blocking
    if (currentPos % 100 == 0) {
        Serial.print("Pos: "); Serial.print(currentPos);
        Serial.print(" | Enc1: "); Serial.print(encCount_1);
        Serial.print(" | Enc2: "); Serial.println(encCount_2);
    }
  }
  
  Serial.println("Run Complete.");
  delay(1000);
}

// ===============================================================================================
// Encoder interrupt function (for telemetry monitoring only, not involved in closed-loop control)
// ===============================================================================================
void encInc_1() {
   static bool prevPhaseA;
   static bool prevPhaseB;
   bool currentPhaseA = digitalRead(encPhaseA_1);                                       
   bool currentPhaseB = digitalRead(encPhaseB_1); 
   if (currentPhaseA != prevPhaseA) {                                              
     if (digitalRead(encPhaseA_1) == HIGH && digitalRead(encPhaseB_1) == HIGH) encCount_1 ++;
     else if (digitalRead(encPhaseA_1) == HIGH && digitalRead(encPhaseB_1) == LOW) encCount_1 --;
     else if (digitalRead(encPhaseA_1) == LOW && digitalRead(encPhaseB_1) == HIGH) encCount_1 --;
     else encCount_1 ++;
   }
   else if (currentPhaseB != prevPhaseB) {                                         
     if (digitalRead(encPhaseA_1) == HIGH && digitalRead(encPhaseB_1) == HIGH) encCount_1 --;
     else if (digitalRead(encPhaseA_1) == HIGH && digitalRead(encPhaseB_1) == LOW) encCount_1 ++;
     else if (digitalRead(encPhaseA_1) == LOW && digitalRead(encPhaseB_1) == HIGH) encCount_1 ++;
     else encCount_1 --;
   }
   prevPhaseA = currentPhaseA; 
   prevPhaseB = currentPhaseB;  
}

void encInc_2() {
   static bool prevPhaseA;
   static bool prevPhaseB;
   bool currentPhaseA = digitalRead(encPhaseA_2);                                       
   bool currentPhaseB = digitalRead(encPhaseB_2); 
   if (currentPhaseA != prevPhaseA) {                                              
     if (digitalRead(encPhaseA_2) == HIGH && digitalRead(encPhaseB_2) == HIGH) encCount_2 ++;
     else if (digitalRead(encPhaseA_2) == HIGH && digitalRead(encPhaseB_2) == LOW) encCount_2 --;
     else if (digitalRead(encPhaseA_2) == LOW && digitalRead(encPhaseB_2) == HIGH) encCount_2 --;
     else encCount_2 ++;
   }
   else if (currentPhaseB != prevPhaseB) {                                      
     if (digitalRead(encPhaseA_2) == HIGH && digitalRead(encPhaseB_2) == HIGH) encCount_2 --;
     else if (digitalRead(encPhaseA_2) == HIGH && digitalRead(encPhaseB_2) == LOW) encCount_2 ++;
     else if (digitalRead(encPhaseA_2) == LOW && digitalRead(encPhaseB_2) == HIGH) encCount_2 ++;
     else encCount_2 --;
   }
   prevPhaseA = currentPhaseA; 
   prevPhaseB = currentPhaseB;  
}

info: 425 of 438 trajectory rows omitted above — download the complete sketch below.
warn: This speed-run variant did not complete successfully on the rig; it is archived for reference only. Download run-2fast.ino

6. Reference

[1] The university of Leeds (2026). MECH Classy Colonoscopy assignment [Powerpoint slides] School of Mechanical Engineering, Minerva.