Diff: main.cpp

Revision:

12:0765ea2c4c85

Parent:

11:28b5ec12a4b9

--- a/main.cpp	Thu Nov 02 11:39:59 2017 +0000
+++ b/main.cpp	Thu Nov 02 13:21:04 2017 +0000
@@ -8,7 +8,7 @@
 #include "encoder.h"
 #include "Servo.h"
 #include "FastPWM.h"
-#include "HIDScope.h"
+//#include "HIDScope.h"
 
 //This code is for Mbed 2
 //------------------------------------------------------------------------------
@@ -38,6 +38,7 @@
 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
@@ -86,7 +87,7 @@
 
 //Calibration-------------------------------------------------------------------
 void setled(){
-    Led_red=0; Led_green=1; Led_blue=1;
+    Led_red=1; Led_green=1; Led_blue=1; Led_calibration=0;
 }
 
 // One calibration
@@ -98,7 +99,7 @@
     kLeft = 1/emg_offsetmaxLeft;
     kRight = 1/emg_offsetmaxRight;
     kMode = 1/emg_offsetmaxMode;
-    Led_red=1;   //Set led to Green
+    Led_calibration = 1;   //Set led to Green
 }
 
 //------------------------------------------------------------------------------
@@ -147,7 +148,7 @@
     pc.printf("\r\n mode = %i \r\n", mode); 
     
     
-    /*if((mode==3) || (mode==6)) {
+    if((mode==3) || (mode==6)) {
         Led_red = 0;
         Led_green = 0;
         Led_blue = 0;
@@ -171,7 +172,7 @@
         Led_red = 0;
         Led_green = 0;
         Led_blue = 1;
-    }*/
+    }
     
 }
 
@@ -233,7 +234,7 @@
 const float M1_N = 0.5;
 float position_math_l =0, position_math_r=0;
 
-float motor1_control(){
+void 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;
@@ -255,7 +256,6 @@
     else{
         motor1DirectionPin = 0;
     }
-    return magnitude1;
 }
 
 //------------------------------------------------------------------------------
@@ -273,7 +273,7 @@
 const double motor2_gain = 2*PI;
 const float M2_N = 0.5;
 
-float motor2_control(){
+void 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;
@@ -295,7 +295,6 @@
     else{
         motor2DirectionPin = 0;
     }
-    return magnitude2;
 }
 
 //------------------------------------------------------------------------------
@@ -309,26 +308,23 @@
     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();
-    
-    
-    
-    
+    motor1_control(); //magnitude1 = 
+    motor2_control(); // magnitude2 = 
+  
 }
 //------------------------------------------------------------------------------
 //-------------------------------HID Scope--------------------------------------
 //------------------------------------------------------------------------------
 
-HIDScope scope(4);
-Ticker scopeTimer;
-Ticker controllerTimer;
+//HIDScope scope(4);
+//Ticker scopeTimer;
+Ticker FilterTimer;
 
-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()))));    
+void FilterUpdate(){
+    FilterLeft(emg0.read());
+    FilterRight(emg2.read());
+    SignalLeft.getmeanLeft(n, fabs(FilterLeft(emg0.read())));
+    SignalRight.getmeanRight(n, fabs(FilterRight(emg2.read())));    
 }
 
 //Working
@@ -353,8 +349,8 @@
     MyTickerServo.attach(&servo, 0.1f);
 
     //Scope
-    scopeTimer.attach_us(&scope, &HIDScope::send, 2e4);
-    controllerTimer.attach_us(&HID_Monitor, 1e3);
+    //scopeTimer.attach_us(&scope, &HIDScope::send, 2e4);
+    FilterTimer.attach_us(&FilterUpdate, 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);