Diff: main.cpp

Revision:

26:fe3a5469dd6b

Parent:

25:49ccdc98639a

Child:

27:0cefe83f83d3

--- a/main.cpp	Fri Oct 16 13:46:39 2015 +0000
+++ b/main.cpp	Sun Oct 18 13:13:07 2015 +0000
@@ -4,6 +4,7 @@
 #include "biquadFilter.h"
 #include "QEI.h"
 #include "math.h"
+#include <string> 
 
 /*--------------------------------------------------------------------------------------------------------------------
 -------------------------------- BIOROBOTICS GROUP 14 ----------------------------------------------------------------
@@ -15,14 +16,12 @@
 #define     CONTROL_RATE    0.01    //100 Hz Control rate
 #define     ENCODER1_CPR    4200    //encoders have 64 (X4), 32 (X2) counts per revolution of motor shaft
 #define     ENCODER2_CPR    4200    //gearbox 1:131.25 ->  4200 counts per revolution of the output shaft (X2), 
-
+#define     PWM_PERIOD      0.0001  //10k Hz pwm motor frequency. Higher -> too hot, lower -> motor doesnt respond correctly
 /*--------------------------------------------------------------------------------------------------------------------
 ---- OBJECTS ---------------------------------------------------------------------------------------------------------
 --------------------------------------------------------------------------------------------------------------------*/
 
 MODSERIAL pc(USBTX,USBRX);      //serial communication
-DigitalIn button(PTA1);         //buttons
-DigitalIn button1(PTB9);        //
 
 //Debug legs
 DigitalOut red(LED_RED);
@@ -49,33 +48,56 @@
 //Speed and Direction of motors - D4 (dir) and D5(speed) = motor 2, D7(dir) and D6(speed) = motor 1
 PwmOut pwm_motor1(D6);          //PWM motor 1
 PwmOut pwm_motor2(D5);          //PWM motor 2
+
 DigitalOut dir_motor1(D7);      //Direction motor 1
 DigitalOut dir_motor2(D4);      //Direction motor 2
 
 //Limit Switches
-DigitalIn shoulder_limit(PTA4);  //using FRDM buttons 
-DigitalIn elbow_limit(PTC6);     //using FRDM buttons
+InterruptIn shoulder_limit(D3);  //using FRDM buttons 
+InterruptIn elbow_limit(D4);     //using FRDM buttons
+
+//Other buttons
+DigitalIn button1(PTA4);        //using FRDM buttons 
+DigitalIn button2(PTC6);        //using FRDM buttons
+
+/*Text colors ASCII code: Set Attribute Mode    <ESC>[{attr1};...;{attrn}m
+
+\ 0 3 3  - ESC 
+[ 3 0 m  - black
+[ 3 1 m  - red
+[ 3 2 m  - green
+[ 3 3 m  - yellow
+[ 3 4 m  - blue
+[ 3 5 m  - magenta
+[ 3 6 m  - cyan
+[ 3 7 m  - white 
+[ 0 m    - reset attributes
+
+Put the text you want to color between GREEN_ and _GREEN
+*/
+string GREEN_ = "\033[32m";    //esc - green
+string _GREEN = "\033[0m";     //esc - reset 
 
 
 /*--------------------------------------------------------------------------------------------------------------------
 ---- DECLARE VARIABLES -----------------------------------------------------------------------------------------------
 --------------------------------------------------------------------------------------------------------------------*/
 
-//EMG variables
+//EMG variables: raw EMG - filtered EMG - maximum voluntary contraction
 double emg_biceps; double biceps_power; double bicepsMVC = 0;
 double emg_triceps; double triceps_power; double tricepsMVC = 0;
 double emg_flexor; double flexor_power; double flexorMVC = 0;
 double emg_extens; double extens_power; double extensMVC = 0;
 
-int muscle; 
-double calibrate_time;
+int muscle;             //Muscle selector for MVC measurement
+double calibrate_time;  //Elapsed time for each MVC measurement
 
 //PID variables
 double u1; double u2;                                               //Output of PID controller (PWM value for motor 1 and 2)
 double m1_error=0; double m1_e_int=0; double m1_e_prev=0;           //Error, integrated error, previous error
 const double m1_kp=0; const double m1_ki=0; const double m1_kd=0;   //Proportional, integral and derivative gains.
 
-double m2_error=0; double m2_e_int=0; double m2_e_prev=0;                 //Error, integrated error, previous error
+double m2_error=0; double m2_e_int=0; double m2_e_prev=0;           //Error, integrated error, previous error
 const double m2_kp=0; const double m2_ki=0; const double m2_kd=0;   //Proportional, integral and derivative gains.
 
 //highpass filter 20 Hz
@@ -119,7 +141,7 @@
 const double notch2_a1 = -1.61244708381 * notch2gain;
 const double notch2_a2 = 0.97415116257 * notch2gain;
  
-//lowpass filter 7 Hz  - envelop
+//lowpass filter 7 Hz  - envelope
 const double low_b0 = 0.000119046743110057;
 const double low_b1 = 0.000238093486220118;
 const double low_b2 = 0.000119046743110057;
@@ -127,52 +149,64 @@
 const double low_a2 = 0.9693784555043481;
 
 
-
 /*--------------------------------------------------------------------------------------------------------------------
 ---- Filters ---------------------------------------------------------------------------------------------------------
 --------------------------------------------------------------------------------------------------------------------*/
 
 //Using biquadFilter library
 //Syntax: biquadFilter     filter(a1, a2, b0, b1, b2); coefficients. Call with: filter.step(u), with u signal to be filtered.
+//Biceps
 biquadFilter     highpass( high_a1 , high_a2 , high_b0 , high_b1 , high_b2 );               // removes DC and movement artefacts
 biquadFilter     notch1( notch1_a1 , notch1_a2 , notch1_b0 , notch1_b1 , notch1_b2 );       // removes 49-51 Hz power interference
 biquadFilter     notch2( notch2_a1 , notch2_a2 , notch2_b0 , notch2_b1 , notch2_b2 );       //
 biquadFilter     lowpass( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 );                     // EMG envelope    
 
+//Triceps
 biquadFilter     highpass2( high_a1 , high_a2 , high_b0 , high_b1 , high_b2 );               // removes DC and movement artefacts
-biquadFilter     notch1_2( notch1_a1 , notch1_a2 , notch1_b0 , notch1_b1 , notch1_b2 );       // removes 49-51 Hz power interference
-biquadFilter     notch2_2( notch2_a1 , notch2_a2 , notch2_b0 , notch2_b1 , notch2_b2 );       //
+biquadFilter     notch1_2( notch1_a1 , notch1_a2 , notch1_b0 , notch1_b1 , notch1_b2 );      // removes 49-51 Hz power interference
+biquadFilter     notch2_2( notch2_a1 , notch2_a2 , notch2_b0 , notch2_b1 , notch2_b2 );      //
 biquadFilter     lowpass2( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 );                     // EMG envelope    
 
+//Flexor
 biquadFilter     highpass3( high_a1 , high_a2 , high_b0 , high_b1 , high_b2 );               // removes DC and movement artefacts
-biquadFilter     notch1_3( notch1_a1 , notch1_a2 , notch1_b0 , notch1_b1 , notch1_b2 );       // removes 49-51 Hz power interference
-biquadFilter     notch2_3( notch2_a1 , notch2_a2 , notch2_b0 , notch2_b1 , notch2_b2 );       //
+biquadFilter     notch1_3( notch1_a1 , notch1_a2 , notch1_b0 , notch1_b1 , notch1_b2 );      // removes 49-51 Hz power interference
+biquadFilter     notch2_3( notch2_a1 , notch2_a2 , notch2_b0 , notch2_b1 , notch2_b2 );      //
 biquadFilter     lowpass3( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 );                     // EMG envelope    
 
+//Extensor
 biquadFilter     highpass4( high_a1 , high_a2 , high_b0 , high_b1 , high_b2 );               // removes DC and movement artefacts
-biquadFilter     notch1_4( notch1_a1 , notch1_a2 , notch1_b0 , notch1_b1 , notch1_b2 );       // removes 49-51 Hz power interference
-biquadFilter     notch2_4( notch2_a1 , notch2_a2 , notch2_b0 , notch2_b1 , notch2_b2 );       //
+biquadFilter     notch1_4( notch1_a1 , notch1_a2 , notch1_b0 , notch1_b1 , notch1_b2 );      // removes 49-51 Hz power interference
+biquadFilter     notch2_4( notch2_a1 , notch2_a2 , notch2_b0 , notch2_b1 , notch2_b2 );      //
 biquadFilter     lowpass4( low_a1 , low_a2 , low_b0 , low_b1 , low_b2 );                     // EMG envelope    
 
+//PID filter (lowpass ??? Hz)
 biquadFilter     derfilter( 0.0009446914586925257 , 0.0018893829173850514 , 0.0009446914586925257 , -1.911196288237583 , 0.914975054072353 );   // derivative filter 
 
 /*--------------------------------------------------------------------------------------------------------------------
 ---- DECLARE FUNCTION NAMES ------------------------------------------------------------------------------------------
 --------------------------------------------------------------------------------------------------------------------*/
+
+void sample_filter(void);   //Sampling and filtering
+void control();             //Control - reference -> error -> pid -> motor output
+void calibrate_emg();       //Instructions + measurement of MVC of each muscle 
+void emg_mvc();             //Helper funcion for storing MVC value
+void calibrate_arm(void);   //Calibration of the arm with limit switches
+void start_sampling(void);  //Attaches the sample_filter function to a 500Hz ticker
+void stop_sampling(void);   //Stops sample_filter
+void start_control(void);   //Attaches the control function to a 100Hz ticker
+void stop_control(void);    //Stops control function
+
+//Keeps the input between min and max value
 void keep_in_range(double * in, double min, double max);
-void sample_filter(void);
-void control();
-void calibrate_emg();
-void emg_mvc();
-void calibrate_arm(void);
-void start_sampling(void);
-void stop_sampling(void);
-void start_control(void);
-void stop_control(void);
+
+//Reusable PID controller, previous and integral error need to be passed by reference
 double pid(double error, double kp, double ki, double kd,double &e_int, double &e_prev);
 
+//Menu functions
 void mainMenu();
 void caliMenu();
+void clearTerminal();
+
 
 /*--------------------------------------------------------------------------------------------------------------------
 ---- MAIN LOOP -------------------------------------------------------------------------------------------------------
@@ -180,10 +214,17 @@
 
 int main()
 {
-    pc.baud(115200);
-    red=1; green=1; blue=1;
+    pc.baud(115200);            //terminal baudrate
+    red=1; green=1; blue=1;     //Make sure debug LEDS are off  
+    
+    //Set PwmOut frequency to 10k Hz
+    pwm_motor1.period(PWM_PERIOD);    
+    pwm_motor2.period(PWM_PERIOD);
+    
+    clearTerminal();            //Clear the putty window
+    
     // make a menu, user has to initiate next step
-    //mainMenu();
+    mainMenu();
     //caliMenu();            // Menu function
     //calibrate_arm();        //Start Calibration
     
@@ -244,31 +285,35 @@
 //Send arm to mechanical limits, and set encoder to 0. Then send arm to starting position.
 void calibrate_arm(void)
 {
-    //const double hit = 0.5;
-    dir_motor1=1;   //ccw
-    dir_motor2=1;   //ccw
-    pwm_motor1.write(0.2f);     //move upper arm slowly ccw
-    pwm_motor2.write(0.2f);     //move forearm slowly ccw
+    red=0; blue=0;              //Debug light is purple during arm calibration
+    bool calibrating = true;
+    bool done1 = false;
+    bool done2 = false;
+    dir_motor1=1;   //cw
+    dir_motor2=1;   //cw
+    pwm_motor1.write(0.2f);     //move upper arm slowly cw
+    pwm_motor2.write(0.2f);     //move forearm slowly cw
+    
+    while(calibrating){
     
-    if(shoulder_limit.read() < 0.5){   //when limit switches are hit, stop motor and reset encoder
-        pwm_motor1.write(0);
-        Encoder1.reset();
-    }
-    if(elbow_limit.read() < 0.5){
-        pwm_motor2.write(0);
-        Encoder2.reset();
-    }    
+        //when limit switches are hit, stop motor and reset encoder
+        if(shoulder_limit.read() < 0.5){   //polling 
+            pwm_motor1.write(0);
+            Encoder1.reset();
+            done1 = true;
+        }
+        if(elbow_limit.read() < 0.5){     //polling
+            pwm_motor2.write(0);
+            Encoder2.reset();
+            done2 = true;
+        }    
+        if(done1 && done2){
+            calibrating=false;      //Leave while loop when both limits are reached
+            red=1; blue=1;          //Turn debug light off when calibration complete
+        }
     
-   /* while(shoulder_limit != hit){
-        pwm_motor1.write(0.4);
-    }
-    Encoder1.reset();
-    
-    while(elbow_limit != hit){
-        pwm_motor2.write(0.4);
-    }
-    Encoder2.reset();
-    */
+    }//end while
+
 }
 
 //EMG Maximum Voluntary Contraction measurement
@@ -284,7 +329,8 @@
         if(muscle==1){
             
             if(biceps_power>bicepsMVC){
-            printf("+ ");
+            //printf("+ ");
+            printf("%s+ %s",GREEN_,_GREEN);
             bicepsMVC=biceps_power;
             }    
             else
@@ -294,22 +340,31 @@
         if(muscle==2){
             
             if(triceps_power>tricepsMVC){
+            printf("%s+ %s",GREEN_,_GREEN);
             tricepsMVC=triceps_power;
             }    
+            else
+            printf("- ");
         }
         
         if(muscle==3){
             
             if(flexor_power>flexorMVC){
+            printf("%s+ %s",GREEN_,_GREEN);    
             flexorMVC=flexor_power;
             }    
+            else
+            printf("- ");
         }
         
         if(muscle==4){
             
             if(extens_power>extensMVC){
+            printf("%s+ %s",GREEN_,_GREEN);    
             extensMVC=extens_power;
             }    
+            else
+            printf("- ");
         }
         
     //}
@@ -324,12 +379,11 @@
 {
    Ticker timer;
    
-   pc.printf("|-- Robot Started --| \r\n");
    pc.printf("Testcode calibration \r\n");
    wait(1);
    pc.printf("+ means current sample is higher than stored MVC\r\n");
    pc.printf("- means current sample is lower than stored MVC\r\n");
-   wait(3);
+   wait(2);
    pc.printf(" Biceps is first. "); wait(1);
    pc.printf(" Press any key to begin... "); wait(1);
    char input;
@@ -344,13 +398,15 @@
    timer.attach(&emg_mvc,0.002);
    wait(5);
    timer.detach();
-   pc.printf("\r\n MVC = %f \r\n",bicepsMVC);
-   
-   pc.printf("Calibrate_emg() exited \r\n");
-   pc.printf("Measured time: %f seconds \r\n",calibrate_time);
+  
+   pc.printf("\r\n Measurement complete."); wait(1);
+   pc.printf("\r\n Biceps MVC = %f \r\n",bicepsMVC); wait(1);
+      pc.printf("Calibrate_emg() exited \r\n"); wait(1);
+   pc.printf("Measured time: %f seconds \r\n\n",calibrate_time);
    calibrate_time=0;
    
    // Triceps:
+   muscle=2;
    pc.printf(" Triceps is next "); wait(1);
    pc.printf(" Press any key to begin... "); wait(1);
    input=pc.getc();
@@ -370,9 +426,11 @@
    calibrate_time=0;
    
    //Flexor:
-   
+   muscle=3;
    //Extensor:
+   muscle=4;
    
+   //Stop sampling, detach ticker
    stop_sampling();
    
 }
@@ -440,14 +498,37 @@
     
 }
 
-void mainMenu(){};
+void mainMenu()
+{
+   //Title Box
+   pc.putc(201); 
+   for(int j=0;j<33;j++){
+   pc.putc(205);
+   }
+   pc.putc(187); 
+   pc.printf("\n\r");
+   pc.putc(186); pc.printf("    BioRobotics M9 - Group 14    "); pc.putc(186);
+   pc.printf("\n\r");
+   pc.putc(186); pc.printf("         Robot powered ON        "); pc.putc(186);
+   pc.printf("\n\r");
+   pc.putc(200);
+   for(int k=0;k<33;k++){
+   pc.putc(205);
+   }
+   pc.putc(188); 
+   
+   pc.printf("\n\r");
+   //endbox
+}
 void caliMenu(){};
 
 //Start sampling
 void start_sampling(void)
 {
     sample_timer.attach(&sample_filter,SAMPLE_RATE);   //500 Hz EMG 
-    blue=0; green=0;
+    
+    //Debug LED will be green when sampling is active
+    green=0;    
     pc.printf("||- started sampling -|| \r\n");
 }
 
@@ -455,7 +536,9 @@
 void stop_sampling(void)
 {
     sample_timer.detach();
-    blue=1; green=1;
+    
+    //Debug LED will be turned off when sampling stops
+    green=1;
     pc.printf("||- stopped sampling -|| \r\n");
 }
 
@@ -463,12 +546,20 @@
 void start_control(void)
 {
     control_timer.attach(&control,SAMPLE_RATE);     //100 Hz control
+    
+    //Debug LED will be blue when control is on. If sampling and control are on -> blue + green = cyan.
+    blue=0;
+    pc.printf("||- started control -|| \r\n");
 }
 
 //stop control
 void stop_control(void)
 {
     control_timer.detach();
+    
+    //Debug LED will be off when control is off
+    blue=1;
+    pc.printf("||- stopped control -|| \r\n");
 }
 
 
@@ -477,6 +568,14 @@
 
 }
 
+//Clears the putty (or other terminal) window
+void clearTerminal()
+{
+   pc.putc(27); 
+   pc.printf("[2J"); // clear screen 
+   pc.putc(27); // ESC 
+   pc.printf("[H"); // cursor to home 
+}
 
 //keeps input limited between min max
 void keep_in_range(double * in, double min, double max)