Diff: main.cpp

Revision:

11:28b5ec12a4b9

Parent:

10:614050a50c2f

Child:

12:0765ea2c4c85

--- a/main.cpp	Thu Nov 02 10:17:33 2017 +0000
+++ b/main.cpp	Thu Nov 02 11:39:59 2017 +0000
@@ -16,7 +16,7 @@
 //------------------------------------------------------------------------------
 MODSERIAL pc(USBTX, USBRX);         //Establish connection
 Ticker MyControllerTicker;          //Declare Ticker for Motors
-Ticker MyTickerMode;                //Declare Ticker 
+Ticker MyTickerServo;               //Declare Ticker Servo control
 Ticker MyTickerMean;                //Declare Ticker Signalprocessing
 
 InterruptIn But1(PTA4);             //Declare button for min calibration
@@ -104,18 +104,30 @@
 //------------------------------------------------------------------------------
 //---------------------------------Servo----------------------------------------
 //------------------------------------------------------------------------------
+void up(){
+    Up = 1;
+    Up = 0;
+    Up = 1;
+}
+void down(){
+    Down = 1;
+    Down = 0;
+    Down = 1;
+}
+
+int mode =0;
 void servo(){
-    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){
-        Up = 0; 
-        Up = 1;
-    }
-    else if (CaseRight >=2){
-        Down = 0;
-        Down = 1;
+     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){
+            up();
+        }
+        else if (CaseRight == 2){
+            down();
+        }
     }
 }
 
@@ -123,7 +135,6 @@
 //------------------------------------------------------------------------------
 //---------------------------Mode selection-------------------------------------
 //------------------------------------------------------------------------------
-int mode =0;
 
 //Recieving mode selection from EMG mode signal
 void mode_selection(){
@@ -132,10 +143,7 @@
     }
     else{
         mode++;
-    }
-    if (mode==3||mode==6){
-        servo();
-    }
+    }    
     pc.printf("\r\n mode = %i \r\n", mode); 
     
     
@@ -203,7 +211,7 @@
 //------------------------------------------------------------------------------
 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 = 700.0, max_rangey = 450.0; //Max range on the y axis
+const float max_rangex = 650.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 = -0.4114;
 const float beta_offset  = 0.0486;
@@ -308,7 +316,7 @@
     
     
 }
-/*//------------------------------------------------------------------------------
+//------------------------------------------------------------------------------
 //-------------------------------HID Scope--------------------------------------
 //------------------------------------------------------------------------------
 
@@ -325,7 +333,7 @@
 
 //Working
 //SignalLeft.getmeanLeft(n, fabs(FilterLeft(emg0.read())))
-//SignalRight.getmeanRight(n, fabs(FilterRight(emg2.read())))*/
+//SignalRight.getmeanRight(n, fabs(FilterRight(emg2.read())))
 
 //------------------------------------------------------------------------------
 //------------------------------Main--------------------------------------------
@@ -342,11 +350,11 @@
     motor1.period(0.0001f); motor2.period(0.0001f);
     MyControllerTicker.attach(&motor_control, CONTROLLER_TS); //Determining motorposiitons
     MyTickerMean.attach(&signalnumber, MEAN_TS);  //Detemining the signalnumbers
-//    MyTickerMode.attach(&signalmode, MEAN_TS);
-/*
+    MyTickerServo.attach(&servo, 0.1f);
+
     //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);