Onno Derkman / Mbed OS statemachineopzet

statemachine opzet

Dependencies:   MODSERIAL

main.cpp@0:64c100cd152a, 2018-10-31 (annotated)

Committer:
onnoderkman
Date:
Wed Oct 31 10:47:46 2018 +0000
Revision:
0:64c100cd152a
State machine opzet. Overgangen van state naar state zijn nog niet compleet.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
onnoderkman 0:64c100cd152a 1 #include "mbed.h"
onnoderkman 0:64c100cd152a 2 #include "MODSERIAL.h"
onnoderkman 0:64c100cd152a 3
onnoderkman 0:64c100cd152a 4 DigitalOut ledr(LED1);
onnoderkman 0:64c100cd152a 5 DigitalOut ledg(LED2);
onnoderkman 0:64c100cd152a 6 DigitalOut ledb(LED3);
onnoderkman 0:64c100cd152a 7 DigitalIn button1(SW2);
onnoderkman 0:64c100cd152a 8 DigitalIn button2(SW3);
onnoderkman 0:64c100cd152a 9 MODSERIAL pc(USBTX, USBRX);
onnoderkman 0:64c100cd152a 10 enum states{WAITING, EMG_CAL, BCM, FAIL};
onnoderkman 0:64c100cd152a 11 states CurrentState = WAITING;
onnoderkman 0:64c100cd152a 12 bool StateChanged = true;
onnoderkman 0:64c100cd152a 13 volatile int x = 0;
onnoderkman 0:64c100cd152a 14 bool failure = false;
onnoderkman 0:64c100cd152a 15
onnoderkman 0:64c100cd152a 16 void ProcessStateMachine(void)
onnoderkman 0:64c100cd152a 17 {
onnoderkman 0:64c100cd152a 18 switch(CurrentState)
onnoderkman 0:64c100cd152a 19 {
onnoderkman 0:64c100cd152a 20 case WAITING: //This state is used to only start calibrating upon user input
onnoderkman 0:64c100cd152a 21 //State initialization
onnoderkman 0:64c100cd152a 22 if(StateChanged)
onnoderkman 0:64c100cd152a 23 {
onnoderkman 0:64c100cd152a 24 pc.printf("Entering WAITING\r\n");
onnoderkman 0:64c100cd152a 25 ledr = 0;
onnoderkman 0:64c100cd152a 26 ledg = 1;
onnoderkman 0:64c100cd152a 27 ledb = 1;
onnoderkman 0:64c100cd152a 28 StateChanged = false;
onnoderkman 0:64c100cd152a 29 }
onnoderkman 0:64c100cd152a 30 //State actions
onnoderkman 0:64c100cd152a 31 pc.printf("Still WAITING\r\n");
onnoderkman 0:64c100cd152a 32 while(button1)
onnoderkman 0:64c100cd152a 33 {
onnoderkman 0:64c100cd152a 34 }
onnoderkman 0:64c100cd152a 35 //State transition
onnoderkman 0:64c100cd152a 36 CurrentState = EMG_CAL;
onnoderkman 0:64c100cd152a 37 StateChanged = true;
onnoderkman 0:64c100cd152a 38 break; // End of WAITING
onnoderkman 0:64c100cd152a 39 case EMG_CAL: //State for calibrating EMG channels
onnoderkman 0:64c100cd152a 40 //State initialization
onnoderkman 0:64c100cd152a 41 if(StateChanged)
onnoderkman 0:64c100cd152a 42 {
onnoderkman 0:64c100cd152a 43 pc.printf("Entering EMG calibration\r\n");
onnoderkman 0:64c100cd152a 44 x = 0;
onnoderkman 0:64c100cd152a 45 ledr = 1;
onnoderkman 0:64c100cd152a 46 ledg = 0;
onnoderkman 0:64c100cd152a 47 ledb = 1;
onnoderkman 0:64c100cd152a 48 StateChanged = false;
onnoderkman 0:64c100cd152a 49 }
onnoderkman 0:64c100cd152a 50 //State actions
onnoderkman 0:64c100cd152a 51 pc.printf("EMG calibration starting\r\n");
onnoderkman 0:64c100cd152a 52 wait(0.3f); //Dit moet nog veranderd worden naar timer die opneemt hoe lang hij in WAITING zit.
onnoderkman 0:64c100cd152a 53 // calibratiestappen voor de emg hier invoegen
onnoderkman 0:64c100cd152a 54 while(x < 4 && !failure)
onnoderkman 0:64c100cd152a 55 {
onnoderkman 0:64c100cd152a 56 if(x == 1)
onnoderkman 0:64c100cd152a 57 {
onnoderkman 0:64c100cd152a 58 //Functie die het eerste EMG signaal calibreert
onnoderkman 0:64c100cd152a 59 }
onnoderkman 0:64c100cd152a 60 if(x == 2)
onnoderkman 0:64c100cd152a 61 {
onnoderkman 0:64c100cd152a 62 //Functie die het tweede EMG signaal calibreert
onnoderkman 0:64c100cd152a 63 }
onnoderkman 0:64c100cd152a 64 if(x == 3)
onnoderkman 0:64c100cd152a 65 {
onnoderkman 0:64c100cd152a 66 //Functie die het derde EMG signaal calibreert
onnoderkman 0:64c100cd152a 67 }
onnoderkman 0:64c100cd152a 68 if(!button2)
onnoderkman 0:64c100cd152a 69 {
onnoderkman 0:64c100cd152a 70 failure = true;
onnoderkman 0:64c100cd152a 71 }
onnoderkman 0:64c100cd152a 72 if(!button1)
onnoderkman 0:64c100cd152a 73 {
onnoderkman 0:64c100cd152a 74 x++;
onnoderkman 0:64c100cd152a 75 pc.printf("EMG calibration %i is now running\r\n", x);
onnoderkman 0:64c100cd152a 76 wait(0.2f);
onnoderkman 0:64c100cd152a 77 }
onnoderkman 0:64c100cd152a 78 }
onnoderkman 0:64c100cd152a 79 //State transition
onnoderkman 0:64c100cd152a 80 if(failure)
onnoderkman 0:64c100cd152a 81 {
onnoderkman 0:64c100cd152a 82 CurrentState = FAIL;
onnoderkman 0:64c100cd152a 83 StateChanged = true;
onnoderkman 0:64c100cd152a 84 }
onnoderkman 0:64c100cd152a 85 else
onnoderkman 0:64c100cd152a 86 {
onnoderkman 0:64c100cd152a 87 CurrentState = BCM;
onnoderkman 0:64c100cd152a 88 StateChanged = true;
onnoderkman 0:64c100cd152a 89 }
onnoderkman 0:64c100cd152a 90 break; //End of EMG_CAL state
onnoderkman 0:64c100cd152a 91 case BCM: //State for moving the robot
onnoderkman 0:64c100cd152a 92 //State initialization
onnoderkman 0:64c100cd152a 93 if(StateChanged)
onnoderkman 0:64c100cd152a 94 {
onnoderkman 0:64c100cd152a 95 pc.printf("Entering Basic Control Mode\r\n");
onnoderkman 0:64c100cd152a 96 ledr = 1;
onnoderkman 0:64c100cd152a 97 ledg = 1;
onnoderkman 0:64c100cd152a 98 ledb = 0;
onnoderkman 0:64c100cd152a 99 StateChanged = false;
onnoderkman 0:64c100cd152a 100 }
onnoderkman 0:64c100cd152a 101 //State actions
onnoderkman 0:64c100cd152a 102 pc.printf("Basic Control Mode is active\r\n");
onnoderkman 0:64c100cd152a 103 while(button1 && !failure)
onnoderkman 0:64c100cd152a 104 {
onnoderkman 0:64c100cd152a 105 if(!button2)
onnoderkman 0:64c100cd152a 106 {
onnoderkman 0:64c100cd152a 107 failure = true;
onnoderkman 0:64c100cd152a 108 }
onnoderkman 0:64c100cd152a 109 }
onnoderkman 0:64c100cd152a 110 //State transition
onnoderkman 0:64c100cd152a 111 if(failure)
onnoderkman 0:64c100cd152a 112 {
onnoderkman 0:64c100cd152a 113 pc.printf("Go to fail mode\r\n");
onnoderkman 0:64c100cd152a 114 CurrentState = FAIL;
onnoderkman 0:64c100cd152a 115 StateChanged = true;
onnoderkman 0:64c100cd152a 116 }
onnoderkman 0:64c100cd152a 117 else
onnoderkman 0:64c100cd152a 118 {
onnoderkman 0:64c100cd152a 119 CurrentState = WAITING;
onnoderkman 0:64c100cd152a 120 StateChanged = true;
onnoderkman 0:64c100cd152a 121 }
onnoderkman 0:64c100cd152a 122 break; //End of BCM state
onnoderkman 0:64c100cd152a 123 case FAIL: //Fail state
onnoderkman 0:64c100cd152a 124 //State initialization
onnoderkman 0:64c100cd152a 125 if(StateChanged)
onnoderkman 0:64c100cd152a 126 {
onnoderkman 0:64c100cd152a 127 pc.printf("Fail mode is entered\r\n");
onnoderkman 0:64c100cd152a 128 failure = false;
onnoderkman 0:64c100cd152a 129 StateChanged = false;
onnoderkman 0:64c100cd152a 130 }
onnoderkman 0:64c100cd152a 131 //State actions
onnoderkman 0:64c100cd152a 132 pc.printf("Fail mode is active. Press button1 to enter WAITING\r\n");
onnoderkman 0:64c100cd152a 133 while(button1)
onnoderkman 0:64c100cd152a 134 {
onnoderkman 0:64c100cd152a 135 }
onnoderkman 0:64c100cd152a 136 //State transition
onnoderkman 0:64c100cd152a 137 CurrentState = WAITING;
onnoderkman 0:64c100cd152a 138 StateChanged = true;
onnoderkman 0:64c100cd152a 139 break; //End of FAIL state
onnoderkman 0:64c100cd152a 140 default: //Emergency state for when something unexpected happens
onnoderkman 0:64c100cd152a 141 //motoren uit enzo
onnoderkman 0:64c100cd152a 142 pc.printf("SOS SOS SOS\r\n");
onnoderkman 0:64c100cd152a 143 ledr = 0;
onnoderkman 0:64c100cd152a 144 ledg = 0;
onnoderkman 0:64c100cd152a 145 ledb = 0;
onnoderkman 0:64c100cd152a 146 while(button1)
onnoderkman 0:64c100cd152a 147 {
onnoderkman 0:64c100cd152a 148 ledr = !ledr;
onnoderkman 0:64c100cd152a 149 wait(0.3f);
onnoderkman 0:64c100cd152a 150 }
onnoderkman 0:64c100cd152a 151 CurrentState = WAITING;
onnoderkman 0:64c100cd152a 152 StateChanged = true;
onnoderkman 0:64c100cd152a 153 }
onnoderkman 0:64c100cd152a 154 }
onnoderkman 0:64c100cd152a 155
onnoderkman 0:64c100cd152a 156 int main()
onnoderkman 0:64c100cd152a 157 {
onnoderkman 0:64c100cd152a 158 pc.baud(115200);
onnoderkman 0:64c100cd152a 159 ledr = 1;
onnoderkman 0:64c100cd152a 160 ledg = 1;
onnoderkman 0:64c100cd152a 161 ledb = 1;
onnoderkman 0:64c100cd152a 162 while (true)
onnoderkman 0:64c100cd152a 163 {
onnoderkman 0:64c100cd152a 164 ProcessStateMachine(); //Activates state machine
onnoderkman 0:64c100cd152a 165 }
onnoderkman 0:64c100cd152a 166 }