Diff: main.cpp

Revision:

68:2879967ebb25

Parent:

63:d17f45d88c7a

Child:

69:bfefdfb04c29

--- a/main.cpp	Fri Nov 01 00:58:58 2019 +0000
+++ b/main.cpp	Tue Nov 05 10:01:56 2019 +0000
@@ -40,7 +40,7 @@
 AnalogIn potmeter1 (A4);
 AnalogIn potmeter2 (A5);
 
-// Encoder inputs
+// Motor Encoder inputs
 DigitalIn encA1(D9);
 DigitalIn encB1(D8);
 DigitalIn encA2(D13);
@@ -114,8 +114,8 @@
 bool led_color_changed = true; // Enable LED entry functions
 
 // Global constants
-const double Fs = 500.0;
-const double Ts = 1/Fs;
+const double Fs = 500.0; //Sampling frequency
+const double Ts = 1/Fs;     
 
 /*
 ------------------------------ LED COLOR FUNCTIONS ------------------------------
@@ -197,8 +197,6 @@
 /*
 ------------------------------ HELPER FUNCTIONS ------------------------------
 */
-// Empty placeholder function, needs to be deleted at end of project
-void doStuff() {}
 
 // Return max value of vector
 double getMax(const vector<double> &vect)
@@ -590,15 +588,15 @@
 QEI encoder2(D12, D13, NC, encoder_res, QEI::X4_ENCODING);   //Encoder of motor 2
 
 // Initialize variables for encoder reading
-volatile int counts1;
-volatile int counts1af;
-int counts1offset;
+volatile int counts1; //Counts after compensation with offset
+volatile int counts1af; //Counts measured by encoder
+int counts1offset; //Offset due to calibration
 volatile int countsPrev1 = 0;
 volatile int deltaCounts1;
 
-volatile int counts2;
-volatile int counts2af;
-int counts2offset;
+volatile int counts2; // Counts after compensation with offset
+volatile int counts2af; //Counts measured by encoder
+int counts2offset; //Offset due to calibration
 volatile int countsPrev2 = 0;
 volatile int deltaCounts2;
 
@@ -619,59 +617,61 @@
 const float l1 = 26.0; // Distance base-joint2 [cm]
 const float l2 = 62.0; // Distance join2-endpoint [cm]
 
-float q1 = -10.0f * deg2rad; // Angle of first joint [rad] (starts off in reference position)
+float q1 = -10.0f * deg2rad; // Angle of first joint [rad] (in calibration position)
 float q1dot; // Velocity of first joint [rad/s]
 
-float q2 = -140.0f * deg2rad;
-float q2dot;
+float q2 = -140.0f * deg2rad; // Angle of second joint [rad] (in calibration position
+float q2dot; // Velocity of second joint [rad/s]
 
-float Vx = 0.0; // Desired linear velocity x direction
-float Vy = 0.0; // Desired linear velocity y direction
+float Vx = 0.0; // Desired linear velocity x direction [cm/s]
+float Vy = 0.0; // Desired linear velocity y direction [cm/s]
 
 float xe; // Endpoint x position [cm]
 float ye; // Endpoint y position [cm]
 
 // Motor angles in starting position
 const float motor1_init = ( q1 + q2 ) * gearratio1; // Measured angle motor 1 in initial (starting) position
-float motor1_ref = motor1_init; // Expected motor angle
+float motor1_ref = motor1_init; // Expected motor angle (starting from initial position)
 float motor1_angle = motor1_init; // Actual motor angle
 
 const float motor2_init = q1 * gearratio2; // Measured angle motor 2 in initial (starting) position
-float motor2_ref = motor2_init;
-float motor2_angle = motor2_init;
+float motor2_ref = motor2_init; // Motor 2 reference position (starting from initial position)
+float motor2_angle = motor2_init;   // Actual motor angle
 
 // Initialize variables for motor control
 float motor1offset; // Offset during calibration
-float omega1; //velocity (rad/s)
+float omega1; //angular velocity of motor [rad/s]
 bool motordir1; // Toggle of motor direction
-double controlsignal1; // ??
-float motor1_error; // Error between encoder and reference
+double controlsignal1; // Result of the PID controller
+float motor1_error; // Error between desired and actual angle of the motor
 
-float motor2offset;
-float omega2;
-bool motordir2;
-double controlsignal2;
-float motor2_error;
+float motor2offset; // Offset during calibration
+float omega2; //Angular velocity  of motor [rad/s]
+bool motordir2; // Toggle of motor direction
+double controlsignal2; // Result of the PID controller
+float motor2_error; // Error between desired and actual angle of the motor
 
 // Initialize variables for PID controller
 float Kp = 0.27; // Proportional gain
 float Ki = 0.35; // Integral gain
-float Kd = 0.1;  // Derivative gain
-float Ka = 1.0;
+float Kd = 0.1; // Derivative gain
+float Ka = 1.0; //Windup gain
 
-float u_p1; //
-float u_i1; //
-float ux1; //
-float up1; // Proportional contribution
-float ek1; //
+float u_p1; // Proportional contribution
+float u_i1; // Integral contribution
+float u_d1; // Derivative contribution
+float ux1; // Windup error (before multiplication with Ka)
+float up1; // Summed contributions
+float ek1; // Windup error
 float ei1 = 0.0; // Integral error (starts at 0)
 
-float u_p2;
-float u_i2;
-float ux2;
-float up2;
-float ek2;
-float ei2 = 0.0;
+float u_p2; // Proportional contribution
+float u_i2; // Integral contribution
+float u_d2; // Derivative contribution
+float ux2; // Windup error (before multiplication with Ka)
+float up2; // Summed contributions
+float ek2; // Windup error
+float ei2 = 0.0; // Integral error (starts at 0)
 
 /*
 ------------------------------ MOTOR FUNCTIONS ------------------------------
@@ -680,14 +680,14 @@
 {
     // Motor 1
     static float error_integral1 = 0.0;
-    static float e_prev1 = motor1_error;
+    static float e_prev1 = motor1_error; // Saving the previous error, needed for derivative computation
 
     //Proportional part
     u_p1 = Kp * motor1_error;
 
     //Integral part
-    if ( motor_curr_state == motor_finish || operation_curr_state == operation_movement || demo_curr_state == demo_XY || demo_curr_state == demo_positioning ) {
-        error_integral1 = error_integral1 + ei1 * Ts;
+    if ( motor_curr_state == motor_finish || operation_curr_state == operation_movement || demo_curr_state == demo_XY || demo_curr_state == demo_positioning ) { // Only active during operational states to prevent windup
+        error_integral1 = error_integral1 + ei1 * Ts; 
         u_i1 = Ki * error_integral1;
     }
 
@@ -697,9 +697,9 @@
     e_prev1 = motor1_error;
 
     // Sum and limit
-    up1 = u_p1 + u_i1 + u_d1;
-    if ( up1 > 1.0f ) {
-        controlsignal1 = 1.0f;
+    up1 = u_p1 + u_i1 + u_d1;   // Sum the contributions of P, I and D
+    if ( up1 > 1.0f ) {         // Saturated limit of motor, contintue with value between -1 and 1
+        controlsignal1 = 1.0f;  
     } else if ( up1 < -1.0f ) {
         controlsignal1 = -1.0f;
     } else {
@@ -707,19 +707,19 @@
     }
 
     // To prevent windup
-    ux1 = up1 - controlsignal1;
+    ux1 = up1 - controlsignal1; // Computation of the overflow
     ek1 = Ka * ux1;
-    ei1 = motor1_error - ek1;
+    ei1 = motor1_error - ek1;   // Integral error with windup subtracted 
 
     // Motor 2
     static float error_integral2 = 0.0;
-    static float e_prev2 = motor2_error;
+    static float e_prev2 = motor2_error; // Saving the previous error, needed for derivative computation
 
     //Proportional part:
     u_p2 = Kp * motor2_error;
 
     //Integral part
-    if ( motor_curr_state == motor_finish || operation_curr_state == operation_movement || demo_curr_state == demo_XY || demo_curr_state == demo_positioning ) {
+    if ( motor_curr_state == motor_finish || operation_curr_state == operation_movement || demo_curr_state == demo_XY || demo_curr_state == demo_positioning ) { // Only active during operational states to prevent windup
         error_integral2 = error_integral2 + ei2 * Ts;
         u_i2 = Ki * error_integral2;
     }
@@ -730,8 +730,8 @@
     e_prev2 = motor2_error;
 
     // Sum and limit
-    up2 = u_p2 + u_i2 + u_d2;
-    if ( up2 > 1.0f ) {
+    up2 = u_p2 + u_i2 + u_d2; // Sum the contributions of P, I and D
+    if ( up2 > 1.0f ) { // Saturated limit of motor, contintue with value between -1 and 1
         controlsignal2 = 1.0f;
     } else if ( up2 < -1.0f ) {
         controlsignal2 = -1.0f;
@@ -740,78 +740,78 @@
     }
 
     // To prevent windup
-    ux2 = up2 - controlsignal2;
+    ux2 = up2 - controlsignal2; // Computation of the overflow
     ek2 = Ka * ux2;
-    ei2 = motor2_error - ek2;
+    ei2 = motor2_error - ek2;   // Integral error with windup subtracted 
 }
 
 void RKI()
 {
     // Derived function for angular velocity of joint angles
-    q1dot = (l2*cos(q1+q2)*Vx+l2*sin(q1+q2)*Vy)/((-l1*sin(q1)-l2*sin(q1+q2))*l2*cos(q1+q2)+l2*sin(q1+q2)*(l1*cos(q1)+l2*cos(q1+q2)));
-    q2dot = ((-l1*cos(q1)-l2*cos(q1+q2))*Vx+(-l1*sin(q1)-l2*sin(q1+q2))*Vy)/((-l1*sin(q1)-l2*sin(q1+q2))*l2*cos(q1+q2)+l2*sin(q1+q2)*(l1*cos(q1)+l2*cos(q1+q2)));
-    q1 = q1 + q1dot * Ts;
-    q2 = q2 + q2dot * Ts;
+    q1dot = (l2*cos(q1+q2)*Vx+l2*sin(q1+q2)*Vy)/((-l1*sin(q1)-l2*sin(q1+q2))*l2*cos(q1+q2)+l2*sin(q1+q2)*(l1*cos(q1)+l2*cos(q1+q2))); // Computation of the desired angular velocities with given linear velocities Vx and Vy
+    q2dot = ((-l1*cos(q1)-l2*cos(q1+q2))*Vx+(-l1*sin(q1)-l2*sin(q1+q2))*Vy)/((-l1*sin(q1)-l2*sin(q1+q2))*l2*cos(q1+q2)+l2*sin(q1+q2)*(l1*cos(q1)+l2*cos(q1+q2))); // Computation of the desired angular velocities with given linear velocities Vx and Vy
+    q1 = q1 + q1dot * Ts; // Calculating desired joint angle by integrating the desired angular velocity
+    q2 = q2 + q2dot * Ts; // Calculating desired joint angle by integrating the desired angular velocity
 
-    xe = l1 * cos(q1) + l2 * cos(q1+q2);
-    ye = l1 * sin(q1) + l2 * sin(q1+q2);
+    xe = l1 * cos(q1) + l2 * cos(q1+q2); // Calculation of the current endpoint position
+    ye = l1 * sin(q1) + l2 * sin(q1+q2); // Calculation of the current endpoint position
 
-    if ( q1 < -5.0f * deg2rad) {
+    if ( q1 < -5.0f * deg2rad) {            // Setting limits to the joint angles
         q1 = -5.0f * deg2rad;
-    } else if ( q1 > 65.0f * deg2rad ) {
+    } else if ( q1 > 65.0f * deg2rad ) {    // Setting limits to the joint angles
         q1 = 65.0f * deg2rad;
     } else {
         q1 = q1;
     }
 
-    if ( q2 > -50.0f * deg2rad ) {
+    if ( q2 > -50.0f * deg2rad ) {          // Setting limits to the joint angles
         q2 = -50.0f * deg2rad;
-    } else if ( q2 < -138.0f * deg2rad ) {
+    } else if ( q2 < -138.0f * deg2rad ) {  // Setting limits to the joint angles
         q2 = -138.0f * deg2rad;
     } else {
         q2 = q2;
     }
 
-    motor1_ref = 5.5f * q1 + 5.5f * q2;
-    motor2_ref = 2.75f * q1;
+    motor1_ref = 5.5f * q1 + 5.5f * q2;     // Calculating the desired motor angle to attain the correct joint angles
+    motor2_ref = 2.75f * q1;                // Calculating the desired motor angle to attain the correct joint angles
 }
 
 void motorControl()
 {
-    counts1 = counts1af - counts1offset;
-    motor1_angle = (counts1 * encoder_factorin / gearbox_ratio) + motor1_init; // Angle of motor shaft in rad + correctie voor q1 en q2
-    omega1 = deltaCounts1 / Ts * encoder_factorin / gearbox_ratio; // Angular velocity of motor shaft in rad/s
-    motor1_error = motor1_ref - motor1_angle;
-    if ( controlsignal1 < 0.0f ) {
+    counts1 = counts1af - counts1offset;    // Counts measured by encoder compensated with the offset due to calibration
+    motor1_angle = (counts1 * encoder_factorin / gearbox_ratio) + motor1_init; // Angle of motor shaft [rad] + correction of initial value
+    omega1 = deltaCounts1 / Ts * encoder_factorin / gearbox_ratio; // Angular velocity of motor shaft [rad/s]
+    motor1_error = motor1_ref - motor1_angle; // Difference between desired and measured angle of motor
+    if ( controlsignal1 < 0.0f ) { // Determine the direction
         motordir1 = 0;
     } else {
         motordir1 = 1;
     }
 
-    motor1Power.write(abs(controlsignal1));
-    motor1Direction = motordir1;
+    motor1Power.write(abs(controlsignal1)); // Assigning the desired power to the motor
+    motor1Direction = motordir1; // Assigning the desired direction
 
-    counts2 = counts2af - counts2offset;
-    motor2_angle = (counts2 * encoder_factorin / gearbox_ratio) + motor2_init; // Angle of motor shaft in rad + correctie voor q1
-    omega2 = deltaCounts2 / Ts * encoder_factorin / gearbox_ratio; // Angular velocity of motor shaft in rad/s
-    motor2_error = motor2_ref-motor2_angle;
-    if ( controlsignal2 < 0.0f ) {
+    counts2 = counts2af - counts2offset; // Counts measured by encoder compensated with the offset due to calibration
+    motor2_angle = (counts2 * encoder_factorin / gearbox_ratio) + motor2_init; // Angle of motor shaft [rad] + correction of initial value
+    omega2 = deltaCounts2 / Ts * encoder_factorin / gearbox_ratio; // Angular velocity of motor shaft [rad/s]
+    motor2_error = motor2_ref-motor2_angle; // Difference between desired and measured angle of motor
+    if ( controlsignal2 < 0.0f ) { // Determine the direction
         motordir2 = 0;
     } else {
         motordir2 = 1;
     }
     if (motor_encoder_cal_done == true) {
-        motor2Power.write(abs(controlsignal2));
+        motor2Power.write(abs(controlsignal2)); // Assinging the desired power to the motor
     }
-    motor2Direction = motordir2;
+    motor2Direction = motordir2; // Assigning the desired direction to the motor
 }
 
 void motorKillPower()
 {
-    motor1Power.write(0.0f);
-    motor2Power.write(0.0f);
-    Vx=0.0f;
-    Vy=0.0f;
+    motor1Power.write(0.0f); // Setting motor power to 0 to stop motion
+    motor2Power.write(0.0f); // Setting motor power to 0 to stop motion
+    Vx=0.0f; // Setting desired linear motion to 0 to prevent RKI from adjusting the motorpower
+    Vy=0.0f; // Setting desired linear motion to 0 to prevent RKI from adjusting the motorpower
 }
 
 /*
@@ -824,10 +824,10 @@
         motor_state_changed = false;
         // More functions
     }
-    motor1Power.write(0.0f);
+    motor1Power.write(0.0f); // Giving the motor no power to be able to adjust the robot configuration
     motor2Power.write(0.0f);
-    counts1offset = counts1af ;
-    counts2offset = counts2af;
+    counts1offset = counts1af ; // Storing the offset of the encoder
+    counts2offset = counts2af; // Storing the offset of the encoder
 
     // State transition guard
     if ( button2_pressed ) {
@@ -849,7 +849,7 @@
 
     // Do stuff until end condition is true
     PID_controller();
-    motorControl();
+    motorControl(); //These three functions cause the robot to move within the link limits
     RKI();
 
     // State transition guard
@@ -1018,8 +1018,8 @@
         timerStateChange.start();
     }
 
-    Vx = 5.0;
-    Vy = 5.0;
+    Vx = 5.0; // Move in the positive x direction
+    Vy = 5.0; // Move in the positive y direction
 
     PID_controller();
     motorControl();
@@ -1027,8 +1027,6 @@
 
     scope.set(0, motor2_ref * rad2deg );
     scope.set(1, motor2_angle * rad2deg );
-    //scope.set(2, motor2_ref );
-    //scope.set(3, motor2_angle );
     scope.send();
 
     // State transition guard
@@ -1049,7 +1047,7 @@
     // Do stuff until end condition is met
     static float t = 0.0;
 
-    if ( t >= 0.0f && t < 3.0f ) {
+    if ( t >= 0.0f && t < 3.0f ) {          // With this code the endpoint will make a square every 12 seconds
         Vx = 5.0;
         Vy = 0.0;
     } else if ( t >= 3.0f && t < 6.0f ) {
@@ -1387,7 +1385,7 @@
 {
     sampleSignals();
     global_state_machine();
-    changeservo();
+    changeservo(); // Servo angle can be adjusted during every state
 }
 
 /*
@@ -1439,10 +1437,6 @@
     while(true) {
         pc.printf("Global state: %i EMG state: %i Motor state: %i Operation state: %i Demo state: %i\r\n", global_curr_state, emg_curr_state, motor_curr_state, operation_curr_state, demo_curr_state);
         pc.printf("Potmeter 1: %f Potmeter 2: %f\r\n", potmeter1.read(), potmeter2.read());
-        //pc.printf("EMG1 direction: %f EMG2 direction: %f \r\n", emg1_dir, emg2_dir);
-        //pc.printf("q1: %f q2: %f \r\n",q1*rad2deg,q2*rad2deg);
-        //pc.printf("Motor1ref: %f Motor1angle: %f\r\n",motor1_ref*rad2deg/5.5f,motor1_angle*rad2deg/5.5f);
-        //pc.printf("Motor2ref: %f Motor2angle: %f\r\n",motor2_ref*rad2deg/2.75f,motor2_angle*rad2deg/2.75f);
         //pc.printf("Xe: %f Ye: %f\r\n",xe,ye);
         wait(0.5f);
     }