Diff: main.cpp

Revision:

12:f4331640e3ad

Parent:

10:8c38a1a5b522

Child:

13:a243388e1790

Child:

14:1343966a79e8

--- a/main.cpp	Mon Oct 21 12:44:20 2019 +0000
+++ b/main.cpp	Tue Oct 22 22:01:16 2019 +0000
@@ -1,30 +1,35 @@
+// MOTOR_CONTROL FUNCTION HAS TO BE ADJUSTED TO TWO MOTORS
+// reference velocity has to be fixed? idk? --> wait for file from bram en paul
+
 #include "mbed.h"
 #include "HIDScope.h"
 #include "BiQuad.h"
 #include "MODSERIAL.h"
 #include "FastPWM.h"
 #include "QEI.h"
+
 #include <math.h>
+#include <deque>
 
 #include "Motor_Control.h"
 
-DigitalIn button1(D12);
-InterruptIn button2(SW2);
-DigitalOut ledr(LED_RED);
+DigitalIn button1(D12);     // Button1 input to go to next state
+InterruptIn button2(SW2);   // Button2 input to activate failuremode()
+DigitalOut ledr(LED_RED);   // Red LED output to show
 
-AnalogIn shield0(A0);
-AnalogIn shield1(A1);
-AnalogIn shield2(A2);
-AnalogIn shield3(A3);
+AnalogIn shield0(A0);   // Input EMG Shield 0
+AnalogIn shield1(A1);   // Input EMG Shield 1
+AnalogIn shield2(A2);   // Input EMG Shield 2
+AnalogIn shield3(A3);   // Input EMG Shield 3
 
-Ticker measurecontrol;
-DigitalOut motor1Direction(D7);
-FastPWM motor1Velocity(D6);
+Ticker measurecontrol;              // Ticker function for motor in- and output
+DigitalOut motor1Direction(D7);     // Direction of motor 1
+FastPWM motor1Velocity(D6);         // FastPWM class to set motor velocity of motor 1
 MODSERIAL pc(USBTX, USBRX);
-QEI Encoder(D8,D9,NC,8400);
+QEI Encoder(D8,D9,NC,8400);         // Input from the encoder to measure how much the motor has turned
 
 float PI = 3.1415926f;//535897932384626433832795028841971693993;
-float timeinterval = 0.001f;         // Time interval of the Ticker function
+float timeinterval = 0.001f;        // Time interval of the Ticker function
 bool whileloop = true;              // Statement to keep the whileloop in the main function running
                                     // While loop has to stop running when failuremode is activated
 
@@ -39,20 +44,78 @@
 
 
 void motorsoff() {
-    bool aa = true;                                 // Boolean for the while loop
-    sendtomotor(0.0f);
-    while (aa) {
-        if (button1.read() == 0) {                  // If button1 is pressed, set MyState to EMG_CALIBRATION and exit the while loop
-            MyState = EMG_CALIBRATION;
-            aa = false;
+    // Function to turn the motors off. First state that the robot has. Robot will stay in this state untill button1 is pressed. 
+    // Robot will not return to this state anymore unless the user sets it back to this state with the keyboard input.
+    
+    bool whileloop_boolean = true;  // Boolean for the while loop
+    sendtomotor(0.0f);              // Set motor velocity to 0
+    
+    while (whileloop_boolean) {
+        if (button1.read() == 0) {          // If button1 is pressed:
+            MyState = EMG_CALIBRATION;      // set MyState to EMG_CALIBRATION and exit the while loop
+            whileloop_boolean = false;      // by making whileloop_boolean equal to false
         }
     }
 }
-/*
+
+float rms_deque(std::deque<float> deque) {
+    float sum = 0;
+    for (int i = 0; i < deque.size(); i++) {
+        sum = sum + pow(deque[i],2);
+    }
+    return pow(sum,0.5f);
+}
+
+
+void measure_data(float &rms_0, float &rms_1, float &rms_2, float &rms_3) {
+    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 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);
+    static BiQuad Filter3(b0,b1,b2,a0,a1);
+    
+    float f_y0 = Filter0.step(shield0);     // Apply filters on the different EMG signals
+    float f_y1 = Filter1.step(shield1);
+    float f_y2 = Filter2.step(shield2);
+    float f_y3 = Filter3.step(shield3);
+    
+    int rms_length = 6;                     // Set the amount of points of which the RMS signal is calculated
+    static std::deque<float> deque_f_y0 (rms_length);   // Create deque for the 4 signals in which data can be added and removed
+    static std::deque<float> deque_f_y1 (rms_length);
+    static std::deque<float> deque_f_y2 (rms_length);
+    static std::deque<float> deque_f_y3 (rms_length);
+    
+    deque_f_y0.push_front(f_y0);    // Take new data point and put it in front of the deque values
+    deque_f_y1.push_front(f_y1);
+    deque_f_y2.push_front(f_y2);
+    deque_f_y3.push_front(f_y3);
+    
+    deque_f_y0.pop_back();          // Remove latest element in deque to keep the deque the same length
+    deque_f_y1.pop_back();
+    deque_f_y2.pop_back();
+    deque_f_y3.pop_back();
+    
+    rms_0 = rms_deque(deque_f_y0);    // Calculate the RMS for the different deques
+    rms_1 = rms_deque(deque_f_y1);    // and give this value to rms_1 which is a reference
+    rms_2 = rms_deque(deque_f_y2);    // 
+    rms_3 = rms_deque(deque_f_y3);
+    
+}
+
+
 void emgcalibration() {
-    // do calibration
+    float rms0, rms1, rms2, rms3;
+    measure_data(rms0, rms1, rms2, rms3);
+    
 }
-*/
+
+
+
 //P control implementation (behaves like a spring)
 double P_controller(double error)
 {
@@ -123,34 +186,35 @@
 }
 
 void failuremode() {
-    MyState = FAILURE_MODE;
-    New_State();
+    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;
-    double period_signal = 1.0f/frequency;
+    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);
-    measurecontrol.attach(measureandcontrol, timeinterval);
+    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;
-    New_State();
+    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 ( (previous_state_int - (int)MyState) != 0 ) {   // If current state is not previous state execute New_State()
             New_State();
         }
-        previous_state_int = (int)MyState;
+        previous_state_int = (int)MyState;                  // Change previous state to current state
     }
-    
-    pc.printf("Programm stops running\r\n");
-    sendtomotor(0.0f);
-    measurecontrol.attach(nothing,10000);
+    // 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;
 }