Diff: OpzetjeBrechje.cpp

Revision:

8:2fc7a3a7f09a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/OpzetjeBrechje.cpp	Thu Oct 03 12:58:44 2019 +0000
@@ -0,0 +1,191 @@
+#include "mbed.h"
+//#include "HIDScope.h"
+//#include "QEI.h"
+#include "MODSERIAL.h"
+//#include "BiQuad.h"
+//#include "FastPWM.h"
+
+MODSERIAL pc(USBTX, USBRX);
+
+//TIMER
+timer
+
+//MOTORSPEED
+Motorspeed
+
+//LED
+DigitalOut GreenLED(LED1);
+DigitalOut RedLED(LED2);
+
+//Buttons
+Button1 //to start ME calibration
+Button2
+
+// declare a new variable type (I called it states)
+enum states {STATE_START, STATE_MECALIBRATION,STATE_EMGCALIBRATION,STATE_MOVETOSTART, STATE_READYTOSTART, STATE_DEMO, STATE_MOVE, STATE_WAIT, STATE_OFF};
+
+// create a variable (of type ‘states’) called ‘mystate', initialize it
+states mystate = STATE_START;
+bool stateChanged = true; // Make sure the initialization of first state is executed
+
+void ProcessStateMachine(void) 
+{
+case STATE_START : // Start Robot
+if (stateChanged) //
+{   printf("Robot is on, ready to play. Press button to calibrate");
+    RedLED = 1 ;  //Turn red led on
+    stateChanged = false; 
+    }
+//State Me Calibration
+if (Button1.Pressed())
+{currentState = STATE_MECALIBRATION;
+stateChanged = true;
+}
+break; // end of STATE_START
+
+case STATE_MECALIBRATION:
+if (stateChanged) //
+{   printf("Robot is calibrating ME");
+    RedLED = SLOW BLINK ;  //Blinking SLOW RedLED 
+    //actions of ME CALIBRATION
+    Move motor to Mechanical stop
+    Stop motor
+    Start timer 
+        stateChanged = false; 
+    }
+
+//State EMG calibration
+if (Motorspeed = 0 && timer t > 2)
+{currentState = STATE_EMGCALIBRATION;
+stateChanged = true;
+}
+break; // end of STATE_MECALIBRATION
+
+case STATE_EMGCALIBRATION:
+if (stateChanged) //
+{   printf("Robot is calibrating EMG");
+    RedLED = FAST BLINK ;  //Blinking FAST RedLED 
+    //actions of EMG CALIBRATION
+    Calculating EMG_max
+    Calibrating
+    Measuring current EMG_signal
+        stateChanged = false; 
+    }
+
+//State EMG calibration
+if (EMG_signal < 10% of EMG_max && timer t > 2)
+{currentState = STATE_MOVETOSTART;
+stateChanged = true;
+}
+break; // end of STATE_EMGCALIBRATION
+
+
+case STATE_MOVETOSTART : 
+if (stateChanged) //
+{   printf("Robot is moving to start position");
+    GreenLED = BLINKING FAST ;  //Blinking FastRedLED
+    //actions of Robot moving to start 
+        stateChanged = false; 
+    }
+//State Ready to start
+if (Position is x,y,z ? && timer t > 2) //hoe gaan we dit bepalen?
+{currentState = STATE_READYTOSTART;
+stateChanged = true;
+}
+break; // end of STATE_MOVETOSTART
+
+case STATE_READYTOSTART : 
+if (stateChanged) //
+{   printf("Robot is ready to start, press button 1 for starting DEMO or press button 2 or contract muscle with EMG electrode to start Game Mode ");
+    GreenLED = ON ;  //GreenLed is on
+    //Wait for input user
+        stateChanged = false; 
+    }
+//State Ready to start
+if (Button2.pressed() && timer t > 2 or EMG_signal > 20% EMG_max)
+{currentState = STATE_MOVE;
+stateChanged = true;
+}
+else if (Button1.pressed())
+{currentState = STATE_DEMO;
+stateChanged = true;
+}
+break; // end of STATE_READYTOSTART
+
+case STATE_DEMO : 
+if (stateChanged) //
+{   printf("Robot is performing DEMO");
+    //actions of DEMO
+    stateChanged = false; 
+    }
+
+//State Move to Start
+if ( Position is x,y,z ? && timer t > 2)
+{currentState = STATE_MOVETOSTART;
+stateChanged = true;
+}
+break; // end of State DEMO
+
+
+case STATE_MOVE : 
+if (stateChanged) //
+{   printf("Time to play!");
+    //actions of game mode
+    stateChanged = false; 
+    }
+
+//State Wait
+if (EMG_signal < 20% EMG_max for t > 5)
+{currentState = STATE_WAIT;
+stateChanged = true;
+}
+break; // end of State MOVE
+
+case STATE_WAIT : 
+if (stateChanged) //
+{   printf("Robot is waiting for your actions. Press button 2 or contract muscle to continue playing. Press button 1 to Restart. Hold button 1 pressed to shut down the game");
+    //Waiting for input actions
+    stateChanged = false; 
+    }
+
+//State Move
+if (Button2.pressed() or EMG_signal > 20% EMG_max)
+{currentState = STATE_MOVE;
+stateChanged = true;
+}
+
+else if (Button1.pressed())
+{currentState = STATE_MOVETOSTART;
+stateChanged = true;
+}
+
+else if (Button1.pressed() for timer t > 5) 
+{currentState = STATE_OFF;
+stateChanged = true;
+}
+break; // end of State WAIT
+
+case STATE_OFF:
+if (stateChanged) //
+{   printf("No Signal. Restart by putting out and back in USB cable"
+  stateChanged = false; 
+    }
+
+
+
+default:
+TurnMotorsOff(); // Safety is important!!
+pc.printf(“Unknown or unimplemented state reached!\n”); 
+
+} //end of switch
+
+
+
+
+
+
+int main()
+{
+    
+    }
+}