Diff: main.cpp

Revision:

10:614050a50c2f

Parent:

8:24f6744d47b2

Child:

11:28b5ec12a4b9

--- a/main.cpp	Wed Nov 01 22:47:30 2017 +0000
+++ b/main.cpp	Thu Nov 02 10:17:33 2017 +0000
@@ -46,9 +46,9 @@
 //-----------------------------EMG Signals--------------------------------------
 //------------------------------------------------------------------------------
 // Filtering the signal and getting the usefull information out of it.
-const int n = 1500;                  //Window size for the mean value, also adjust in signalnumber.cpp
+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 = 1500;                  //Number of samples for calibration
+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
@@ -105,20 +105,20 @@
 //---------------------------------Servo----------------------------------------
 //------------------------------------------------------------------------------
 void servo(){
-    Signal_filteredLeft = fabs(FilterLeft(emg0));//*kLeft
-    Signal_filteredRight = fabs(FilterRight(emg2)); //*kRight
+    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>=3){
+    if (CaseLeft>=2){
         Up = 0; 
         Up = 1;
     }
-    else if (CaseRight >=3){
+    else if (CaseRight >=2){
         Down = 0;
         Down = 1;
     }
 }
-int milli =0;
+
 
 //------------------------------------------------------------------------------
 //---------------------------Mode selection-------------------------------------
@@ -137,6 +137,34 @@
         servo();
     }
     pc.printf("\r\n mode = %i \r\n", mode); 
+    
+    
+    /*if((mode==3) || (mode==6)) {
+        Led_red = 0;
+        Led_green = 0;
+        Led_blue = 0;
+    }
+    else if (mode==1) {
+        Led_red = 0;
+        Led_green = 1;
+        Led_blue = 1;
+    }
+    else if (mode==2) {
+        Led_red = 1;
+        Led_green = 0;
+        Led_blue = 1;
+    }
+    else if (mode==4) {
+        Led_red = 1;
+        Led_green = 1;
+        Led_blue = 0;
+        }
+    else if (mode==5) {
+        Led_red = 0;
+        Led_green = 0;
+        Led_blue = 1;
+    }*/
+    
 }
 
 // Control mode selection-------------------------------------------------------
@@ -175,10 +203,10 @@
 //------------------------------------------------------------------------------
 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_rangey = 300.0; //Max range on the y axis
+const float max_rangex = 700.0, max_rangey = 450.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 alpha_offset = -0.4114;
+const float beta_offset  = 0.0486;
 const float L1 = 450.0, L2 = 490.0;                 //Length of the bodies
 
 //------------------------------------------------------------------------------
@@ -197,12 +225,21 @@
 const float M1_N = 0.5;
 float position_math_l =0, position_math_r=0;
 
-void motor1_control(){
+float motor1_control(){
     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;
-    motor1 = fabs(magnitude1);
+            
+    //Determine Motor Magnitude
+    if (fabs(magnitude1)>1) {
+        motor1 = 1;
+    }
+    else {
+        motor1 = fabs(magnitude1);
+    }
+    
+    
     // Determine Motor Direction 
     if (magnitude1  < 0){
         motor1DirectionPin = 1;
@@ -210,6 +247,7 @@
     else{
         motor1DirectionPin = 0;
     }
+    return magnitude1;
 }
 
 //------------------------------------------------------------------------------
@@ -227,12 +265,21 @@
 const double motor2_gain = 2*PI;
 const float M2_N = 0.5;
 
-void motor2_control(){
+float motor2_control(){
     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;
-    motor2 = fabs(magnitude2);
+        
+    //Determine Motor Magnitude
+    if (fabs(magnitude2)>1) {
+        motor2 = 1;
+    }
+    else {
+        motor2 = fabs(magnitude2);
+    }
+    
+    
     //Determine Motor Direction
     if (magnitude2 > 0){
         motor2DirectionPin = 1;
@@ -240,22 +287,28 @@
     else{
         motor2DirectionPin = 0;
     }
+    return magnitude2;
 }
 
 //------------------------------------------------------------------------------
 //----------------------------Motor controller----------------------------------
 //------------------------------------------------------------------------------
 int direction_l, direction_r;
+float magnitude1, magnitude2;
 
 void motor_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();
+    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
+    magnitude1 = motor1_control(); 
+    magnitude2 = motor2_control();
+    
+    
+    
+    
 }
-//------------------------------------------------------------------------------
+/*//------------------------------------------------------------------------------
 //-------------------------------HID Scope--------------------------------------
 //------------------------------------------------------------------------------
 
@@ -272,7 +325,7 @@
 
 //Working
 //SignalLeft.getmeanLeft(n, fabs(FilterLeft(emg0.read())))
-//SignalRight.getmeanRight(n, fabs(FilterRight(emg2.read())))
+//SignalRight.getmeanRight(n, fabs(FilterRight(emg2.read())))*/
 
 //------------------------------------------------------------------------------
 //------------------------------Main--------------------------------------------
@@ -290,13 +343,14 @@
     MyControllerTicker.attach(&motor_control, CONTROLLER_TS); //Determining motorposiitons
     MyTickerMean.attach(&signalnumber, MEAN_TS);  //Detemining the signalnumbers
 //    MyTickerMode.attach(&signalmode, MEAN_TS);
-
+/*
     //Scope
     scopeTimer.attach_us(&scope, &HIDScope::send, 2e4);
-    controllerTimer.attach_us(&HID_Monitor, 1e3);
+    controllerTimer.attach_us(&HID_Monitor, 1e3);*/
     
     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  \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);
     }
 }