Diff: main.cpp

Revision:

6:a02ad75f0333

Parent:

5:07e401cb251d

Child:

7:d4090f334ce2

--- a/main.cpp	Mon Oct 22 16:18:40 2018 +0000
+++ b/main.cpp	Thu Oct 25 13:55:32 2018 +0000
@@ -1,203 +1,348 @@
 #include "mbed.h"
 #include "MODSERIAL.h"
+#include "QEI.h"
+#include "FastPWM.h"
+#include "math.h"
 
+// LED's
 DigitalOut led_red(LED_RED);
 DigitalOut led_green(LED_GREEN);
 DigitalOut led_blue(LED_BLUE);
-DigitalIn button(SW2);
-InterruptIn Fail_button(SW3);
+// Buttons
+DigitalIn button_clbrt(SW2);
+DigitalIn Fail_button(SW3);
+// Modserial
 MODSERIAL pc(USBTX, USBRX);
+// Encoders
+QEI Encoder1(D11, D10, NC, 4200) ;  // Encoder motor 1, (pin 1A, pin 1B, index pin(not used), counts/rev)
+QEI Encoder2(D9, D8, NC, 4200) ;    // Encoder motor 2, (pin 2A, pin 2B, index pin (not used), counts/rev)
+// Motors (direction and PWM)
+DigitalOut directionM1(D4);
+DigitalOut directionM2(D7);
+FastPWM motor1_pwm(D5);
+FastPWM motor2_pwm(D6);
+// Declare timers and Tickers
+Timer timer;                        // Timer for counting time in this state
+Ticker WriteValues;                 // Ticker to show values of velocity to screen
+Ticker StateMachine;
 
+// Set up ProcessStateMachine
 enum states {WAITING, MOTOR_ANGLE_CLBRT, EMG_CLBRT, HOMING, WAITING4SIGNAL, MOVE_W_EMG, MOVE_W_DEMO, FAILURE_MODE};
 states currentState = WAITING;
+bool stateChanged = true;
+volatile bool writeVelocityFlag = false;
+
+
+// Global variables
 char c;
+int counts1; 
+int counts2;
+float theta1;
+float theta2;
+float vel_1;
+float vel_2;
+float theta1_prev = 0.0;
+float theta2_prev = 0.0;
+const float pi = 3.14159265359;
+float tijd = 0.005;
+float time_in_state;
+
+// ----------------------------------------------
+// ------- FUNCTIONS ----------------------------
+// ----------------------------------------------
+
+float ReadEncoder1()            // Read Encoder of motor 1.
+{
+    counts1 = Encoder1.getPulses();              // Counts of outputshaft of motor 1
+    theta1 = (float(counts1)/4200) * 2*pi;       // Angle of outputshaft of motor 1
+    vel_1 = (theta1 - theta1_prev) / tijd;       // Velocity, current angle - previous angle, devided by avarage time between encoder read-outs
+    theta1_prev = theta1;                        // Define theta_prev
+    return vel_1;
+}
+float ReadEncoder2()                // Read encoder of motor 2.
+{
+    counts2 = Encoder2.getPulses();              // Counts of outputshaft of motor 2
+    theta2 = (float(counts2)/4200) * 2*pi;       // Angle of outputshaft of motor 2
+    vel_2 = (theta2 - theta2_prev) / tijd;       // Velocity, current angle - previous angle, devided by avarage time between encoder read-outs
+    theta2_prev = theta2;                        // Define theta_prev
+    return vel_2;
+}
+void MotorAngleCalibrate()                  // Function that drives motor 1 and 2.
+{
+        float U1 = -0.2;            // Negative, so arm goes backwards.
+        float U2 = -0.2;             // Motor 2 is not taken into account yet.
+        
+        motor1_pwm.write(fabs(U1));         // Send PWM values to motor
+        motor2_pwm.write(fabs(U2));
+        
+        directionM1 = U1 > 0.0f;            // Either true or false, determines direction.
+        directionM2 = U2 > 0.0f;
+} 
+// ---------------------------------------------------
+// --------STATEMACHINE------------------------------- 
+// --------------------------------------------------- 
+void ProcessStateMachine(void)
+{
+    switch (currentState)
+    {
+        case WAITING:
+        // Description:
+        // In this state we do nothing, and wait for a command
+        
+        // Actions        
+        pc.printf("WAITING... \r\n");
+        led_red = 0; led_green = 0; led_blue = 0;   // Colouring the led WHITE
+        
+        // State transition logic
+        if (button_clbrt == 0) 
+        {
+            currentState = MOTOR_ANGLE_CLBRT;
+            stateChanged = true;
+            pc.printf("Starting Calibration\n\r");
+        }
+        if (Fail_button == 0)
+        {
+            currentState = FAILURE_MODE;
+            stateChanged = true;
+        }
+        break;
+                
+        case MOTOR_ANGLE_CLBRT:
+        // Description:
+        // In this state the robot moves with low motor PWM to some 
+        // mechanical limit of motion, in order to calibrate the motors.
+         
+        // Actions       
+        led_red = 1; led_green = 0; led_blue = 0;   // Colouring the led TURQUOISE
+        timer.start();                              //Start timer to get time in the state "MOTOR_ANGLE_CLRBRT" 
+        if (stateChanged)
+            {
+                MotorAngleCalibrate();                      // Actuate motor 1 and 2.
+                vel_1 = ReadEncoder1();                     // Get velocity of motor 1    
+                vel_2 = ReadEncoder2();                     // Get velocity of motor 2
+                stateChanged = true;                        // Keep this loop going, until the transition conditions are satisfied.    
+            }
+    
+        // State transition logic
+        time_in_state = timer.read();                       // Determine if this state has run for long enough.
     
-void Failing() {
-    currentState = FAILURE_MODE;
-    wait(1.0f);
+        if(time_in_state > 2.0f && vel_1 < 1.1f && vel_2 < 1.1f)
+            {
+                //pc.printf( "Tijd in deze staat = %f \n\r", time_in_state);           
+                //pc.printf( "Tijd tijdens actions loop (Waarde voor bepalen van snelheid)") = %f \n\r", tijd);
+                pc.printf("Change State, Motor calibration is ended. \n\r");
+                timer.stop();                              // Stop timer for this state
+                timer.reset();                             // Reset timer for this state
+                motor1_pwm.write(fabs(0.0));               // Send PWM values to motor
+                motor2_pwm.write(fabs(0.0));
+                Encoder1.reset();                          // Reset Encoders when arrived at zero-position
+                Encoder2.reset();
+        
+                currentState = EMG_CLBRT;                   // Switch to next state (EMG_CLRBRT).
+                stateChanged = true;                       
+           }
+        if (Fail_button == 0)
+        {
+            currentState = FAILURE_MODE;
+            stateChanged = true;
+        }
+        break; 
+                        
+        case EMG_CLBRT:
+        // In this state the person whom is connected to the robot needs 
+        // to flex his/her muscles as hard as possible, in order to 
+        // measure the maximum EMG-signal, which can be used to scale 
+        // the EMG-filter.
+                
+         pc.printf("EMG calibration \r\n");
+         led_red = 1; led_green = 1; led_blue = 0;   // Colouring the led BLUE
+                
+         // Requirements to move to the next state:
+         // If enough time has passed (5 sec), and the EMG-signal drops below 10%
+         // of the maximum measured value, we move to the Homing state.
+                    
+         wait(5.0f); // time_in_this_state > 5.0f
+         // if moving_average(procd_emg) < 0.1*max_procd_emg_ever
+         // INSERT CALIBRATING
+         currentState = HOMING;
+        if (Fail_button == 0)
+        {
+            currentState = FAILURE_MODE;
+            stateChanged = true;
+        }
+         break;       
+                            
+        case HOMING:
+        // Description:
+        // Robot moves to the home starting configuration
+                    
+        pc.printf("HOMING, moving to the home starting configuration. \r\n");
+        led_red = 0; led_green = 1; led_red = 0;                  // Colouring the led PURPLE
+                
+        // Requirements to move to the next state:
+        // If we are in the right location, within some margin, we move to the Waiting for
+        // signal state.
+                
+        wait(5.0f); // time_in_this_state > 5.0f
+        // if ((fabs(angle_error1) < 0.01f) && (fabs(angle_error2) < 0.01f)) {
+        // INSERT MOVEMENT
+        currentState = WAITING4SIGNAL;
+        if (Fail_button == 0)
+        {
+            currentState = FAILURE_MODE;
+            stateChanged = true;
+        }
+        break;
+                
+        case WAITING4SIGNAL:
+        // Description:
+        // In this state the robot waits for an action to occur.
+                    
+        led_red = 0; led_green = 0; led_blue = 0;                // Colouring the led WHITE
+                
+        // Requirements to move to the next state:
+        // If a certain button is pressed we move to the corresponding 
+        // state (MOVE_W_DEMO, MOVE_W_EMG or SHUTDOWN)
+                
+        // CANNOT USE KEYBOARD. SO LOOK FOR ALTERNATIVE
+        pc.printf("Press d to run the demo, Press e to move with EMG, Press c to re-calibrate. \r\n");
+        c = pc.getc();
+        if (c == 'd') 
+        {
+            currentState = MOVE_W_DEMO;
+        }
+        else if (c == 'e') 
+        {
+            currentState = MOVE_W_EMG;
+        }
+        else if (c == 'c') 
+        {
+        currentState = MOTOR_ANGLE_CLBRT;
+        }
+        
+        if (Fail_button == 0)
+        {
+            currentState = FAILURE_MODE;
+            stateChanged = true;
+        }
+        break;
+                
+        case MOVE_W_DEMO:
+        // Description:
+        // In this state the robot follows a preprogrammed shape, e.g. 
+        // a square.
+                    
+        led_red = 1; led_green = 1; led_blue = 0;   // Colouring the led GREEN
+        pc.printf("MOVE_W_DEMO, Running the demo \r\n");
+                
+        // Requirements to move to the next state:
+        // When the home button or the failure button is pressed, we 
+        // will the move to the corresponding state.
+                
+        // BUILD DEMO MODE
+        // FIND ALTERNATIVE FOR KEYBOARD
+        c = pc.getcNb();
+        if (c == 'h') 
+        {
+        currentState = HOMING;
+        }
+        wait(1.0f);
+        
+        if (Fail_button == 0)
+        {
+            currentState = FAILURE_MODE;
+            stateChanged = true;
+        }
+        break;
+                
+        case MOVE_W_EMG:
+        // Description:
+        // In this state the robot will be controlled by use of 
+        // EMG-signals. 
+                    
+        led_red = 1; led_green = 0; led_blue = 1;   // Colouring the led GREEN
+        pc.printf("MOVE_W_EMG, Moving with use of EMG \r\n");
+                
+        // Requirements to move to the next state:
+        // When the home button or the failure button is pressed, we 
+        // will the move to the corresponding state.
+                
+        // BUILD THE MOVEMENT WITH EMG
+        wait(1);
+        c = pc.getcNb();
+        if (c == 'h') 
+        {
+        currentState = HOMING;
+        }
+        wait(1.0f);
+        
+        if (Fail_button == 0)
+        {
+            currentState = FAILURE_MODE;
+            stateChanged = true;
+        }
+        break;      
+                 
+        case FAILURE_MODE:
+        // Description:
+        // This state is reached when the failure button is reached. 
+        // In this state everything is turned off.
+            
+        led_red = 0; led_green = 1; led_blue = 1;   // Colouring the led RED
+        // Actions
+        if (stateChanged)
+        {
+            motor1_pwm.write(fabs(0.0));               // Stop all motors!
+            motor2_pwm.write(fabs(0.0));
+            pc.printf("FAILURE MODE \r\n PLEASE RESTART THE WHOLE ROBOT \r\n (and make sure this does not happen again) \r\n");
+            stateChanged = false;
+        }    
+        break;
+        
+        // State transition logic
+        // No state transition, you need to restart the robot.
+                        
+        default: 
+        // This state is a default state, this state is reached when 
+        // the program somehow defies all of the other states. 
+                
+        pc.printf("Unknown or unimplemented state reached!!! \n\r");
+        led_red = 1; led_green = 1; led_blue = 1;   // Colouring the led BLACK
+        for (int n = 0; n < 50; n++)                // Making an SOS signal with the RED led
+        {              
+            for (int i = 0; i < 6; i++) 
+            { 
+                led_red = !led_red;
+                wait(0.6f);
+            }
+            wait(0.4f);
+            for (int i = 0 ; i < 6; i++) 
+            {
+            led_red = !led_red;
+            wait(0.2f);
+            }
+            wait(0.4f);
+        }
     }
-
+}
+            
+ // --------------------------------  
+ // ----- MAIN LOOP ----------------
+ // --------------------------------  
+    
 int main()
 {
     // Switch all LEDs off
     led_red = 1;
     led_green = 1;
     led_blue = 1;
+    
     pc.baud(115200);
-    Fail_button.fall(&Failing);    
-
-    pc.printf("\r\n______________ STATE MACHINE ______________ \r\n\r\n");
-    
+    StateMachine.attach(&ProcessStateMachine, 0.005f);   // Run statemachine 200 times per second
+  
     while (true) {
-        switch (currentState)
-        {
-            case WAITING:
-                // Description:
-                // In this state we do nothing
-                
-                pc.printf("WAITING... \r\n");
-                led_red = 0; led_green = 0; led_blue = 0;   // Colouring the led WHITE
-                if (!button) {
-                    currentState = MOTOR_ANGLE_CLBRT;
-                    }
-                wait(1.0f);
-                break;
-                
-            case MOTOR_ANGLE_CLBRT:
-                // Description:
-                    // In this state the robot moves with low motor PWM to some 
-                    // mechanical limit of motion.
-                
-                pc.printf("Motor angle calibration \r\n");
-                led_red = 1; led_green = 0; led_blue = 0;   // Colouring the led TURQUOISE
-                
-                // Requirements to move to the next state:
-                    // If enough time has passed in this state (3 sec) and the 
-                    // motors stopped moving, we move to the EMG-calibration state.
-                
-                wait(3.0f);                                 // time_in_this_state > 3.0f
-                // if (fabs(motor_velocity) < 0.01f) {
-                // INSERT CALIBRATING
-                currentState = EMG_CLBRT;
-                break;
-                
-            case EMG_CLBRT:
-                // In this state the person whom is connected to the robot needs 
-                    // to flex his/her muscles as hard as possible, in order to 
-                    // measure the maximum EMG-signal, which can be used to scale 
-                    // the EMG-filter.
-                
-                pc.printf("EMG calibration \r\n");
-                led_red = 1; led_green = 1; led_blue = 0;   // Colouring the led BLUE
-                
-                // Requirements to move to the next state:
-                    // If enough time has passed (5 sec), and the EMG-signal drops below 10%
-                    // of the maximum measured value, we move to the Homing state.
-                    
-                wait(5.0f); // time_in_this_state > 5.0f
-                // if moving_average(procd_emg) < 0.1*max_procd_emg_ever
-                // INSERT CALIBRATING
-                currentState = HOMING;
-                break;       
-                            
-            case HOMING:
-                // Description:
-                    // Robot moves to the home starting configuration
-                    
-                pc.printf("HOMING, moving to the home starting configuration. \r\n");
-                led_red = 0; led_green = 1; led_red = 0;                  // Colouring the led PURPLE
-                
-                // Requirements to move to the next state:
-                    // If we are in the right location, within some margin, we move to the Waiting for
-                    // signal state.
-                
-                wait(5.0f); // time_in_this_state > 5.0f
-                // if ((fabs(angle_error1) < 0.01f) && (fabs(angle_error2) < 0.01f)) {
-                // INSERT MOVEMENT
-                currentState = WAITING4SIGNAL;
-                break;
-                
-            case WAITING4SIGNAL:
-                // Description:
-                    // In this state the robot waits for an action to occur.
-                    
-                led_red = 0; led_green = 0; led_blue = 0;                // Colouring the led WHITE
-                
-                // Requirements to move to the next state:
-                    // If a certain button is pressed we move to the corresponding 
-                    // state (MOVE_W_DEMO, MOVE_W_EMG or SHUTDOWN)
-                
-                // CANNOT USE KEYBOARD. SO LOOK FOR ALTERNATIVE
-                pc.printf("Press d to run the demo, Press e to move with EMG, Press c to re-calibrate. \r\n");
-                c = pc.getc();
-                if (c == 'd') {
-                    currentState = MOVE_W_DEMO;
-                    }
-                else if (c == 'e') {
-                    currentState = MOVE_W_EMG;
-                    }
-                else if (c == 'c') {
-                    currentState = MOTOR_ANGLE_CLBRT;
-                    }
-  
-                break;
-                
-                
-            case MOVE_W_DEMO:
-                // Description:
-                    // In this state the robot follows a preprogrammed shape, e.g. 
-                    // a square.
-                    
-                led_red = 1; led_green = 1; led_blue = 0;   // Colouring the led GREEN
-                pc.printf("MOVE_W_DEMO, Running the demo \r\n");
-                
-                // Requirements to move to the next state:
-                    // When the home button or the failure button is pressed, we 
-                    // will the move to the corresponding state.
-                
-                // BUILD DEMO MODE
-                // FIND ALTERNATIVE FOR KEYBOARD
-                c = pc.getcNb();
-                if (c == 'h') {
-                    currentState = HOMING;
-                    }
-                Fail_button.fall(&Failing); 
-                wait(1.0f);
-                break;
-                
-            case MOVE_W_EMG:
-                // Description:
-                    // In this state the robot will be controlled by use of 
-                    // EMG-signals. 
-                    
-                led_red = 1; led_green = 0; led_blue = 1;   // Colouring the led GREEN
-                pc.printf("MOVE_W_EMG, Moving with use of EMG \r\n");
-                
-                // Requirements to move to the next state:
-                    // When the home button or the failure button is pressed, we 
-                    // will the move to the corresponding state.
-                
-                // BUILD THE MOVEMENT WITH EMG
-                wait(1);
-                c = pc.getcNb();
-                if (c == 'h') {
-                    currentState = HOMING;
-                    }
-                Fail_button.fall(&Failing); 
-                wait(1.0f);
-                break;      
-                 
-            case FAILURE_MODE:
-                // Description:
-                // This state is reached when the failure button is reached. 
-                // In this state everything is turned off.
+        
+    }
+}
+   
             
-                led_red = 0; led_green = 1; led_blue = 1;   // Colouring the led RED
-                // WHAT NEEDS TO HAPPEN IN FAILURE MODE?
-                pc.printf("FAILURE MODE \r\n");
-                wait(2.0f);
-                break;
-
-                        
-            default: 
-                // This state is a default state, this state is reached when 
-                // the program somehow defies all of the other states. 
-                
-                pc.printf("Unknown or unimplemented state reached!!! \n\r");
-                led_red = 1; led_green = 1; led_blue = 1;   // Colouring the led BLACK
-                for (int n = 0; n < 50; n++) {              // Making an SOS signal with the RED led
-                    for (int i = 0; i < 6; i++) { 
-                        led_red = !led_red;
-                        wait(0.6f);
-                        }
-                    wait(0.4f);
-                    for (int i = 0 ; i < 6; i++) {
-                        led_red = !led_red;
-                        wait(0.2f);
-                        }
-                    wait(0.4f);
-                }
-        }   // end of switch
-    }
-}   
-