Diff: main.cpp

Revision:

14:a8a69fee3fad

Parent:

12:0765ea2c4c85

Child:

15:a73b5abd0696

--- a/main.cpp	Thu Nov 02 13:23:01 2017 +0000
+++ b/main.cpp	Thu Nov 02 15:11:19 2017 +0000
@@ -1,4 +1,3 @@
-#include "MODSERIAL.h"
 #include "AnglePosition.h"
 #include "PIDControl.h"
 #include "BiQuad.h"
@@ -8,55 +7,57 @@
 #include "encoder.h"
 #include "Servo.h"
 #include "FastPWM.h"
-//#include "HIDScope.h"
 
-//This code is for Mbed 2
+//This code is for the main Mbed
 //------------------------------------------------------------------------------
 //------------------------------------------------------------------------------
 //------------------------------------------------------------------------------
-MODSERIAL pc(USBTX, USBRX);         //Establish connection
-Ticker MyControllerTicker;          //Declare Ticker for Motors
+Ticker MyTickerController;          //Declare Ticker for Motors
 Ticker MyTickerServo;               //Declare Ticker Servo control
 Ticker MyTickerMean;                //Declare Ticker Signalprocessing
+Ticker MyTickerFilter;              //Declare Ticker Filtering
 
-InterruptIn But1(PTA4);             //Declare button for min calibration
-InterruptIn Mode(PTC6);             //Declare button for max calibration
+InterruptIn But1(PTA4);             //Declare button for emg calibration
+InterruptIn Mode(PTC6);             //Declare button for mode selection
 
 AnglePosition Angle;                //Declare Angle calculater
 PIDControl PID;                     //Declare PID Controller
 SignalNumber SignalLeft;            //Declare Signal determiner for Left arm
 SignalNumber SignalRight;           //Declare Signal determiner for Right arm
-SignalNumber SignalMode;
-Movement MoveLeft;                  //Declare Movement determiner
-Movement MoveRight;
+Movement MoveLeft;                  //Declare Left Movement determiner
+Movement MoveRight;                 //Declare Right movement determiner
 
 AnalogIn    emg0( A0 );             //Set Inputpin for EMG 0 signal Left 
 AnalogIn    emg2( A2 );             //Set Inputpin for EMG 2 signal Right
-AnalogIn    emg4( A4 );             //Set Inputpin for EMG 4 signal Mode
-DigitalOut     Up( D9 );             //Set digital in for mode selection
-DigitalOut   Down( D8 );
-DigitalOut  Led_red(LED_RED);          
+DigitalOut     Up( D9 );            //Set digital out for raising the pen off the board
+DigitalOut   Down( D8 );            //Set digital out for lowering the pen off the board
+
+//Setting LEDs
+DigitalOut  Led_red(LED_RED);       
 DigitalOut  Led_green(LED_GREEN);
 DigitalOut  Led_blue(LED_BLUE);  
 DigitalOut  Led_calibration(D2);
 
 const float CONTROLLER_TS = 0.02;   //Motor frequency
-const float MEAN_TS = 0.002;        //Filter frequency
+const float MEAN_TS = 0.002;        //Moving average frequency
+const float FILTER_TS = 1e3;        //Filter frequency
+const float SERVO_TS = 0.01;        //Servo frequency
 
+void setled(){
+    Led_red=1; Led_green=1; Led_blue=1; Led_calibration=0;
+}
 //------------------------------------------------------------------------------
 //-----------------------------EMG Signals--------------------------------------
 //------------------------------------------------------------------------------
+
 // Filtering the signal and getting the usefull information out of it.
-const int n = 500;                  //Window size for the mean value, also adjust in signalnumber.cpp
-const int action =100;               //Number of same mean values to change the signalnumber
-const int m = 500;                  //Number of samples for calibration
-int CaseLeft, CaseRight, CaseMode;           //Strength of the muscles 
-float emg_offsetLeft, emg_offsetmaxLeft;     //Calibration offsets from zero to one for the left arm
-float emg_offsetRight, emg_offsetmaxRight;   //Calibration offsets from zero to one for the right arm
-float emg_offsetMode, emg_offsetmaxMode;
-float mean_value_left, mean_value_right, mean_value_mode;   //Mean value of the filtered system
-float kLeft, kRight, kMode;            //Scaling factors
-float Signal_filteredLeft, Signal_filteredRight, Signal_filteredMode;
+const int n = 500;                                  //Window size for the mean value, also adjust in signalnumber.cpp
+const int action =100;                              //Number of same mean values to change the signalnumber
+const int m = 500;                                  //Number of samples for calibration
+int SpeedLeft, SpeedRight;                            //Strength of the muscles 
+float emg_offsetLeft, emg_offsetRight;              //Calibration offsets from zero to one for the right arm
+float kLeft, kRight;                                //Scaling factors
+float Signal_filteredLeft, Signal_filteredRight;    //Variables to store the filterd signals
 
 //BiQuad filter variables
 BiQuad LP1L( 0.6389437261127493, 1.2778874522254986, 0.6389437261127493, 1.1429772843080919, 0.4127976201429053 ); //Lowpass filter Biquad
@@ -65,148 +66,108 @@
 BiQuad LP1R( 0.6389437261127493, 1.2778874522254986, 0.6389437261127493, 1.1429772843080919, 0.4127976201429053 ); //Lowpass filter Biquad
 BiQuad HP2R( 0.8370879899975344, -1.6741759799950688, 0.8370879899975344, -1.6474576182593796, 0.7008943417307579 ); //Highpass filter Biquad
 BiQuad NO3R( 0.7063988100714527, -1.1429772843080923, 0.7063988100714527, -1.1429772843080923,  0.41279762014290533); //Notch filter Biquad 
-BiQuad LP1M( 0.6389437261127493, 1.2778874522254986, 0.6389437261127493, 1.1429772843080919, 0.4127976201429053 ); //Lowpass filter Biquad
-BiQuad HP2M( 0.8370879899975344, -1.6741759799950688, 0.8370879899975344, -1.6474576182593796, 0.7008943417307579 ); //Highpass filter Biquad
-BiQuad NO3M( 0.7063988100714527, -1.1429772843080923, 0.7063988100714527, -1.1429772843080923,  0.41279762014290533); //Notch filter Biquad 
 BiQuadChain BiQuad_filter_Left;
 BiQuadChain BiQuad_filter_Right;
-BiQuadChain BiQuad_filter_Mode;
 
+//Filters-----------------------------------------------------------------------
+
+// Filter for the left signal
 float FilterLeft(float input){
     float Signal_filtered= BiQuad_filter_Left.step(input);
     return Signal_filtered;
 }
+// Filter for the right signal
 float FilterRight(float input){
     float Signal_filtered= BiQuad_filter_Right.step(input);
     return Signal_filtered;
 }
-float FilterMode(float input){
-    float Signal_filtered= BiQuad_filter_Mode.step(input);
-    return Signal_filtered;
-}
 
 //Calibration-------------------------------------------------------------------
-void setled(){
-    Led_red=1; Led_green=1; Led_blue=1; Led_calibration=0;
+
+// Calibrating the EMG signals. This function needs to be ativated when the 
+// muscles are flexed for over a second
+void calibration(){
+    emg_offsetLeft = SignalLeft.calibrate(m, FilterLeft(emg0));
+    emg_offsetRight = SignalRight.calibrate(m, FilterRight(emg2));
+    //Setting the emg scaling constants to reed between zero and one.
+    kLeft = 1/emg_offsetLeft;
+    kRight = 1/emg_offsetRight;
+    //Set led to Green to indicate if calibration is done
+    Led_calibration = 1;
 }
 
-// One calibration
-void maxcalibration(){
-    pc.printf("start max calibration \r\n");
-    emg_offsetmaxLeft = SignalLeft.calibrate(m, FilterLeft(emg0));
-    emg_offsetmaxRight = SignalRight.calibrate(m, FilterRight(emg2));
-    emg_offsetmaxMode = SignalMode.calibrate(m, FilterMode(emg4));
-    kLeft = 1/emg_offsetmaxLeft;
-    kRight = 1/emg_offsetmaxRight;
-    kMode = 1/emg_offsetmaxMode;
-    Led_calibration = 1;   //Set led to Green
+//Speedcontrol------------------------------------------------------------------
+//Determine the signalnumbers (i.e. speed) based on the strenght of the EMG signals
+void Speedcontrol(){
+    //Left Signals
+    Signal_filteredLeft = fabs(FilterLeft(emg0)*kLeft);//*kLeft
+    SpeedLeft = SignalLeft.getspeedLeft(n, action, Signal_filteredLeft);
+    //Right Signals
+    Signal_filteredRight = fabs(FilterRight(emg2)*kRight);//*kRight
+    SpeedRight = SignalRight.getspeedRight(n, action, Signal_filteredRight);
 }
 
 //------------------------------------------------------------------------------
 //---------------------------------Servo----------------------------------------
 //------------------------------------------------------------------------------
+//Sending a pulse to the other Mbed to raise the pen
 void up(){
-    Up = 1;
-    Up = 0;
-    Up = 1;
+    Up = 1; Up = 0; Up = 1;
 }
+//Sending a pulse to the other Mbed to lower the pen
 void down(){
-    Down = 1;
-    Down = 0;
-    Down = 1;
+    Down = 1; Down = 0; Down = 1;
 }
 
+//Setting the begin mode to zero
 int mode =0;
+// Enable the servo function when mode 3 or mode 6 is activated.
+// If the mean value for the Left arm stays long enough high the servo will raise
+// If the mean value for the Right arm stays long enough high the servo will lower 
 void servo(){
      if(mode==3||mode==6){   
-        Signal_filteredLeft = fabs(FilterLeft(emg0)*kLeft);//*kLeft
-        CaseLeft = SignalLeft.getnumberLeft(n, action, Signal_filteredLeft);
-        Signal_filteredRight = fabs(FilterRight(emg2)*kRight);//*kRight
-        CaseRight = SignalRight.getnumberRight(n, action, Signal_filteredRight);
-        if (CaseLeft == 2){
+        Signal_filteredLeft = fabs(FilterLeft(emg0)*kLeft);
+        Signal_filteredRight = fabs(FilterRight(emg2)*kRight);
+        SpeedLeft = SignalLeft.getspeedLeft(n, action, Signal_filteredLeft);
+        SpeedRight = SignalRight.getspeedRight(n, action, Signal_filteredRight);
+        if (SpeedLeft == 2){
             up();
         }
-        else if (CaseRight == 2){
+        if (SpeedRight == 2){
             down();
         }
     }
 }
-
-
 //------------------------------------------------------------------------------
 //---------------------------Mode selection-------------------------------------
 //------------------------------------------------------------------------------
 
-//Recieving mode selection from EMG mode signal
+//Function is called when the mode button is pressed and switches modes and led indication 
 void mode_selection(){
-    if(mode ==6){
+    //Switching to the next mode. (Total of 6 modes)
+    if(mode == 6){
         mode=1;
     }
     else{
         mode++;
-    }    
-    pc.printf("\r\n mode = %i \r\n", mode); 
-    
-    
+    }
+    //Setting the LED colour to indicate the operating mode    
     if((mode==3) || (mode==6)) {
-        Led_red = 0;
-        Led_green = 0;
-        Led_blue = 0;
+        Led_red = 0, Led_green = 0, Led_blue = 0;   //Set Led Mode to White
     }
     else if (mode==1) {
-        Led_red = 0;
-        Led_green = 1;
-        Led_blue = 1;
+        Led_red = 0, Led_green = 1, Led_blue = 1;   //Set Led Mode to Red
     }
     else if (mode==2) {
-        Led_red = 1;
-        Led_green = 0;
-        Led_blue = 1;
+        Led_red = 1, Led_green = 0, Led_blue = 1;   //Set Led Mode to Green
     }
     else if (mode==4) {
-        Led_red = 1;
-        Led_green = 1;
-        Led_blue = 0;
+        Led_red = 1, Led_green = 1, Led_blue = 0;   //Set Led Mode to Blue
         }
     else if (mode==5) {
-        Led_red = 0;
-        Led_green = 0;
-        Led_blue = 1;
+        Led_red = 0, Led_green = 0, Led_blue = 1;   //Set Led Mode to Yellow
     }
-    
 }
-
-// Control mode selection-------------------------------------------------------
-
-//Determine the signalnumbers (i.e. speed) based on the strenght of the EMG signals
-void signalnumber(){
-    //Left
-    Signal_filteredLeft = fabs(FilterLeft(emg0)*kLeft);//*kLeft
-//    mean_value_left = SignalLeft.getmeanLeft(n, Signal_filteredLeft);  
-    mean_value_left = SignalLeft.getmeanLeft(n, fabs(FilterLeft(emg0.read())*kLeft));        
-    CaseLeft = SignalLeft.getnumberLeft(n, action, Signal_filteredLeft);
-    //Right
-    Signal_filteredRight = fabs(FilterRight(emg2)*kRight);//*kRight
-//    mean_value_right = SignalRight.getmeanRight(n, Signal_filteredRight);
-    mean_value_right = SignalRight.getmeanRight(n, fabs(FilterRight(emg2.read())*kRight)); 
-    CaseRight = SignalRight.getnumberRight(n, action, Signal_filteredRight);
-    //Mode
-    /*
-    Signal_filteredMode = fabs(FilterMode(emg4));//*kMode
-    mean_value_mode = SignalMode.getmeanMode(n, Signal_filteredMode);
-    CaseMode = SignalMode.getnumberMode(n, action, Signal_filteredMode);
-    if(CaseMode >= 3){
-        milli ++;
-        if(milli>=150){
-            mode_selection();
-            milli=0;
-        }
-    }
-    else{
-        milli=0;
-    }*/
-}
-
 //------------------------------------------------------------------------------
 //-------------------------Kinematic Constants----------------------------------
 //------------------------------------------------------------------------------
@@ -239,7 +200,6 @@
     float position_alpha = RAD_PER_PULSE * encoder1.getPosition();
     float error_alpha = reference_alpha-position_alpha;
     float magnitude1 = PID.get(error_alpha, MOTOR1_KP, MOTOR1_KI, MOTOR1_KD, CONTROLLER_TS, M1_N, M1_v1, M1_v2) / motor1_gain;
-            
     //Determine Motor Magnitude
     if (fabs(magnitude1)>1) {
         motor1 = 1;
@@ -247,8 +207,6 @@
     else {
         motor1 = fabs(magnitude1);
     }
-    
-    
     // Determine Motor Direction 
     if (magnitude1  < 0){
         motor1DirectionPin = 1;
@@ -257,7 +215,6 @@
         motor1DirectionPin = 0;
     }
 }
-
 //------------------------------------------------------------------------------
 //--------------------------------Motor2----------------------------------------
 //------------------------------------------------------------------------------
@@ -278,7 +235,6 @@
     float position_beta = RAD_PER_PULSE * -encoder2.getPosition();
     float error_beta = reference_beta-position_beta;
     float magnitude2 = PID.get(error_beta, MOTOR2_KP, MOTOR2_KI, MOTOR2_KD, CONTROLLER_TS, M2_N, M1_v1, M1_v2) / motor2_gain;
-        
     //Determine Motor Magnitude
     if (fabs(magnitude2)>1) {
         motor2 = 1;
@@ -286,8 +242,6 @@
     else {
         motor2 = fabs(magnitude2);
     }
-    
-    
     //Determine Motor Direction
     if (magnitude2 > 0){
         motor2DirectionPin = 1;
@@ -296,67 +250,55 @@
         motor2DirectionPin = 0;
     }
 }
-
 //------------------------------------------------------------------------------
 //----------------------------Motor controller----------------------------------
 //------------------------------------------------------------------------------
-int direction_l, direction_r;
-float magnitude1, magnitude2;
+int direction_l, direction_r;           //Variables to store the directions
+float magnitude1, magnitude2;           //Variables to store the magnitude signal
 
 void motor_control(){
     direction_l = MoveLeft.getdirectionLeft(mode);                              //x-direction
     direction_r = MoveRight.getdirectionRight(mode);                            //y-direction
-    position_math_l= MoveLeft.getpositionLeft(CaseLeft, mode, max_rangex);      //x-direction
-    position_math_r= MoveRight.getpositionRight(CaseRight, mode, max_rangey);   //y-direction
-    motor1_control(); //magnitude1 = 
-    motor2_control(); // magnitude2 = 
-  
+    position_math_l= MoveLeft.getpositionLeft(SpeedLeft, mode, max_rangex);      //x-direction
+    position_math_r= MoveRight.getpositionRight(SpeedRight, mode, max_rangey);   //y-direction
+    motor1_control(); 
+    motor2_control();
 }
 //------------------------------------------------------------------------------
-//-------------------------------HID Scope--------------------------------------
+//-----------------------------Filter Update------------------------------------
 //------------------------------------------------------------------------------
 
-//HIDScope scope(4);
-//Ticker scopeTimer;
-Ticker FilterTimer;
-
+//Updating Filter values
 void FilterUpdate(){
     FilterLeft(emg0.read());
     FilterRight(emg2.read());
     SignalLeft.getmeanLeft(n, fabs(FilterLeft(emg0.read())));
     SignalRight.getmeanRight(n, fabs(FilterRight(emg2.read())));    
 }
-
-//Working
-//SignalLeft.getmeanLeft(n, fabs(FilterLeft(emg0.read())))
-//SignalRight.getmeanRight(n, fabs(FilterRight(emg2.read())))
-
 //------------------------------------------------------------------------------
 //------------------------------Main--------------------------------------------
 //------------------------------------------------------------------------------
 
 int main(){
-    pc.baud(115200);
     setled(); 
+    
+    //Creating Filters
     BiQuad_filter_Left.add( &LP1L ).add( &HP2L ).add( &NO3L);
     BiQuad_filter_Right.add( &LP1R ).add( &HP2R ).add( &NO3R);
-    BiQuad_filter_Mode.add( &LP1M ).add( &HP2M ).add( &NO3M);
-    But1.rise(&maxcalibration);
+    
+    //Button detection
+    But1.rise(&calibration);
     Mode.rise(&mode_selection);
+    
+    //Setting motor periods
     motor1.period(0.0001f); motor2.period(0.0001f);
-    MyControllerTicker.attach(&motor_control, CONTROLLER_TS); //Determining motorposiitons
-    MyTickerMean.attach(&signalnumber, MEAN_TS);  //Detemining the signalnumbers
-    MyTickerServo.attach(&servo, 0.1f);
-
-    //Scope
-    //scopeTimer.attach_us(&scope, &HIDScope::send, 2e4);
-    FilterTimer.attach_us(&FilterUpdate, 1e3);
+    
+    //Tickers
+    MyTickerController.attach(&motor_control, CONTROLLER_TS);
+    MyTickerMean.attach(&Speedcontrol, MEAN_TS);  
+    MyTickerServo.attach(&servo, SERVO_TS);
+    MyTickerFilter.attach_us(&FilterUpdate, FILTER_TS);
     
     while(1) {
-        //pc.printf(" direction  %i , %i  Signal numbers  %i  %i Position %f  %f  \r\n", direction_l, direction_r, CaseLeft, CaseRight, position_math_l, position_math_r); 
-        pc.printf("direction  %i , %i  Signal numbers  %i  %i Position %f  %f  magnitude1 =%f   magnitude2=%f \r\n",direction_l, direction_r, CaseLeft, CaseRight, position_math_l, position_math_r, magnitude1, magnitude2);
-        wait(0.1f);
     }
-}
-
-
+}
\ No newline at end of file