Diff: main.cpp

Revision:

37:633dd1901681

Parent:

36:f3f3327d1d5a

Child:

38:f1d2d42a4bdc

--- a/main.cpp	Sun Oct 22 08:39:28 2017 +0000
+++ b/main.cpp	Mon Oct 23 13:50:49 2017 +0000
@@ -1,4 +1,5 @@
 #include <vector>
+#include <numeric>
 #include "mbed.h"
 #include "MODSERIAL.h"
 #include "HIDScope.h"
@@ -12,8 +13,15 @@
 #include "biquadChain.h"
 
 // ADJUSTABLE PARAMETERS
+// controller ticker time interval
+const float Ts = 0.1;
 
 // EMG filter parameters
+// calibration time
+const int calSamples = 100;
+
+volatile float avgEMGvalue = 0;
+
 
 // high pass
 biquadFilter HPbq1(0.9837,-1.9674, 0.9837,1.0000,-1.9769,0.9770);
@@ -32,7 +40,11 @@
 // Defining motor gear ratio - for BOTH motors as this is the same in the current configuration
 const float gearRatio = 131;
 
-
+// LOGISTICS
+// Declaring finite-state-machine states
+enum robotStates {KILLED, ACTIVE, CALIBRATING};
+volatile robotStates currentState = KILLED;
+volatile bool stateChanged = true;
 
 // Declaring a controller ticker and volatile variables to store encoder counts and revs
 Ticker controllerTicker;
@@ -56,10 +68,15 @@
 InterruptIn sw3(SW3);
 InterruptIn button1(D2);
 InterruptIn button2(D3);
-AnalogIn pot2(A3);
+//AnalogIn pot2(A3);
 //AnalogIn emg0( A0 );
 //AnalogIn emg1( A1 );
 
+// Defining LED outputs to indicate robot state-us
+DigitalOut ledG(LED_GREEN);
+DigitalOut ledR(LED_RED);
+DigitalOut ledB(LED_BLUE);
+
 // Setting up HIDscope
 volatile float x;
 volatile float y;  
@@ -86,11 +103,11 @@
 // REFERENCE PARAMETERS
 int posRevRange = 2; // describes the ends of the position range in complete motor output shaft revolutions in both directions
 const float maxAngle = 2*3.14*posRevRange; // max angle in radians
-const float Ts = 0.1;
+
 
 // Function getReferencePosition returns reference angle based on potmeter 1
 refGen ref1(A1, maxAngle);
-//refGen ref2(A1, maxAngle);
+//refGen ref2(A2, maxAngle);
 
 // readEncoder reads counts and revs and logs results to serial window
 errorFetch e1(gearRatio, Ts);
@@ -100,20 +117,126 @@
 controller motorController1(k_p, k_d, k_i);
     
 // setMotor1 sets the direction and magnitude pins of motor1 depending on the given motorValue. Negative motorValue means clockwise rotation
-motorConfig motor1(LED_GREEN,LED_RED,LED_BLUE,D5,D4);
-    
+motorConfig motor1(D4,D5);
+motorConfig motor2(D7,D6);
+
+// PROBLEM: if I'm processing the state machine in the endless while loop, how can I adjust robot behavior in the ticker (as it'll keep running)? Do I need to also implement it there? If so, why bother with the while(1) in the main function in the first place?
 void measureAndControl(){
+    // Read encoders and EMG signal (unnfiltered reference)
     m1counts = Encoder1.getPulses();
     m2counts = Encoder2.getPulses();
-    float r1 = ref1.getReference();
-    r1 = HPbqc.applyFilter(r1);
-    r1 = fabs(r1);
-    r1 = LPbqc.applyFilter(r1);
-    e1.fetchError(m1counts, r1);
-    float motorValue = motorController1.control(e1.e_pos, e1.e_int, e1.e_der);
-    float r1_unfiltered = ref1.getReference();
-    sendDataToPc(r1_unfiltered, r1, e1.e_pos, e1.e_der, motorValue);
-    motor1.setMotor(motorValue);
+    float r1_uf = ref1.getReference();
+    //float r2_uf = ref2.getReference();
+    pc.printf("In controller ticker \r\n");
+    // Finite state machine
+    switch(currentState){
+            case KILLED:
+                {
+                // Initialization of KILLED state: cut power to both motors
+                if(stateChanged){
+                    motor1.kill();
+                    //motor2.kill();
+                    pc.printf("Killed state \r\n");
+                    stateChanged = false;               
+                    }
+
+                // Send reference data to pc
+                sendDataToPc(r1_uf, r1_uf, r1_uf, r1_uf, r1_uf); // just send the EMG signal value to HIDscope
+                
+                // Set LED to red
+                ledR = 0;
+                ledG = 1;
+                ledB = 1;
+                // need something here to check if "killswitch" has been pressed (?)
+                // NOTE: state transition is handled using buttons triggering functions motorConfig::kill() and motorConfig::turnMotorOn
+                break;
+                }
+            case ACTIVE:
+                {
+                if(stateChanged){
+                    pc.printf("Active state \r\n");
+                    }
+                // Filter reference
+                float r1 = HPbqc.applyFilter(r1_uf);
+                r1 = fabs(r1);
+                r1 = LPbqc.applyFilter(r1);
+
+                //float r2 = HPbqc.applyFilter(r2_uf);
+                //r2 = fabs(r2);
+                //r2 = LPbqc.applyFilter(r2);
+
+                
+                // Compute error
+                e1.fetchError(m1counts, r1);
+                //e2.fetchError(m2counts, r2);
+                
+                // Compute motor value using controller and set motor
+                float motorValue = motorController1.control(e1.e_pos, e1.e_int, e1.e_der);
+                motor1.setMotor(motorValue);
+                
+                // Send data to HIDscope
+                sendDataToPc(r1_uf, r1, e1.e_pos, e1.e_der, motorValue);
+                
+                // Set LED to blue
+                ledR = 1;
+                ledG = 1;
+                ledB = 0;
+                // NOTE: state transition is handled using buttons triggering functions motorConfig::kill() and motorConfig::turnMotorOn
+                break;
+                }
+            case CALIBRATING:
+                {
+                // NOTE: maybe wrap this whole calibrating thing in a library?
+                
+               // Do I need to kill motors here?
+
+                
+                // Initialization of CALIBRATE state
+                static int Ncal = 0;
+                std::vector<float> EMGsamples;
+                
+                if(stateChanged){                                        
+                    // Kill motors
+                    pc.printf("Calibrate state \r\n");
+                    motor1.kill();
+                    //motor2.kill();
+                    
+                    // Clear sample value vector and reset counter variable
+                    EMGsamples.clear();
+                    Ncal = 0;
+                    stateChanged = false;
+                    }
+
+                // fill the array with sample data if it is not yet filled
+                if(Ncal < calSamples){
+                    pc.printf("%.2f \r\n", r1_uf);
+                    EMGsamples.push_back(r1_uf);
+                    pc.printf("%.2f \r\n", EMGsamples.end());
+                    Ncal++;
+                    }
+                // if array is filled compute the mean value and switch to active state
+                else {
+                    // Set new avgEMGvalue
+                    avgEMGvalue = std::accumulate(EMGsamples.begin(), EMGsamples.end(), 0)/calSamples; // still needs to return this value 
+                    
+                    pc.printf("Avg emg value is %.2f changed state to active", avgEMGvalue);
+                    // State transition logic
+                    currentState = ACTIVE;
+                    stateChanged = true;
+                    Ncal = 0;
+                    }
+                
+                // Set LED to green
+                ledR = 1;
+                ledG = 0;
+                ledB = 1;
+                sendDataToPc(r1_uf, r1_uf, r1_uf, r1_uf, r1_uf);
+                break;
+                }
+        }
+    
+    
+    
     }
 
 void rSwitchDirection(){
@@ -121,14 +244,31 @@
     pc.printf("Switched reference direction! \r\n");
     }
 
+void killSwitch(){
+    currentState = KILLED;
+    stateChanged = true;
+    }
+    
+void activateRobot(){
+    currentState = ACTIVE;
+    stateChanged = true;
+    }
+    
+void calibrateRobot(){
+    currentState = CALIBRATING;
+    stateChanged = true;
+    }
 
 int main()
     {
+    pc.baud(115200);
     pc.printf("Main function");
-    sw2.fall(&motor1,&motorConfig::killSwitch);
-    sw3.fall(&motor1, &motorConfig::turnMotorOn);
+    // Attaching state change functions to buttons;
+    sw2.fall(&killSwitch);
+    sw3.fall(&activateRobot);
+    button1.rise(&calibrateRobot);
     button2.rise(&rSwitchDirection);
-    pc.baud(115200);
+    
     controllerTicker.attach(measureAndControl, Ts);
     pc.printf("Encoder ticker attached and baudrate set");    
     }