Diff: THE.cpp

Revision:

34:23eedc10a4f7

Parent:

33:a03bb006dff4

Child:

35:27418c5fefb9

--- a/THE.cpp	Fri Nov 02 15:29:56 2018 +0000
+++ b/THE.cpp	Fri Nov 02 18:03:52 2018 +0000
@@ -1,12 +1,12 @@
-#include "mbed.h" // Use revision 119!!
-#include "QEI.h" // For reading the encoder of the motors 
-#include <ctime> // for the timer during the process (if needed)
-#include "Servo.h" // For controlling the servo
+#include "mbed.h" 
+#include "QEI.h" 
+#include <ctime> 
+#include "Servo.h" 
 #include "BiQuad.h"
 
 #define SERIAL_BAUD 115200
 
-// In- en outputs 
+// In- and outputs 
 // -----------------------------------------------------------------------------
 
 // EMG
@@ -14,17 +14,17 @@
 AnalogIn emg1_in( A1 ); // y_direction
 AnalogIn emg2_in( A2 ); // changing directions
 
-// Motor related
+// Motor 
 DigitalOut dirpin_1(D4); // direction of motor 1 (translation)
 PwmOut pwmpin_1(D5); // PWM pin of motor 1
 DigitalOut dirpin_2(D7); // direction of motor 2 (rotation)
 PwmOut pwmpin_2(D6); // PWM pin of motor 2
 
-// Extra stuff
-DigitalIn button_motorcal(SW2); // button for motor calibration, on mbed
-DigitalIn button_emergency(D8); // button for emergency mode, on bioshield
-DigitalIn button_wait(SW3); // button for wait mode, on mbed 
-DigitalIn button_demo(D9); // button for demo mode, on bioshield  
+// Extra
+DigitalIn button_motorcal(SW2); // mbed
+DigitalIn button_emergency(D8); // bioshield
+DigitalIn button_wait(SW3); // mbed 
+DigitalIn button_demo(D9); // bioshield  
 
 DigitalOut led_red(LED_RED); // red led 
 DigitalOut led_green(LED_GREEN); // green led
@@ -32,9 +32,8 @@
 
 Servo myservo(D3); // Define the servo to control (in penholder), up to start with
 
-// Other stuff
+// Other
 // -----------------------------------------------------------------------------
-// Define stuff like tickers etc
 
 Ticker process_tick;
 Ticker emergency;
@@ -53,7 +52,6 @@
 volatile int counts1;
 volatile int counts2;
 
-// EMG related
 // Constants EMG filter
 const double m1 = 0.5000;
 const double m2 = -0.8090;
@@ -105,38 +103,38 @@
 const float T = 0.001f;
 
 // EMG
-const int sizeMovAg = 100; //Size of array over which the moving average (MovAg) is calculated
-double sum, sum1, sum2, sum3; //Variables used in calibration and MovAg to sum the elements in the array
+const int sizeMovAg = 100; // Size of array over which the moving average (MovAg) is calculated
+double sum, sum1, sum2, sum3; // Variables used in calibration and MovAg to sum the elements in the array
 double StoreArray0[sizeMovAg] = {}, StoreArray1[sizeMovAg] = {}, StoreArray2[sizeMovAg] = {};
 
-//Empty arrays to calculate MovAgs
-double Average0, Average1, Average2; //Outcome of MovAg
-const int sizeCali = 500; //Size of array over which the Threshold will be calculated
+// Empty arrays to calculate MovAgs
+double Average0, Average1, Average2; // Outcome of MovAg
+const int sizeCali = 500; // Size of array over which the Threshold will be calculated
 double StoreCali0[sizeCali] = {}, StoreCali1[sizeCali] = {}, StoreCali2[sizeCali] = {};
 
-//Empty arrays to calculate means in calibration
-double Mean0, Mean1, Mean2; //Mean of maximum contraction, calculated in the calibration
-double Threshold0 = 1, Threshold1 = 1, Threshold2 = 1; //Thresholds for muscles 0 to 2
-int g = 0; //Part of the switch void, where the current state can be changed
-int emg_calib=0; //After calibration this value will be 1, enabling the
+// Empty arrays to calculate means in calibration
+double Mean0, Mean1, Mean2; // Mean of maximum contraction, calculated in the calibration
+double Threshold0 = 1, Threshold1 = 1, Threshold2 = 1; // Thresholds for muscles 0 to 2
+int g = 0; // Part of the switch, where the current state of calibration can be changed
+int emg_calib = 0; // After calibration this value will be 1
 
-// MOTOR_CAL
+// Motor calibration
 volatile double tower_1_position = 0.1; // the tower which he reaches first
 volatile double tower_end_position = 0.1; // the tower which he reaches second
 volatile double rotation_start_position = 0.1; // the position where the rotation will remain
 volatile double position;
 volatile float speed = 1.0;
-volatile int dir = 0;
+volatile int dir = 0; // direction of rotation of the motors
 
 // RKI related
 const double Ts = 0.001;// sample frequency 
 
 // Constants motor
 const double delta_t = 0.01;
-const double el_1 = 370.0 / 2.0;
-const double el_2 = 65.0 / 2.0;
+const double el_1 = 370.0 / 2.0; // radius of inner circle 
+const double el_2 = 65.0 / 2.0; // half length of pen holder 
 const double pi = 3.14159265359;
-const double alpha = (2.0 * pi) /(25.0*8400.0);
+const double alpha = (2.0 * pi) /(25.0*8400.0); 
 const double beta = (((2.0 * el_1) - (2.0 * el_2)) * 20.0 * pi) / (305.0 * 8400.0);
 const double q1start = rotation_start_position * alpha;
 const double q2start = tower_1_position * beta; 
@@ -181,7 +179,7 @@
 states CurrentState = WAIT; // the CurrentState to start with is the WAIT state
 bool StateChanged = true; // the state must be changed to go into the next state
 
-enum direction {Pos_RB, Pos_LB, Pos_RO, Pos_LO};
+enum direction {Pos_RB, Pos_LB, Pos_RO, Pos_LO}; // states for the direction control during OPERATING mode 
 direction currentdirection = Pos_RB;
 bool directionchanged = true;
 
@@ -282,7 +280,7 @@
     Average2 = sum3/sizeMovAg; 
     }
 
-// This must be applied to all emg signals coming in
+// This must be applied to all EMG signals coming in
 void processing_emg()
 {
     filtering();
@@ -291,8 +289,7 @@
 
 // MOTOR_CAL
 // To calibrate the motor angle to some mechanical boundaries
-// Kenneth mee bezig 
-void pos_store(int a){ //store position in counts to know count location of the ends of bridge
+void pos_store(int a){ // store position in counts to know count location of the ends of the rail
 
     if (tower_1_position == 0.1)
     {
@@ -407,58 +404,58 @@
     // Go back to the initial values
     led_red = 1;
     led_blue = 1;
-    led_green = 1;
-     
-    // All pwm's to zero
+    led_green = 1;     
     translation_stop();
     rotation_stop();
-    // All counts to zero
     counts1 = 0;
     counts2 = 0;
-    // EMG calibration to zero
     g = 0;
     dir = 0;
     }
 
 // EMG_CAL
 // To calibrate the EMG signal to some boundary values
-// Void to switch between signals to calibrate
+// Function to switch between signals to calibrate
 void switch_to_calibrate() 
 {
     g++;
+    
     //If g = 0, led is blue
     if (g == 0) 
     { 
-        led_blue=0;
-        led_red=1;
-        led_green=1;
+        led_blue = 0;
+        led_red = 1;
+        led_green = 1;
         } 
+        
     //If g = 1, led is red
     else if(g == 1) 
     { 
-        led_blue=1;
-        led_red=0;
-        led_green=1;
+        led_blue = 1;
+        led_red = 0;
+        led_green = 1;
         } 
+        
     //If g = 2, led is green
     else if(g == 2) 
     { 
-        led_blue=1;
-        led_red=1;
-        led_green=0;
-        } 
+        led_blue = 1;
+        led_red = 1;
+        led_green = 0;
+        }
+        
     //If g > 3, led is white
     else 
     { 
-        led_blue=0;
-        led_red=0;
-        led_green=0;
+        led_blue = 0;
+        led_red = 0;
+        led_green = 0;
         emg_calib = 0;
         g = 0;
         }
     }
 
-// Void to calibrate the signals, depends on value g. While calibrating, maximal contraction is required
+//  Function to calibrate the signals, depends on value g. While calibrating, maximal contraction is required
 void calibrate() 
 {
     switch (g) 
@@ -468,11 +465,11 @@
             sum = 0.0;
             
             //For statement to make an array of the latest datapoints of the filtered signal
-            for (int j = 0; j<=sizeCali-1; j++) 
+            for (int j = 0; j <= sizeCali - 1; j++) 
             {
                 
                 StoreCali0[j] = low0; // Stores the latest datapoint in the first element of the array
-                sum+=StoreCali0[j]; // Sums the elements in the array
+                sum+ = StoreCali0[j]; // Sums the elements in the array
                 wait(0.001f);
                 }
             Mean0 = sum/sizeCali; // Calculates the mean of the signal
@@ -482,10 +479,10 @@
         case 1: 
         { // Case one, calibrate EMG signal of left biceps
             sum = 0.0;
-            for(int j=0; j<=sizeCali-1; j++) 
+            for(int j = 0; j <= sizeCali - 1; j++) 
             {
                 StoreCali1[j] = low1;
-                sum+=StoreCali1[j];
+                sum+ = StoreCali1[j];
                 wait(0.001f);
                 }
             Mean1 = sum/sizeCali;
@@ -495,10 +492,10 @@
         case 2: 
         { // Case two, calibrate EMG signal of calf
             sum = 0.0;
-            for(int j=0; j<=sizeCali-1; j++) 
+            for(int j = 0; j <= sizeCali - 1; j++) 
             {
                 StoreCali2[j] = low2;
-                sum+=StoreCali2[j];
+                sum+ = StoreCali2[j];
                 wait(0.001f);
                 }   
             Mean2 = sum/sizeCali;
@@ -507,7 +504,7 @@
             }
         case 3: 
         { // Sets calibration value to 1; robot can be set to Home position
-            emg_calib=1;
+            emg_calib = 1;
             wait(0.001f);
             break;
             }
@@ -518,7 +515,7 @@
         }
     }
 
-// Void to calibrate the EMG signal
+// Function to calibrate the EMG signal
 void emg_calibration()
 {
     for(int m = 1; m <= 4; m++)
@@ -566,7 +563,7 @@
 void start_mode() 
 {
     // move to middle, only motor1 has to do something, the other one you can move yourself during the calibration
-    int a = tower_end_position - ((tower_end_position - tower_1_position)/2);
+    int a = tower_end_position - ((tower_end_position - tower_1_position) / 2);
     pc.printf("position middle = %i, position pen = %i \r\n", a, Counts1(counts1));
     
     //translation home
@@ -673,7 +670,6 @@
     
 // PID controller
 // To control the input signal before it goes into the motor control
-// PID execution
 double PID_control(volatile double error, const double kp, const double ki, const double kd, volatile double &error_int, volatile double &error_prev)
 { 
     // P control
@@ -691,13 +687,14 @@
     return u_k + u_i + u_d;
     }
 
+// Function to make boundaries for the motors and limit the motion
 void boundaries()
 {
     double q2tot = q2 + dq2;
     if (q2tot > q2end)
     {
         dq2 = 0;
-        } //kan ook zeggen q2end-q2 is dat dan juiste waarde of moet q2-q2end?
+        } 
     else if (q2tot < q2start)
     {
         dq2 = 0;
@@ -706,7 +703,7 @@
 
 void motor_control()
 {
-    // servocontrol(); // make sure the servo is used in this mode, maybe attach to a ticker?
+    // servocontrol(); 
     
     // filtering emg
     
@@ -728,32 +725,29 @@
         desired_y = 0.0;
         }
         
-    // calling functions
-        
-    
     // motor control
-    out1 = desired_x; //control x-direction
-    out2 = desired_y; //control y-direction
+    out1 = desired_x; // control x-direction
+    out2 = desired_y; // control y-direction
     Directioncontrol();
-    vdesx = out1 * 20.0; //speed x-direction
-    vdesy = out2 * 20.0; //speed y-direction
+    vdesx = out1 * 20.0; // speed x-direction
+    vdesy = out2 * 20.0; // speed y-direction
 
-    q1 = Counts2(counts2) * alpha + q1start; //counts to rotation (rad) 
-    q2 = Counts1(counts1)* beta + q2start; //counts to translation (mm)
-    MPe = el_1 - el_2 + q2; //x location end effector, x-axis along the translation
-    xe = cos(q1) * MPe; //x location in frame 0
-    ye = sin(q1) * MPe; //y location in frame 0
-    gamma = 1.0 /((-1.0 * ye * sin(q1)) - (xe * cos(q1))); //(1 / det(J'')inverse)
-    dq1 = gamma * delta_t * (sin(q1) * vdesx - cos(q1) * vdesy); //target rotation
-    dq2 = gamma * delta_t * (xe * vdesx + ye * vdesy) * -1.0; //target translation
-    //boundaries();
-    dC1 = PID_control( dq1, Rot_Kp, Rot_Ki, Rot_Kd, Rot_error, Rot_prev_error) / alpha; //target rotation to counts
-    dC2 = PID_control( dq2, Trans_Kp, Trans_Ki, Trans_Kd, Trans_error, Trans_prev_error) / beta; //target translation to counts
-    pwm1 = 3.0 * (dC1 / delta_t) / 8400.0; //
-    pwm2 = 7.0 * (dC2 / delta_t) / 8400.0; //
-    dirpin_1.write(pwm2 < 0); // translatie                 
+    q1 = Counts2(counts2) * alpha + q1start; // counts to rotation (rad) 
+    q2 = Counts1(counts1)* beta + q2start; // counts to translation (mm)
+    MPe = el_1 - el_2 + q2; // x location end effector, x-axis along the translation
+    xe = cos(q1) * MPe; // x location in frame 0
+    ye = sin(q1) * MPe; // y location in frame 0
+    gamma = 1.0 /((-1.0 * ye * sin(q1)) - (xe * cos(q1))); // (1 / det(J'')inverse)
+    dq1 = gamma * delta_t * (sin(q1) * vdesx - cos(q1) * vdesy); // target rotation
+    dq2 = gamma * delta_t * (xe * vdesx + ye * vdesy) * -1.0; // target translation
+    //boundaries(); // boundaries can be implemented here
+    dC1 = PID_control( dq1, Rot_Kp, Rot_Ki, Rot_Kd, Rot_error, Rot_prev_error) / alpha; // target rotation to counts
+    dC2 = PID_control( dq2, Trans_Kp, Trans_Ki, Trans_Kd, Trans_error, Trans_prev_error) / beta; // target translation to counts
+    pwm1 = 3.0 * (dC1 / delta_t) / 8400.0; 
+    pwm2 = 7.0 * (dC2 / delta_t) / 8400.0; 
+    dirpin_1.write(pwm2 < 0); // translation
     pwmpin_1 = fabs (pwm2);                    
-    dirpin_2.write(pwm1 < 0); // rotatie
+    dirpin_2.write(pwm1 < 0); // rotation
     pwmpin_2 = fabs (pwm1);
     }   
 
@@ -763,7 +757,7 @@
 {
     if (button_emergency == false) // condition for MOTOR_CAL --> FAILURE; button_emergency press 
     {    
-        led_red =0; // turning red led on to show emergency mode 
+        led_red = 0; // turning red led on to show emergency mode 
         led_blue = 1;
         led_green = 1;