Mart Wijntjes
sdf
Dependencies: mbed QEI HIDScope biquadFilter MODSERIAL FXOS8700Q FastPWM
Diff: Opzet_Eli.cpp
- Revision:
- 2:5730195cf595
- Parent:
- 1:268bf7dbb15c
- Child:
- 3:0c31a4a5d1fe
--- a/Opzet_Eli.cpp Mon Oct 07 08:26:51 2019 +0000
+++ b/Opzet_Eli.cpp Mon Oct 14 12:14:18 2019 +0000
@@ -1,14 +1,17 @@
// Robot states
#include "mbed.h"
#include "HIDScope.h"
+#include "QEI.h"
#include "MODSERIAL.h"
// Define objects
-DigitalOut led1(LED_RED);
-DigitalOut led2(LED_BLUE);
-DigitalOut led3(LED_GREEN);
-DigitalIn ptc1(PTC6); //Button 1
-DigitalIn ptc2(PTA4); //Button 2
+MODSERIAL pc(USBTX,USBRX);
+DigitalOut led_red(LED_RED);
+DigitalOut led_blue(LED_BLUE);
+DigitalOut led_green(LED_GREEN);
+InterruptIn button_Mbed(PTC6); //Button 1 Mbed
+InterruptIn button_1(PTB10); //Button 2 BRS
+InterruptIn button_2(PTB11); // Button 3 BRS
// Functions
enum states{START,KAL_ME,KAL_EMG,MOVE_START,READY_START,DEMO,MOVE,WAIT,OFF};
@@ -16,6 +19,8 @@
states CurrentState = START;
bool StateChanged = true; // this is the initialization of the first state
+// Function START_TO_KAL_ME
+
void StateMachine(void)
{
switch(CurrentState)
@@ -24,37 +29,34 @@
if (StateChanged)
{
pc.printf("Start state, red led is on. If button 1 pressed, go to kal_me state");
- led1 = 0; // Red led is on
- led2 = 1;
- led3 = 1;
+ led_red = 0; // Red led is on
+ led_blue = 1;
+ led_green = 1;
StateChanged = false;
}
- if (ptc1 = 0) // State switches when button pressed
+ if (button_Mbed.mode (PullDown)== false; ) // State switches when button pressed
{
CurrentState = KAL_ME;
StateChanged = true;
+ wait(0.2f);
+
}
break; // end of state START
case KAL_ME:
if (StateChanged)
{
- pc.printf("Calibration ME state, red ld flickers slow.
- while(true) // Red lef flickers slow
- {
- led2 = 1;
- led3 = 1;
- led1 = !led1;
- wait(0.7);
- }
+ pc.printf("Calibration ME state, red ld flickers slow");
+ //FUNCTION Red led flickers slow
- // FUNCTION Move to mechanical stop, include v_motor
+
+ // FUNCTION Move to mechanical stop, include v_motor, t_passed
// FUNCTION Reset encoders
StateChanged = false;
}
- if (v_motor == 0; t_passed > 2) // FUNCTION t_passed, included in v_motor?
+ if (v_motor == 0 && t_passed > 2) // FUNCTION t_passed, included in v_motor?
{
CurrentState = KAL_EMG;
StateChanged = true;
@@ -64,14 +66,9 @@
case KAL_EMG:
if (StateChanged)
{
- while(true) // Red led flickers fast
- {
- led2 = 1;
- led3 = 1;
- led1 = !led1;
- wait(0.2);
- }
- //FUNCTION Measure EMG_max
+ // FUCNTION Red led flickers fast
+
+ //FUNCTION Measure EMG_max, EMG variable meten, t_passed
StateChanged = false;
}
@@ -86,43 +83,57 @@
case MOVE_START:
if(StateChanged)
{
- led1 = 1;
- led2 = 1;
- led3 = 0; // Green led is on
+ led_red = 1;
+ led_blue = 1;
+ led_green = 0; // Green led is on
- // define start position
+ // FUNCTION move to start, t_passed
+ // Define current_position & start_position
+ StateChanged = false;
+ }
- if //position realised and tmie passed
+ if (current_position == start_position && t_passed > 2) // FUNCTIO t_passed
{
- // Stop moving
CurrentState = READY_START;
StateChanged = true;
}
break; // end of state MOVE_START
case READY_START:
- while(true)
+ if (StateChanged)
{
- led1 = 1;
- led2 = 1;
- led3 = 0;
+ led_red = 1;
+ led_blue = 1;
+ led_green = 0; // Green led is on
+
+ StateChanged = false;
}
- if (ptc1 == 0)
+ if (button_1.read() == false) // Button 1
{
CurrentState = DEMO;
StateChanged = true;
+ wait(0.2f);
}
- else if (ptc2 == 0) // or EMG signal >= 20% max
+ else if (button_2.read() == false || EMG > 0.2*EMG_max) // Button 2 or 20% EMG_max
{
CurrentState = MOVE;
StateChanged = true;
+ wait(0.2f);
}
break; // end of state READY_START
case DEMO:
- //perform straight movements in some way, dunno how to make that here yet
- if //position = end and time
+ if (StateChanged)
+ {
+ //FUNCTION Blue led blink fast
+
+ //FUNCTION perform straight movements for demo
+
+ StateChanged = false;
+ }
+
+ if (current_position == end_position && t_passed > 2)
{
CurrentState = MOVE_START;
StateChanged = true;
@@ -130,38 +141,69 @@
break; // end of state DEMO
case MOVE:
- if // no movement for 2 sec after input
+ if (StateChanged)
+ {
+ led_red = 1;
+ led_green = 1;
+ led_blue = 0; //Blue led is on
+
+ // FUNCTION Play the game with EMG signal
+
+ StateChanged = false;
+ }
+
+ if (button_Mbed.read() == false)
+ {
+ CurrentState = OFF;
+ StateChanged = true;
+ wait(0.2f);
+ }
+
+ else if (button_1.read() == false || EMG < 0.2*EMG_max)
{
CurrentState = WAIT;
StateChanged = true;
+ wait(0.2f);
}
- else if // input signal
- {
- // move in direction linked to EMG
- }
- // Iets met dat oranje lichtje maar snap die stap niet helemaal
- break; // end of state MOVE
+ break; // end of state MOVE
case WAIT:
- if (ptc1 == 0)
+ if (StateChanged)
+ {
+ led_red = 0; // Pink led iS on
+ led_blue = 0;
+ led_green = 1
+
+ if (button_Mbed.read() == false) // For five second, too hard
{
- CurrentState = READY_START;
+ CurrentState = OFF;
StateChanged = true;
+ wait(0.2f);
)
- else if (ptc2 == 0) // or EMG>= 20%max
+ else if (button_1.read() == false)
+ {
+ CurrentState = MOVE_START;
+ StateChanged = true;
+ wait(0.2f);
+ }
+ else if (button_2.read() == false || EMG > 0.2*EMG_max)
{
CurrentState = MOVE;
StateChanged = true;
+ wait(0.2f);
}
- else if (ptc1 == 0) //maar dan heel lang achter elkaar
+
break; // end of state WAIT
case OFF:
- // turns all of
+ led_red = 0; // White led is on
+ led_blue = 0;
+ led_green = 0;
+
break; // end of state OFF
default:
- TurnMotorsoff();
+ TurnMotorsoff(); //FUNCTION
printf("Unknown state reached");
} // End of the switch, all states are prescribed
}