Diff: main.cpp

Revision:

17:3b463660aa42

Parent:

15:5f9450964075

--- a/main.cpp	Fri Oct 25 08:44:57 2019 +0000
+++ b/main.cpp	Tue Oct 29 10:50:37 2019 +0000
@@ -1,5 +1,7 @@
 // MOTOR_CONTROL FUNCTION HAS TO BE ADJUSTED TO TWO MOTORS
 // reference velocity has to be fixed? idk? --> wait for file from bram en paul
+// Coefficients for the filters have to be adjusted --> Is 1 filter enough?
+
 
 #include "mbed.h"
 #include "HIDScope.h"
@@ -9,33 +11,40 @@
 #include "QEI.h"
 
 #include <math.h>
-#include <deque>
+//#include <deque>
+//#include <array>
 
-#include "Motor_Control.h"
+
+//#include "Motor_Control.h"
 
 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);   // 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
+// CHECK DIGITAL PORTS
+AnalogIn S0(A0), S1(A1), S2(A2) ,S3(A3);   // Input EMG Shield 0,1,2,3
+DigitalOut  MD0(D1), MD1(D2), MD2(D3); // MotorDirection of motors 0,1,2
+FastPWM MV0(D4), MV1(D5), MV2(D6); // MotorVelocities of motors 0,1,2
+QEI E0(D7,D8,NC,8400), E1(D9,D10,NC,8400), E3(D11,D13,NC,8400); // Encoders of motors 0,1,2
 
 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
+
+// Make arrays for the different variables for the motors
+AnalogIn Shields[4] = {S0, S1, S2, S3};
+DigitalOut MotorDirections[3] = {MD0, MD1, MD2};
+FastPWM MotorVelocities[3] = {MV0, MV1, MV2};
+QEI Encoders[3] = {E0, E1, E3};
+
 MODSERIAL pc(USBTX, USBRX);
-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
+double PI = 3.1415926;//535897932384626433832795028841971693993;
+double timeinterval = 0.001;        // 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
 
 // Define the different states in which the robot can be
 enum States {MOTORS_OFF, EMG_CALIBRATION, MOTOR_CALIBRATION,
-    START_GAME, DEMO_MODE, GAME_MODE, MOVE_END_EFFECTOR,
+    START_GAME, DEMO_MODE, OPERATING_MODE, MOVE_END_EFFECTOR,
     MOVE_GRIPPER, END_GAME, FAILURE_MODE};
 
 // Default state is the state in which the motors are turned off
@@ -43,13 +52,19 @@
 
 // Initialise the functions
 
+double getmeasuredvelocity();
+
+double getreferencevelocity();
+
+void sendtomotor(double motorvalue);
+
+void measureandcontrol();
+
 void motorsoff();
 
-float rms_deque(std::deque<float> deque);
+void measure_data(double &f_y0, double &f_y1, double &f_y2, double &f_y3);
 
-void measure_data(float &rms_0, float &rms_1, float &rms_2, float &rms_3);
-
-void det_max(float rms, float &max_rms);
+void det_max(double y, double &max_y);
 
 void emgcalibration();
 
@@ -59,22 +74,30 @@
     // Do nothing
 }
 
+void get_input(char c);
+
+void startgame() ;
+
 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
+    double frequency = 17000.0;            // Set motorfrequency
+    double period_signal = 1.0/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
+    
+    for (int i = 0; i < 3; i++) {
+        MotorVelocities[i].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()
@@ -90,17 +113,49 @@
     }
     // 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
+    sendtomotor(0.0);                          // Set the motor velocity to 0
     measurecontrol.attach(nothing,10000);       // Attach empty function to Ticker (?? Appropriate solution ??)
     return 0;
 }
+//__________________________________________________________________________________________________________________________________
+//__________________________________________________________________________________________________________________________________
+
+
+//get the measured velocity
+double getmeasuredvelocity()
+{   
+    //iets
+    return 0;
+}
+
+//get the reference of the velocity: positive or negative
+double getreferencevelocity()
+{
+    //iets
+    return 0;
+}   
+
+//send value to motor
+void sendtomotor(double motorvalue)
+{   
+    //iets
+}
+
+// function to call reference velocity, measured velocity and controls motor with feedback
+void measureandcontrol()
+{
+    //iets
+}
+
+
+
 
 void motorsoff() {
     // 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
+    sendtomotor(0.0);              // Set motor velocity to 0
     
     while (whileloop_boolean) {
         if (button1.read() == 0) {          // If button1 is pressed:
@@ -110,84 +165,76 @@
     }
 }
 
-float rms_deque(std::deque<float> deque) {
-    float sum = 0;
-    for (int i = 0; i < deque.size(); i++) {
-        sum = sum + pow(deque[i],2);        // Square the entries of the deque and add them to sum
-    }
-    return pow(sum,0.5f);                   // Return the square root of the sum (and thus the RMS)
-}
-
-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
+void measure_data(double &f_y0, double &f_y1, double &f_y2, double &f_y3) {
+    // High pass
+    double hb0 = 0.9169;  // Coefficients from the following formula:
+    double hb1 = -1.8338; //
+    double hb2 = 0.9169;  //        b0 + b1 z^-1 + b2 z^-2
+    double ha0 = 1.0;     // H(z) = ----------------------
+    double ha1 = -1.8268; //        a0 + a1 z^-1 + a2 z^-2
+    double ha2 = 0.8407;  //
     
-    static float max_rms0 = 0;
-    static float max_rms1 = 0;
-    static float max_rms2 = 0;
-    static float max_rms3 = 0;
+    // Low pass
+    double lb0 = 0.000083621;  // Coefficients from the following formula:
+    double lb1 = 0.0006724; //
+    double lb2 = 0.000083621;  //        b0 + b1 z^-1 + b2 z^-2
+    double la0 = 1.0;     // H(z) = ----------------------
+    double la1 = -1.9740; //        a0 + a1 z^-1 + a2 z^-2
+    double la2 = 0.9743;  //
     
-    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);
+    static double max_y0 = 0;
+    static double max_y1 = 0;
+    static double max_y2 = 0;
+    static double max_y3 = 0;
     
-    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);
+    static BiQuad hFilter0(hb0,hb1,hb2,ha0,ha1,ha2);  // Create 4 equal filters used for the different EMG signals
+    static BiQuad hFilter1(hb0,hb1,hb2,ha0,ha1,ha2);
+    static BiQuad hFilter2(hb0,hb1,hb2,ha0,ha1,ha2);
+    static BiQuad hFilter3(hb0,hb1,hb2,ha0,ha1,ha2);
     
-    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);
+    static BiQuad lFilter0(lb0,lb1,lb2,la0,la1,la2);  // Create 4 equal filters used for the different EMG signals
+    static BiQuad lFilter1(lb0,lb1,lb2,la0,la1,la2);
+    static BiQuad lFilter2(lb0,lb1,lb2,la0,la1,la2);
+    static BiQuad lFilter3(lb0,lb1,lb2,la0,la1,la2);
     
-    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);
+    f_y0 = hFilter0.step(S0);     // Apply filters on the different EMG signals
+    f_y1 = hFilter1.step(S1);
+    f_y2 = hFilter2.step(S2);
+    f_y3 = hFilter3.step(S3);
     
-    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);
-    
+    f_y0 = lFilter0.step(f_y0);
+    f_y1 = lFilter1.step(f_y1);
+    f_y2 = lFilter2.step(f_y2);
+    f_y3 = lFilter3.step(f_y3);
+
+
     if (MyState == EMG_CALIBRATION) {
         
-        det_max(rms_0, max_rms0);       // Determine the maximum RMS value of the EMG signals during the EMG_CALIBRATION state
-        det_max(rms_1, max_rms1);
-        det_max(rms_2, max_rms2);
-        det_max(rms_3, max_rms3);
+        det_max(f_y0, max_y0);       // Determine the maximum RMS value of the EMG signals during the EMG_CALIBRATION state
+        det_max(f_y1, max_y1);
+        det_max(f_y2, max_y2);
+        det_max(f_y3, max_y3);
         
     }
     else if ((int)MyState > 4) {
-        rms_0 = rms_0/max_rms0;     // Normalise the RMS value by dividing by the maximum RMS value
-        rms_1 = rms_1/max_rms1;     // This is done during the states with a value higher than 4, as this is when you start the playable game mode
-        rms_2 = rms_2/max_rms2;
-        rms_3 = rms_3/max_rms3;
+        f_y0 = f_y0/max_y0;     // Normalise the RMS value by dividing by the maximum RMS value
+        f_y1 = f_y1/max_y1;     // This is done during the states with a value higher than 4, as this is when you start the operating mode
+        f_y2 = f_y2/max_y2;
+        f_y3 = f_y3/max_y3;
     }
     
 }
 
-void det_max(float rms, float &max_rms) {
-    max_rms = max_rms < rms ? rms : max_rms; // if max_rms is smaller than rms, set rms as new max_rms, otherwise keep max_rms
+void det_max(double y, double &max_y) {
+    max_y = max_y < y ? y : max_y; // if max_rms is smaller than rms, set rms as new max_rms, otherwise keep max_rms
 }
 
 void emgcalibration() {
-    float rms0, rms1, rms2, rms3;           // RMS values of the different EMG signals
+    double y0, y1, y2, y3;           // RMS values of the different EMG signals
 
-    measure_data(rms0, rms1, rms2, rms3);   // Calculate RMS values
+    measure_data(y0, y1, y2, y3);   // Calculate RMS values
     
-    float duration = 10.0f;                 // Duration of the emgcalibration function, in this case 10 seconds
+    double duration = 10.0;                 // 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
     
@@ -212,6 +259,55 @@
     return u_k;
 }
 
+void get_input(char c)
+{
+    if (c == 'd') {
+        MyState = DEMO_MODE;
+    }
+    else if (c == 'o') {
+        MyState = OPERATING_MODE;
+    }
+    else {
+    pc.printf("'d' or 'o' were not pressed\r\nPress 'd' to start the demo mode and press 'o' to start the operating mode\r\n");
+    get_input(pc.getc());
+    }
+}
+
+void startgame() {
+    pc.printf("Please choose which game you would like to start:\r\n");
+
+    pc.printf("- Press 'd' to start the demo mode\r\n     Demo mode is a mode in which the different movements of the robot are shown.\r\n");
+    pc.printf("     It will show the edges of the space that the end effector is allowed to go to and will also move in straight lines to show that the robot meets the requirements.\r\n");
+    pc.printf("     It will also show how it is able to grab and lift objects which are on the board.\r\n\r\n");
+
+    pc.printf("- Press 'o' to start the operating mode\r\n     The operating mode is a mode in which you can control the robot by flexing the muscles to which the electrodes are attached.\r\n");
+    pc.printf("     You will be able to move the arm and use the gripper to try and grab and lift objects from the board to the container.\r\n\r\n");
+
+    get_input(pc.getc());
+}
+
+/*
+void demo_mode() {
+    
+}
+*/
+
+
+void operating_mode() {
+        double y0,y1,y2,y3;
+        measure_data(y0,y1,y2,y3);
+        double F_Y[4] = {y0, y1, y2, y3};
+        
+        double threshold = 0.5;
+        for (int i = 0; i < 4; i++) {
+            if (F_Y[i] > threshold) {
+                //hey
+            }
+        } 
+    
+}
+
+
 void New_State() {
     switch (MyState)
     {
@@ -231,14 +327,17 @@
         
         case START_GAME :
             pc.printf("State: Start game");
+            startgame();
             break;
         
         case DEMO_MODE :
             pc.printf("State: Demo mode");
+            //demo_mode();
             break;
         
-        case GAME_MODE :
-            pc.printf("State: Game mode");
+        case OPERATING_MODE :
+            pc.printf("State: Operating mode");
+            measurecontrol.attach(operating_mode,timeinterval);
             break;
         
         case MOVE_END_EFFECTOR :
@@ -261,7 +360,7 @@
         
         default :
             pc.printf("Default state: Motors are turned off");
-            sendtomotor(0.0f);
+            sendtomotor(0.0);
             break;
     }
 }