Gaston Gabriël / Mbed 2 deprecated StateMachine_EMg_RKI_PID_MOTORwerkend_10milli

This is with minumum of 10 milliseconds threshold delay 00:53

Dependencies:   biquadFilter MODSERIAL QEI Servo mbed

Fork of StateMachine_EMg_RKI_PID_MOTOR_metklikenalles by Gaston Gabriël

main.cpp@0:90750f158475, 2018-10-31 (annotated)

Committer:
aschut
Date:
Wed Oct 31 14:15:54 2018 +0000
Revision:
0:90750f158475
Child:
1:070092564648
werkt met lampjes

Who changed what in which revision?

UserRevisionLine numberNew contents of line
aschut 0:90750f158475 1 //Voor het toevoegen van een button:
aschut 0:90750f158475 2 #include "mbed.h"
aschut 0:90750f158475 3 #include <iostream>
aschut 0:90750f158475 4 DigitalOut gpo(D0);
aschut 0:90750f158475 5
aschut 0:90750f158475 6 DigitalIn button2(SW3);
aschut 0:90750f158475 7 DigitalIn button1(SW2); //or SW2
aschut 0:90750f158475 8
aschut 0:90750f158475 9 DigitalOut led1(LED_GREEN);
aschut 0:90750f158475 10 DigitalOut led2(LED_RED);
aschut 0:90750f158475 11 DigitalOut led3(LED_BLUE);
aschut 0:90750f158475 12
aschut 0:90750f158475 13 Timer t;
aschut 0:90750f158475 14
aschut 0:90750f158475 15 enum states {MOTORS_OFF,CALIBRATION,HOMING,DEMO,MOVEMENT,CLICK};
aschut 0:90750f158475 16 int f = 1;
aschut 0:90750f158475 17 states currentState = MOTORS_OFF;
aschut 0:90750f158475 18 bool stateChanged = true; // Make sure the initialization of first state is executed
aschut 0:90750f158475 19
aschut 0:90750f158475 20 void ProcessStateMachine(void)
aschut 0:90750f158475 21 {
aschut 0:90750f158475 22 switch (currentState)
aschut 0:90750f158475 23 {
aschut 0:90750f158475 24 case MOTORS_OFF:
aschut 0:90750f158475 25 // Actions
aschut 0:90750f158475 26 if (stateChanged)
aschut 0:90750f158475 27 {
aschut 0:90750f158475 28 // state initialization: rood
aschut 0:90750f158475 29 led1 = 1;
aschut 0:90750f158475 30 led2 = 0;
aschut 0:90750f158475 31 led3 = 1;
aschut 0:90750f158475 32 wait (1);
aschut 0:90750f158475 33 stateChanged = false;
aschut 0:90750f158475 34 }
aschut 0:90750f158475 35
aschut 0:90750f158475 36 // State transition logic: Als button 1 word ingedrukt --> calibratie, anders motor uithouden
aschut 0:90750f158475 37 if (!button1)
aschut 0:90750f158475 38 {
aschut 0:90750f158475 39 currentState = CALIBRATION;
aschut 0:90750f158475 40 stateChanged = true;
aschut 0:90750f158475 41 }
aschut 0:90750f158475 42 else if (!button2)
aschut 0:90750f158475 43 {
aschut 0:90750f158475 44 currentState = HOMING ;
aschut 0:90750f158475 45 stateChanged = true;
aschut 0:90750f158475 46 }
aschut 0:90750f158475 47 else
aschut 0:90750f158475 48 {
aschut 0:90750f158475 49 currentState = MOTORS_OFF;
aschut 0:90750f158475 50 stateChanged = true;
aschut 0:90750f158475 51 }
aschut 0:90750f158475 52
aschut 0:90750f158475 53 break;
aschut 0:90750f158475 54
aschut 0:90750f158475 55 case CALIBRATION:
aschut 0:90750f158475 56 // Actions
aschut 0:90750f158475 57 if (stateChanged)
aschut 0:90750f158475 58 {
aschut 0:90750f158475 59 // state initialization: oranje
aschut 0:90750f158475 60 led1 = 0;
aschut 0:90750f158475 61 led2 = 0;
aschut 0:90750f158475 62 led3 = 1;
aschut 0:90750f158475 63 wait (1);
aschut 0:90750f158475 64
aschut 0:90750f158475 65 stateChanged = false;
aschut 0:90750f158475 66 }
aschut 0:90750f158475 67
aschut 0:90750f158475 68 // State transition logic: automatisch terug naar motors off.
aschut 0:90750f158475 69
aschut 0:90750f158475 70 currentState = MOTORS_OFF;
aschut 0:90750f158475 71 stateChanged = true;
aschut 0:90750f158475 72 break;
aschut 0:90750f158475 73
aschut 0:90750f158475 74 case HOMING:
aschut 0:90750f158475 75 // Actions
aschut 0:90750f158475 76 if (stateChanged)
aschut 0:90750f158475 77 {
aschut 0:90750f158475 78 // state initialization: green
aschut 0:90750f158475 79 t.start();
aschut 0:90750f158475 80 led1 = 0;
aschut 0:90750f158475 81 led2 = 1;
aschut 0:90750f158475 82 led3 = 1;
aschut 0:90750f158475 83 wait (1);
aschut 0:90750f158475 84
aschut 0:90750f158475 85 stateChanged = false;
aschut 0:90750f158475 86 }
aschut 0:90750f158475 87
aschut 0:90750f158475 88 // State transition logic: naar DEMO (button1), naar MOVEMENT(button2)
aschut 0:90750f158475 89 if (!button1)
aschut 0:90750f158475 90 {
aschut 0:90750f158475 91 currentState = DEMO;
aschut 0:90750f158475 92 stateChanged = true;
aschut 0:90750f158475 93 }
aschut 0:90750f158475 94 else if (!button2)
aschut 0:90750f158475 95 {
aschut 0:90750f158475 96 currentState = MOVEMENT ;
aschut 0:90750f158475 97 stateChanged = true;
aschut 0:90750f158475 98 }
aschut 0:90750f158475 99 else if (t>300)
aschut 0:90750f158475 100 {
aschut 0:90750f158475 101 t.stop();
aschut 0:90750f158475 102 t.reset();
aschut 0:90750f158475 103 currentState = MOTORS_OFF ;
aschut 0:90750f158475 104 stateChanged = true;
aschut 0:90750f158475 105 }
aschut 0:90750f158475 106 else
aschut 0:90750f158475 107 {
aschut 0:90750f158475 108 currentState = HOMING ;
aschut 0:90750f158475 109 stateChanged = true;
aschut 0:90750f158475 110 }
aschut 0:90750f158475 111 break;
aschut 0:90750f158475 112
aschut 0:90750f158475 113 case DEMO:
aschut 0:90750f158475 114 // Actions
aschut 0:90750f158475 115 if (stateChanged)
aschut 0:90750f158475 116 {
aschut 0:90750f158475 117 // state initialization: light blue
aschut 0:90750f158475 118 led1 = 0;
aschut 0:90750f158475 119 led2 = 1;
aschut 0:90750f158475 120 led3 = 0;
aschut 0:90750f158475 121 wait (1);
aschut 0:90750f158475 122
aschut 0:90750f158475 123 stateChanged = false;
aschut 0:90750f158475 124 }
aschut 0:90750f158475 125
aschut 0:90750f158475 126 // State transition logic: automatisch terug naar HOMING
aschut 0:90750f158475 127 currentState = HOMING;
aschut 0:90750f158475 128 stateChanged = true;
aschut 0:90750f158475 129 break;
aschut 0:90750f158475 130
aschut 0:90750f158475 131 case MOVEMENT:
aschut 0:90750f158475 132 // Actions
aschut 0:90750f158475 133 if (stateChanged)
aschut 0:90750f158475 134 {
aschut 0:90750f158475 135 // state initialization: purple
aschut 0:90750f158475 136 t.start();
aschut 0:90750f158475 137 led1 = 1;
aschut 0:90750f158475 138 led2 = 0;
aschut 0:90750f158475 139 led3 = 0;
aschut 0:90750f158475 140 wait (1);
aschut 0:90750f158475 141
aschut 0:90750f158475 142 stateChanged = false;
aschut 0:90750f158475 143 }
aschut 0:90750f158475 144
aschut 0:90750f158475 145 // State transition logic: naar CLICK (button1), naar MOTORS_OFF(button2) anders naar MOVEMENT
aschut 0:90750f158475 146 if (!button1)
aschut 0:90750f158475 147 {
aschut 0:90750f158475 148 currentState = CLICK;
aschut 0:90750f158475 149 stateChanged = true;
aschut 0:90750f158475 150 }
aschut 0:90750f158475 151 else if (!button2)
aschut 0:90750f158475 152 {
aschut 0:90750f158475 153 currentState = MOTORS_OFF ;
aschut 0:90750f158475 154 stateChanged = true;
aschut 0:90750f158475 155 }
aschut 0:90750f158475 156 else if (t>300)
aschut 0:90750f158475 157 {
aschut 0:90750f158475 158 t.stop();
aschut 0:90750f158475 159 t.reset();
aschut 0:90750f158475 160 currentState = HOMING ;
aschut 0:90750f158475 161 stateChanged = true;
aschut 0:90750f158475 162 }
aschut 0:90750f158475 163 else
aschut 0:90750f158475 164 {
aschut 0:90750f158475 165 currentState = MOVEMENT ;
aschut 0:90750f158475 166 stateChanged = true;
aschut 0:90750f158475 167 }
aschut 0:90750f158475 168 break;
aschut 0:90750f158475 169
aschut 0:90750f158475 170 case CLICK:
aschut 0:90750f158475 171 // Actions
aschut 0:90750f158475 172 if (stateChanged)
aschut 0:90750f158475 173 {
aschut 0:90750f158475 174 // state initialization: blue
aschut 0:90750f158475 175 led1 = 1;
aschut 0:90750f158475 176 led2 = 1;
aschut 0:90750f158475 177 led3 = 0;
aschut 0:90750f158475 178 wait (1);
aschut 0:90750f158475 179
aschut 0:90750f158475 180 stateChanged = false;
aschut 0:90750f158475 181 }
aschut 0:90750f158475 182
aschut 0:90750f158475 183 // State transition logic: automatisch terug naar MOVEMENT.
aschut 0:90750f158475 184
aschut 0:90750f158475 185 currentState = MOVEMENT;
aschut 0:90750f158475 186 stateChanged = true;
aschut 0:90750f158475 187 break;
aschut 0:90750f158475 188
aschut 0:90750f158475 189 }
aschut 0:90750f158475 190 }
aschut 0:90750f158475 191
aschut 0:90750f158475 192 int main()
aschut 0:90750f158475 193 {
aschut 0:90750f158475 194 while (true)
aschut 0:90750f158475 195 {
aschut 0:90750f158475 196 led1 = 1;
aschut 0:90750f158475 197 led2 = 1;
aschut 0:90750f158475 198 led3 = 1;
aschut 0:90750f158475 199 ProcessStateMachine();
aschut 0:90750f158475 200
aschut 0:90750f158475 201 }
aschut 0:90750f158475 202
aschut 0:90750f158475 203 }
aschut 0:90750f158475 204
aschut 0:90750f158475 205
aschut 0:90750f158475 206
aschut 0:90750f158475 207
aschut 0:90750f158475 208
aschut 0:90750f158475 209