Diff: main.cpp

Revision:

8:24f6744d47b2

Parent:

7:105e3ae0fb19

Child:

10:614050a50c2f

--- a/main.cpp	Wed Nov 01 14:11:58 2017 +0000
+++ b/main.cpp	Wed Nov 01 22:23:25 2017 +0000
@@ -8,19 +8,19 @@
 #include "encoder.h"
 #include "Servo.h"
 #include "FastPWM.h"
+#include "HIDScope.h"
 
 //This code is for Mbed 2
 //------------------------------------------------------------------------------
 //------------------------------------------------------------------------------
 //------------------------------------------------------------------------------
 MODSERIAL pc(USBTX, USBRX);         //Establish connection
-Ticker MyControllerTicker;          //Declare Ticker Motor 1
-Ticker MyTickerMode;                //Declare Ticker Motor 2
+Ticker MyControllerTicker;          //Declare Ticker for Motors
+Ticker MyTickerMode;                //Declare Ticker 
 Ticker MyTickerMean;                //Declare Ticker Signalprocessing
 
-InterruptIn But2(PTA4);             //Declare button for min calibration
-InterruptIn But1(PTC6);             //Declare button for max calibration
-InterruptIn Mode(D7);
+InterruptIn But1(PTA4);             //Declare button for min calibration
+InterruptIn Mode(PTC6);             //Declare button for max calibration
 
 AnglePosition Angle;                //Declare Angle calculater
 PIDControl PID;                     //Declare PID Controller
@@ -31,29 +31,24 @@
 Movement MoveRight;
 
 AnalogIn    emg0( A0 );             //Set Inputpin for EMG 0 signal Left 
-//AnalogIn    emg1( A1 );             //Set Inputpin for EMG 1 signal Left
 AnalogIn    emg2( A2 );             //Set Inputpin for EMG 2 signal Right
-//AnalogIn    emg3( A3 );             //Set Inputpin for EMG 3 signal Right        
 AnalogIn    emg4( A4 );             //Set Inputpin for EMG 4 signal Mode
-//AnalogIn    emg5( A5 );             //Set Inputpin for EMG 5 signal Mode
 DigitalOut     Up( D9 );             //Set digital in for mode selection
 DigitalOut   Down( D8 );
 DigitalOut  Led_red(LED_RED);          
 DigitalOut  Led_green(LED_GREEN);
 DigitalOut  Led_blue(LED_BLUE);  
-DigitalOut  test(D2);
 
 const float CONTROLLER_TS = 0.02;   //Motor frequency
 const float MEAN_TS = 0.002;        //Filter frequency
 
-
 //------------------------------------------------------------------------------
 //-----------------------------EMG Signals--------------------------------------
 //------------------------------------------------------------------------------
 // Filtering the signal and getting the usefull information out of it.
-const int n = 400;                  //Window size for the mean value, also adjust in signalnumber.cpp
-const int action =50;               //Number of same mean values to change the signalnumber
-const int m = 400;                  //Number of samples for calibration
+const int n = 1500;                  //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 = 1500;                  //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
@@ -93,56 +88,27 @@
 void setled(){
     Led_red=0; Led_green=1; Led_blue=1;
 }
-// Zero calibration
-void mincalibration(){
-    pc.printf("start min calibration \r\n");
-    emg_offsetLeft = SignalLeft.calibrate(m,FilterLeft(emg0));
-    emg_offsetRight = SignalRight.calibrate(m,FilterRight(emg2));
-    emg_offsetMode = SignalMode.calibrate(m, FilterMode(emg4));
-    pc.printf("offsets: %f %f \r\n", emg_offsetLeft, emg_offsetRight);
-    Led_green=0; Led_red=0; //Set led to Yellow
-}
+
 // One calibration
 void maxcalibration(){
     pc.printf("start max calibration \r\n");
-    emg_offsetmaxLeft = SignalLeft.calibrate(m, FilterLeft(emg0))-emg_offsetLeft;
-    emg_offsetmaxRight = SignalRight.calibrate(m, FilterRight(emg2))-emg_offsetRight;
-    emg_offsetmaxMode = SignalMode.calibrate(m, FilterMode(emg4))-emg_offsetMode;
+    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;
-    pc.printf("offsets: %f %f, scale %f %f \r\n", emg_offsetmaxLeft, emg_offsetmaxRight, kLeft, kRight);
     Led_red=1;   //Set led to Green
 }
 
-//Filtering the signals---------------------------------------------------------
-//Filter de EMG signals with a BiQuad filter
-/*
-float FilterLeft(float input, float offset){
-    float Signal=(input-offset);                    //((input0+input1)/2)
-    float Signal_filtered= BiQuad_filter_Left.step(Signal);
-    return Signal_filtered;
-}
-float FilterRight(float input, float offset){
-    float Signal=input-offset;                    //((input0+input1)/2)
-    float Signal_filtered= BiQuad_filter_Right.step(Signal);
-    return Signal_filtered;
-}
-float FilterMode(float input, float offset){
-    float Signal=input-offset;                    //((input0+input1)/2)
-    float Signal_filtered= BiQuad_filter_Mode.step(Signal);
-    return Signal_filtered;
-}
-*/
-
 //------------------------------------------------------------------------------
 //---------------------------------Servo----------------------------------------
 //------------------------------------------------------------------------------
 void servo(){
-    Signal_filteredLeft = fabs(FilterLeft(emg0)-emg_offsetLeft);//*kLeft
+    Signal_filteredLeft = fabs(FilterLeft(emg0));//*kLeft
     Signal_filteredRight = fabs(FilterRight(emg2)); //*kRight
-    CaseLeft = SignalLeft.getnumber(n, action, Signal_filteredLeft);
-    CaseRight = SignalRight.getnumber(n, action, Signal_filteredRight);
+    CaseLeft = SignalLeft.getnumberLeft(n, action, Signal_filteredLeft);
+    CaseRight = SignalRight.getnumberRight(n, action, Signal_filteredRight);
     if (CaseLeft>=3){
         Up = 0; 
         Up = 1;
@@ -175,27 +141,24 @@
 
 // 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)-emg_offsetLeft);//*kLeft
-    mean_value_left = SignalLeft.getmean(n, Signal_filteredLeft);          
-    CaseLeft = SignalLeft.getnumber(n, action, Signal_filteredLeft);
+    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);
-    mean_value_right = SignalRight.getmean(n, Signal_filteredRight); 
-    CaseRight = SignalRight.getnumber(n, action, Signal_filteredRight);
+    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.getmean(n, Signal_filteredMode);
-    CaseMode = SignalMode.getnumber(n, action, Signal_filteredMode);
-    /*if(CaseMode >= 3){
+    /*
+    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();
@@ -207,28 +170,16 @@
     }*/
 }
 
-
-
-
-
-
-
-
 //------------------------------------------------------------------------------
 //-------------------------Kinematic Constants----------------------------------
 //------------------------------------------------------------------------------
 const double RAD_PER_PULSE = 0.00074799825*2;       //Number of radials per pulse from the encoder
 const double PI = 3.14159265358979323846;           //Pi
-const float max_rangex = 500.0;                     //Max range on the x axis
-const float max_rangey = 300.0;                     //Max range on the y axis
-const float x_offset = 156.15;                      //Start x position from the shoulder joint
-const float y_offset = -76.97;                      //Start y position from the shoulder joint
+const float max_rangex = 500.0, max_rangey = 300.0; //Max range on the y axis
+const float x_offset = 156.15, y_offset = -76.97;   //Starting positions from the shoulder joint
 const float alpha_offset = -(21.52/180)*PI;         //Begin angle Alpha at zero zero
 const float beta_offset  = 0.0;                     //Begin angle Beta at zero zero
-const float L1 = 450.0;                             //Length of the first body
-const float L2 = 490.0;                             //Length of the second body
-
-
+const float L1 = 450.0, L2 = 490.0;                 //Length of the bodies
 
 //------------------------------------------------------------------------------
 //--------------------------------Motor1----------------------------------------
@@ -244,10 +195,10 @@
 double M1_v1 = 0.0, M1_v2 = 0.0; //Calculation values
 const double motor1_gain = 2*PI;
 const float M1_N = 0.5;
-float position_math[2]={};
+float position_math_l =0, position_math_r=0;
 
 void motor1_control(){
-    float reference_alpha = Angle.getbeta(max_rangex, max_rangey, x_offset, y_offset, beta_offset, L1, L2, position_math[0], position_math[1]);
+    float reference_alpha = Angle.getbeta(max_rangex, max_rangey, x_offset, y_offset, beta_offset, L1, L2, position_math_l, position_math_r);
     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;
@@ -277,7 +228,7 @@
 const float M2_N = 0.5;
 
 void motor2_control(){
-    float reference_beta = Angle.getalpha(max_rangex, max_rangey, x_offset, y_offset, alpha_offset, L1, L2, position_math[0], position_math[1]);
+    float reference_beta = Angle.getalpha(max_rangex, max_rangey, x_offset, y_offset, alpha_offset, L1, L2, position_math_l, position_math_r);
     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;
@@ -291,37 +242,61 @@
     }
 }
 
+//------------------------------------------------------------------------------
+//----------------------------Motor controller----------------------------------
+//------------------------------------------------------------------------------
+int direction_l, direction_r;
+
 void motor_control(){
-    position_math[0]= MoveLeft.getposition(CaseLeft, mode, 0, max_rangex);
-    position_math[1]= MoveRight.getposition(CaseRight, mode, 1, max_rangey);
-    motor1_control();
-    motor2_control();
+    direction_l = MoveLeft.getdirectionLeft(mode);
+    direction_r = MoveRight.getdirectionRight(mode); 
+    position_math_l= MoveLeft.getpositionLeft(CaseLeft, mode, max_rangex);
+    position_math_r= MoveRight.getpositionRight(CaseRight, mode, max_rangey);
+//    motor1_control(); 
+//    motor2_control();
 }
 //------------------------------------------------------------------------------
+//-------------------------------HID Scope--------------------------------------
 //------------------------------------------------------------------------------
+
+HIDScope scope(4);
+Ticker scopeTimer;
+Ticker controllerTimer;
+
+void HID_Monitor(){
+    scope.set(0, fabs(FilterLeft(emg0.read())));
+    scope.set(1, fabs(FilterRight(emg2.read())));
+    scope.set(2, SignalLeft.getmeanLeft(n, fabs(FilterLeft(emg0.read()))));    
+    scope.set(3, 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);
-    test=0;
     setled(); 
     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);
-    But2.rise(&mincalibration);
     But1.rise(&maxcalibration);
     Mode.rise(&mode_selection);
     motor1.period(0.0001f); motor2.period(0.0001f);
-    MyControllerTicker.attach(&motor_control, CONTROLLER_TS); 
-    MyTickerMean.attach(&signalnumber, MEAN_TS);
+    MyControllerTicker.attach(&motor_control, CONTROLLER_TS); //Determining motorposiitons
+    MyTickerMean.attach(&signalnumber, MEAN_TS);  //Detemining the signalnumbers
 //    MyTickerMode.attach(&signalmode, MEAN_TS);
-//    MyTickerMean.attach(&signalnumberright, MEAN_TS);
-//    MyTickerMean.attach(&signalmode,MEAN_TS);
 
+    //Scope
+    scopeTimer.attach_us(&scope, &HIDScope::send, 2e4);
+    controllerTimer.attach_us(&HID_Monitor, 1e3);
+    
     while(1) {
-//        pc.printf("Mean %f %f %f \r\n", mean_value_left, mean_value_right, mean_value_mode);
-//        pc.printf("Case %i %i %i, mode = %i \r\n", CaseLeft, CaseRight, CaseMode, mode);
-        pc.printf("mean %f , case = %i \r\n", mean_value_left, CaseLeft);
+        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); 
         wait(0.1f);
     }
 }