Diff: main.cpp

Revision:

14:1343966a79e8

Parent:

12:f4331640e3ad

Child:

15:5f9450964075

--- a/main.cpp	Tue Oct 22 22:01:16 2019 +0000
+++ b/main.cpp	Fri Oct 25 08:32:53 2019 +0000
@@ -41,7 +41,59 @@
 // Default state is the state in which the motors are turned off
 States MyState = MOTORS_OFF;
 
+// Initialise the functions
 
+void motorsoff();
+
+float rms_deque(std::deque<float> deque);
+
+void measure_data(float &rms_0, float &rms_1, float &rms_2, float &rms_3);
+
+void det_max(float rms, float &max_rms);
+
+void emgcalibration();
+
+double P_controller(double error);
+
+void nothing(){
+    // Do nothing
+}
+
+void New_State();
+
+void failuremode();
+
+
+
+
+int main()
+{
+    pc.printf("Starting...\r\n\r\n");
+    double frequency = 17000.0f;            // Set motorfrequency
+    double period_signal = 1.0f/frequency;  // Convert to period of the signal
+    pc.baud(115200);
+    
+    button2.fall(failuremode);              // Function is always activated when the button is pressed
+    motor1Velocity.period(period_signal);   // Set the period of the PWMfunction
+    measurecontrol.attach(measureandcontrol, timeinterval); // Ticker function to measure motor input and control the motors
+    
+    int previous_state_int = (int)MyState;  // Save previous state to compare with current state and possibly execute New_State()
+                                            // in the while loop
+    New_State();                            // Execute the functions belonging to the current state
+    
+    while(whileloop)
+    {
+        if ( (previous_state_int - (int)MyState) != 0 ) {   // If current state is not previous state execute New_State()
+            New_State();
+        }
+        previous_state_int = (int)MyState;                  // Change previous state to current state
+    }
+    // While has stopped running
+    pc.printf("Programm stops running\r\n");    // So show that the programm is quiting
+    sendtomotor(0.0f);                          // Set the motor velocity to 0
+    measurecontrol.attach(nothing,10000);       // Attach empty function to Ticker (?? Appropriate solution ??)
+    return 0;
+}
 
 void motorsoff() {
     // Function to turn the motors off. First state that the robot has. Robot will stay in this state untill button1 is pressed. 
@@ -66,14 +118,18 @@
     return pow(sum,0.5f);
 }
 
-
-void measure_data(float &rms_0, float &rms_1, float &rms_2, float &rms_3) {
+void measure_data(float &rms_0, float &rms_1, float &rms_2, float &rms_3, bool determine_max) {
     float b0 = 0.0f;  // Coefficients from the following formula:
     float b1 = 0.0f;  //
     float b2 = 0.0f;  //        b0 + b1 z^-1 + b2 z^-2
     float a0 = 0.0f;  // H(z) = ----------------------
     float a1 = 0.0f;  //        a0 + a1 z^-1 + a2 z^-2
     
+    static float max_rms0 = 0;
+    static float max_rms1 = 0;
+    static float max_rms2 = 0;
+    static float max_rms3 = 0;
+    
     static BiQuad Filter0(b0,b1,b2,a0,a1);  // Create 4 equal filters used for the different EMG signals
     static BiQuad Filter1(b0,b1,b2,a0,a1);
     static BiQuad Filter2(b0,b1,b2,a0,a1);
@@ -105,16 +161,45 @@
     rms_2 = rms_deque(deque_f_y2);    // 
     rms_3 = rms_deque(deque_f_y3);
     
-}
-
-
-void emgcalibration() {
-    float rms0, rms1, rms2, rms3;
-    measure_data(rms0, rms1, rms2, rms3);
+    if (determine_max == true) {
+        
+        det_max(rms_0, max_rms0);
+        det_max(rms_1, max_rms1);
+        det_max(rms_2, max_rms2);
+        det_max(rms_3, max_rms3);
+        
+    }
+    else if (determine_max == false) {
+        rms_0 = rms_0/max_rms0;
+        rms_1 = rms_1/max_rms1;
+        rms_2 = rms_2/max_rms2;
+        rms_3 = rms_3/max_rms3;
+    }
     
 }
 
+void det_max(float rms, float &max_rms) {
+    max_rms = max_rms < rms ? rms : max_rms;
+}
 
+void emgcalibration() {
+    float rms0, rms1, rms2, rms3;           // RMS values of the different EMG signals
+
+    measure_data(rms0, rms1, rms2, rms3, true);   // Calculate RMS values
+    
+    float duration = 10.0f;                 // Duration of the emgcalibration function, in this case 10 seconds
+    int rounds = (duration / timeinterval); // Determine the amount of times this function has to run to run for the duration time
+    // rounds is an integer so the value of duration / timeinterval is floored
+    
+    static int counter = 0;             // Counter which keeps track of the amount of times the function has executed
+    if (counter >= rounds) {
+        MyState = MOTOR_CALIBRATION;    // If counter is larger than rounds, change MyState to MOTOR_CALIBRATION
+    }
+    else {
+        counter++;  // Else increase counter by 1
+    }
+    
+}
 
 //P control implementation (behaves like a spring)
 double P_controller(double error)
@@ -127,10 +212,6 @@
     return u_k;
 }
 
-void nothing(){// Do nothing
-}
-
-
 void New_State() {
     switch (MyState)
     {
@@ -141,7 +222,7 @@
         
         case EMG_CALIBRATION :
             pc.printf("State: EMG Calibration");
-            //measureandcontrol(emgcalibration,timeinterval);
+            measurecontrol.attach(emgcalibration,timeinterval);
             break;
         
         case MOTOR_CALIBRATION :
@@ -188,33 +269,4 @@
 void failuremode() {
     MyState = FAILURE_MODE; // failurmode() is activated so set MyState to FAILURE_MODE
     New_State();            // Execute actions coupled to FAILURE_MODE
-}
-
-int main()
-{
-    pc.printf("Starting...\r\n\r\n");
-    double frequency = 17000.0f;            // Set motorfrequency
-    double period_signal = 1.0f/frequency;  // Convert to period of the signal
-    pc.baud(115200);
-    
-    button2.fall(failuremode);              // Function is always activated when the button is pressed
-    motor1Velocity.period(period_signal);   // Set the period of the PWMfunction
-    measurecontrol.attach(measureandcontrol, timeinterval); // Ticker function to measure motor input and control the motors
-    
-    int previous_state_int = (int)MyState;  // Save previous state to compare with current state and possibly execute New_State()
-                                            // in the while loop
-    New_State();                            // Execute the functions belonging to the current state
-    
-    while(whileloop)
-    {
-        if ( (previous_state_int - (int)MyState) != 0 ) {   // If current state is not previous state execute New_State()
-            New_State();
-        }
-        previous_state_int = (int)MyState;                  // Change previous state to current state
-    }
-    // While has stopped running
-    pc.printf("Programm stops running\r\n");    // So show that the programm is quiting
-    sendtomotor(0.0f);                          // Set the motor velocity to 0
-    measurecontrol.attach(nothing,10000);       // Attach empty function to Ticker (?? Appropriate solution ??)
-    return 0;
-}
+}
\ No newline at end of file