Group 9 BioRobotics / Mbed 2 deprecated the_code

The code without functions and variables filled in

Dependencies:   HIDScope Servo mbed QEI biquadFilter

main.cpp@7:ec5add330cb3, 2018-10-22 (annotated)

Committer:
s1725696
Date:
Mon Oct 22 14:24:13 2018 +0000
Revision:
7:ec5add330cb3
Parent:
6:f495a77c2c95
Child:
8:5ad8a7892693
with buttons implemented

Who changed what in which revision?

UserRevisionLine numberNew contents of line
s1725696 5:3581013d4505 1 #include "mbed.h" // Use revision 119!!
s1725696 5:3581013d4505 2 #include "HIDScope.h" // For displaying data, select MBED - HID device, restart for every new code
s1725696 5:3581013d4505 3 #include "QEI.h" // For reading the encoder of the motors
s1725696 6:f495a77c2c95 4 #include <ctime> // for the timer in the states
s1725696 0:cb8857cf3ea4 5
s1725696 5:3581013d4505 6 #define SERIAL_BAUD 115200
s1725696 5:3581013d4505 7
s1725696 5:3581013d4505 8 // In- en outputs
s1725696 4:8183e7b228f0 9 // -----------------------------------------------------------------------------
s1725696 4:8183e7b228f0 10
s1725696 5:3581013d4505 11 /*
s1725696 0:cb8857cf3ea4 12 // EMG related
s1725696 5:3581013d4505 13 AnalogIn emg1(); // EMG signal 1
s1725696 5:3581013d4505 14 AnalogIn emg2(); // EMG signal 2
s1725696 5:3581013d4505 15 // if needed
s1725696 5:3581013d4505 16 AnalogIn emg3(); // EMG signal 3
s1725696 5:3581013d4505 17 AnalogIn emg4(); // EMG signal 4
s1725696 0:cb8857cf3ea4 18 */
s1725696 0:cb8857cf3ea4 19
s1725696 0:cb8857cf3ea4 20 // Motor related
s1725696 0:cb8857cf3ea4 21 DigitalOut dirpin_1(D4); // direction of motor 1
s1725696 0:cb8857cf3ea4 22 PwmOut pwmpin_1(D5); // PWM pin of motor 1
s1725696 5:3581013d4505 23 DigitalOut dirpin_2(D7); // direction of motor 2
s1725696 5:3581013d4505 24 PwmOut pwmpin_2(D6); // PWM pin of motor 2
s1725696 0:cb8857cf3ea4 25
s1725696 0:cb8857cf3ea4 26 // Extra stuff
s1725696 0:cb8857cf3ea4 27 // Like LED lights, buttons etc
s1725696 5:3581013d4505 28 /*
s1725696 7:ec5add330cb3 29 DigitalIn button_motorcal(SW1); // button for motor calibration
s1725696 7:ec5add330cb3 30 DigitalIn button_emergency(D7); // button for emergency mode
s1725696 7:ec5add330cb3 31 DigitalIn button_wait(SW2); // button for wait mode
s1725696 7:ec5add330cb3 32 DigitalIn button_demo(D6); // button for demo mode
s1725696 5:3581013d4505 33 */
s1725696 4:8183e7b228f0 34 DigitalIn led_red(LED_RED); // red led
s1725696 4:8183e7b228f0 35 DigitalIn led_green(LED_GREEN); // green led
s1725696 4:8183e7b228f0 36 DigitalIn led_blue(LED_BLUE); // blue led
s1725696 5:3581013d4505 37 AnalogIn pot_1(A1); // potmeter for simulating EMG input
s1725696 5:3581013d4505 38
s1725696 0:cb8857cf3ea4 39
s1725696 0:cb8857cf3ea4 40 // Other stuff
s1725696 4:8183e7b228f0 41 // -----------------------------------------------------------------------------
s1725696 0:cb8857cf3ea4 42 // Define stuff like tickers etc
s1725696 0:cb8857cf3ea4 43
s1725696 6:f495a77c2c95 44 //Ticker NAME; // Name a ticker, use each ticker only for 1 function!
s1725696 0:cb8857cf3ea4 45 HIDScope scope(2); // Number of channels in HIDScope
s1725696 0:cb8857cf3ea4 46 QEI Encoder(D13,D12,NC,64,QEI::X4_ENCODING); // Define the type of encoding: X4 encoding(default is X2)
s1725696 5:3581013d4505 47 Serial pc(USBTX,USBRX);
s1725696 6:f495a77c2c95 48 Timer t; // For timing the time in each state (https://os.mbed.com/handbook/Timer)
s1725696 0:cb8857cf3ea4 49
s1725696 0:cb8857cf3ea4 50 // Variables
s1725696 4:8183e7b228f0 51 // -----------------------------------------------------------------------------
s1725696 0:cb8857cf3ea4 52 // Define here all variables needed throughout the whole code
s1725696 5:3581013d4505 53 int counts;
s1725696 6:f495a77c2c95 54 float potmeter_value;
s1725696 6:f495a77c2c95 55 double time;
s1725696 6:f495a77c2c95 56 float time_in_state;
s1725696 6:f495a77c2c95 57
s1725696 6:f495a77c2c95 58 // states
s1725696 7:ec5add330cb3 59 enum states (WAIT, MOTOR_CAL, EMG_CAL, START, OPERATING, FAILURE, DEMO); // states the robot can be in
s1725696 7:ec5add330cb3 60 states CurrentState = WAIT; // the CurrentState to start with is the WAIT state
s1725696 6:f495a77c2c95 61 bool StateChanged = true; // the state must be changed to go into the next state
s1725696 0:cb8857cf3ea4 62
s1725696 0:cb8857cf3ea4 63 // Functions
s1725696 4:8183e7b228f0 64 // -----------------------------------------------------------------------------
s1725696 0:cb8857cf3ea4 65
s1725696 4:8183e7b228f0 66 // Encoder
s1725696 4:8183e7b228f0 67 // Getting encoder information from motors
s1725696 4:8183e7b228f0 68 int encoder()
s1725696 4:8183e7b228f0 69 {
s1725696 5:3581013d4505 70 int counts = Encoder.getPulses();
s1725696 4:8183e7b228f0 71 return counts;
s1725696 5:3581013d4505 72 }
s1725696 5:3581013d4505 73
s1725696 5:3581013d4505 74 // potmeter
s1725696 5:3581013d4505 75 float potmeter()
s1725696 5:3581013d4505 76 {
s1725696 5:3581013d4505 77 float out_1 = pot_1 * 2.0f;
s1725696 5:3581013d4505 78 float out_2 = out_1 - 1.0f;
s1725696 5:3581013d4505 79
s1725696 5:3581013d4505 80 dirpin_1.write(out_2 < 0);
s1725696 5:3581013d4505 81 dirpin_2.write(out_2 < 0);
s1725696 5:3581013d4505 82
s1725696 5:3581013d4505 83 pwmpin_1 = fabs (out_2); // Has to be positive value
s1725696 5:3581013d4505 84 pwmpin_2 = fabs (out_2);
s1725696 5:3581013d4505 85
s1725696 5:3581013d4505 86 return out_2;
s1725696 6:f495a77c2c95 87 }
s1725696 4:8183e7b228f0 88
s1725696 0:cb8857cf3ea4 89 // Motor calibration
s1725696 0:cb8857cf3ea4 90 // To calibrate the motor angle to some mechanical boundaries
s1725696 4:8183e7b228f0 91 // Kenneth mee bezig
s1725696 0:cb8857cf3ea4 92
s1725696 0:cb8857cf3ea4 93 // EMG calibration
s1725696 0:cb8857cf3ea4 94 // To calibrate the EMG signal to some boundary values
s1725696 6:f495a77c2c95 95 // Simon mee bezig
s1725696 0:cb8857cf3ea4 96
s1725696 0:cb8857cf3ea4 97 // Send EMG to HIDScope
s1725696 6:f495a77c2c95 98 void hidscope() // Attach this to a ticker!
s1725696 0:cb8857cf3ea4 99 {
s1725696 6:f495a77c2c95 100 scope.set(0, counts); // send EMG 1 to channel 1 (=0)
s1725696 6:f495a77c2c95 101 scope.set(1, potmeter_value); // sent EMG 2 to channel 2 (=1)
s1725696 5:3581013d4505 102
s1725696 1:1a8211e1f3f3 103 // Ensure that enough channels are available (HIDScope scope(2))
s1725696 0:cb8857cf3ea4 104 scope.send(); // Send all channels to the PC at once
s1725696 6:f495a77c2c95 105 }
s1725696 0:cb8857cf3ea4 106
s1725696 0:cb8857cf3ea4 107 // Start
s1725696 0:cb8857cf3ea4 108 // To move the robot to the starting position: middle
s1725696 4:8183e7b228f0 109 void start()
s1725696 4:8183e7b228f0 110 {
s1725696 4:8183e7b228f0 111 // move to middle
s1725696 4:8183e7b228f0 112 }
s1725696 0:cb8857cf3ea4 113
s1725696 0:cb8857cf3ea4 114 // Operating mode
s1725696 0:cb8857cf3ea4 115 // To control the robot with EMG signals
s1725696 6:f495a77c2c95 116 // Gertjan bezig met motor aansturen
s1725696 0:cb8857cf3ea4 117
s1725696 0:cb8857cf3ea4 118 // Processing EMG
s1725696 0:cb8857cf3ea4 119 // To process the raw EMG to a usable value, with filters etc
s1725696 6:f495a77c2c95 120 // Kees mee bezig
s1725696 0:cb8857cf3ea4 121
s1725696 0:cb8857cf3ea4 122 // Demo mode
s1725696 0:cb8857cf3ea4 123 // To control the robot with a written code and write 'HELLO'
s1725696 4:8183e7b228f0 124 // Voor het laatst bewaren
s1725696 0:cb8857cf3ea4 125
s1725696 0:cb8857cf3ea4 126 // Emergency mode
s1725696 0:cb8857cf3ea4 127 // To shut down the robot after an error etc
s1725696 0:cb8857cf3ea4 128
s1725696 0:cb8857cf3ea4 129 // Main function
s1725696 4:8183e7b228f0 130 // -----------------------------------------------------------------------------
s1725696 0:cb8857cf3ea4 131 int main()
s1725696 0:cb8857cf3ea4 132 {
s1725696 0:cb8857cf3ea4 133 pc.baud(115200); // For TeraTerm, the baudrate, set also in TeraTerm itself!
s1725696 0:cb8857cf3ea4 134 pc.printf("Start code\r\n"); // To check if the program starts
s1725696 5:3581013d4505 135 pwmpin_1.period_us(60); // Needed for PWM, not exactly known why
s1725696 6:f495a77c2c95 136 NAME.attach(&hidscope(),0.002);
s1725696 1:1a8211e1f3f3 137
s1725696 1:1a8211e1f3f3 138 while(true){
s1725696 6:f495a77c2c95 139 // timer
s1725696 6:f495a77c2c95 140 clock_t start; // start the timer
s1725696 6:f495a77c2c95 141 start = clock();
s1725696 6:f495a77c2c95 142 time = (clock() - start) / (double) CLOCKS_PER_SEC;
s1725696 5:3581013d4505 143
s1725696 5:3581013d4505 144 counts = encoder();
s1725696 6:f495a77c2c95 145 potmeter_value = potmeter();
s1725696 6:f495a77c2c95 146
s1725696 5:3581013d4505 147
s1725696 6:f495a77c2c95 148 //pc.printf("potmeter value = %f ", potmeter_value);
s1725696 6:f495a77c2c95 149 //pc.printf("counts = %i\r\n", counts);
s1725696 5:3581013d4505 150
s1725696 6:f495a77c2c95 151 // With the help of a switch loop and states we can switch between states and the robot knows what to do
s1725696 6:f495a77c2c95 152 switch{CurrentState}
s1725696 6:f495a77c2c95 153 {
s1725696 6:f495a77c2c95 154 case WAIT:
s1725696 6:f495a77c2c95 155
s1725696 6:f495a77c2c95 156 if(StateChanged) // so if StateChanged is true
s1725696 6:f495a77c2c95 157 {
s1725696 6:f495a77c2c95 158 // Execute WAIT mode
s1725696 6:f495a77c2c95 159 pc.printf("State is WAIT\r\n");
s1725696 6:f495a77c2c95 160 StateChanged = false; // the state is still WAIT
s1725696 6:f495a77c2c95 161 }
s1725696 6:f495a77c2c95 162
s1725696 7:ec5add330cb3 163 if(button_motorcal == true) // condition for WAIT --> MOTOR_CAl; button_motorcal press
s1725696 6:f495a77c2c95 164 {
s1725696 6:f495a77c2c95 165 CurrentState = MOTOR_CAL;
s1725696 6:f495a77c2c95 166 StateChanged = true;
s1725696 6:f495a77c2c95 167 }
s1725696 7:ec5add330cb3 168
s1725696 7:ec5add330cb3 169 if (button_emergency == true) // condition for WAIT --> FAILURE; button_emergency press
s1725696 7:ec5add330cb3 170 {
s1725696 7:ec5add330cb3 171 CurrentState = FAILURE;
s1725696 7:ec5add330cb3 172 StateChanged = true;
s1725696 7:ec5add330cb3 173 }
s1725696 6:f495a77c2c95 174
s1725696 6:f495a77c2c95 175 break;
s1725696 6:f495a77c2c95 176
s1725696 6:f495a77c2c95 177 case MOTOR_CAL:
s1725696 6:f495a77c2c95 178
s1725696 6:f495a77c2c95 179 if(StateChanged) // so if StateChanged is true
s1725696 6:f495a77c2c95 180 {
s1725696 6:f495a77c2c95 181 t.start();
s1725696 6:f495a77c2c95 182 // Execute MOTOR_CAL mode
s1725696 6:f495a77c2c95 183 t.stop();
s1725696 6:f495a77c2c95 184 time_in_state = t.read();
s1725696 6:f495a77c2c95 185 pc.printf("State is MOTOR_CAL\r\n");
s1725696 6:f495a77c2c95 186 StateChanged = false; // the state is still MOTOR_CAL
s1725696 6:f495a77c2c95 187 }
s1725696 6:f495a77c2c95 188
s1725696 6:f495a77c2c95 189 if(time_in_state > 3.0 && motor_velocity < 0.01) // condition for MOTOR_CAL --> EMG_CAL; 3s en motors stopped moving
s1725696 6:f495a77c2c95 190 {
s1725696 6:f495a77c2c95 191 CurrentState = EMG_CAL;
s1725696 6:f495a77c2c95 192 StateChanged = true;
s1725696 6:f495a77c2c95 193 }
s1725696 6:f495a77c2c95 194
s1725696 7:ec5add330cb3 195 if (button_emergency == true) // condition for MOTOR_CAL --> FAILURE; button_emergency press
s1725696 7:ec5add330cb3 196 {
s1725696 7:ec5add330cb3 197 CurrentState = FAILURE;
s1725696 7:ec5add330cb3 198 StateChanged = true;
s1725696 7:ec5add330cb3 199 }
s1725696 7:ec5add330cb3 200
s1725696 6:f495a77c2c95 201 break;
s1725696 6:f495a77c2c95 202
s1725696 6:f495a77c2c95 203 case EMG_CAL:
s1725696 6:f495a77c2c95 204
s1725696 6:f495a77c2c95 205 if(StateChanged) // so if StateChanged is true
s1725696 6:f495a77c2c95 206 {
s1725696 6:f495a77c2c95 207 t.reset();
s1725696 6:f495a77c2c95 208 t.start();
s1725696 6:f495a77c2c95 209 // Execute EMG_CAL mode
s1725696 6:f495a77c2c95 210 t.stop();
s1725696 6:f495a77c2c95 211 time_in_state = t.read();
s1725696 6:f495a77c2c95 212 pc.printf("State is EMG_CAL\r\n");
s1725696 6:f495a77c2c95 213 StateChanged = false; // state is still EMG_CAL
s1725696 6:f495a77c2c95 214 }
s1725696 6:f495a77c2c95 215
s1725696 6:f495a77c2c95 216 if(time_in_state < 5 && EMG < 0.01) // condition for EMG_CAL --> START; 5s and EMG is low
s1725696 6:f495a77c2c95 217 {
s1725696 6:f495a77c2c95 218 CurrentState = START;
s1725696 6:f495a77c2c95 219 StateChanged = true;
s1725696 6:f495a77c2c95 220 }
s1725696 6:f495a77c2c95 221
s1725696 7:ec5add330cb3 222 if (button_emergency == true) // condition for EMG_CAL --> FAILURE; button_emergency press
s1725696 7:ec5add330cb3 223 {
s1725696 7:ec5add330cb3 224 CurrentState = FAILURE;
s1725696 7:ec5add330cb3 225 StateChanged = true;
s1725696 7:ec5add330cb3 226 }
s1725696 7:ec5add330cb3 227
s1725696 6:f495a77c2c95 228 break;
s1725696 6:f495a77c2c95 229
s1725696 6:f495a77c2c95 230 case START:
s1725696 6:f495a77c2c95 231
s1725696 6:f495a77c2c95 232 if(StateChanged) // so if StateChanged is true
s1725696 6:f495a77c2c95 233 {
s1725696 6:f495a77c2c95 234 t.reset();
s1725696 6:f495a77c2c95 235 t.start();
s1725696 6:f495a77c2c95 236 // Execute START mode
s1725696 6:f495a77c2c95 237 t.stop();
s1725696 6:f495a77c2c95 238 time_in_state = t.read();
s1725696 6:f495a77c2c95 239 pc.printf("State is START\r\n");
s1725696 6:f495a77c2c95 240 StateChanged = false; // state is still START
s1725696 6:f495a77c2c95 241 }
s1725696 6:f495a77c2c95 242
s1725696 6:f495a77c2c95 243 if(time_in_state < 5 && error < 0.01) // condition for START --> OPERATING; 5s and error is small
s1725696 6:f495a77c2c95 244 {
s1725696 6:f495a77c2c95 245 CurrentState = OPERATING;
s1725696 6:f495a77c2c95 246 StateChanged = true;
s1725696 6:f495a77c2c95 247 }
s1725696 6:f495a77c2c95 248
s1725696 7:ec5add330cb3 249 if (button_emergency == true) // condition for START --> FAILURE; button_emergency press
s1725696 7:ec5add330cb3 250 {
s1725696 7:ec5add330cb3 251 CurrentState = FAILURE;
s1725696 7:ec5add330cb3 252 StateChanged = true;
s1725696 7:ec5add330cb3 253 }
s1725696 7:ec5add330cb3 254
s1725696 6:f495a77c2c95 255 break;
s1725696 6:f495a77c2c95 256
s1725696 6:f495a77c2c95 257 case OPERATING:
s1725696 6:f495a77c2c95 258
s1725696 6:f495a77c2c95 259 if(StateChanged) // so if StateChanged is true
s1725696 6:f495a77c2c95 260 {
s1725696 6:f495a77c2c95 261 // Execute OPERATING mode
s1725696 6:f495a77c2c95 262 pc.printf("State is OPERATING\r\n");
s1725696 6:f495a77c2c95 263 StateChanged = false; // state is still OPERATING
s1725696 6:f495a77c2c95 264 }
s1725696 6:f495a77c2c95 265
s1725696 7:ec5add330cb3 266 if(button_emergency == true) // condition for OPERATING --> FAILURE; button_emergency press
s1725696 6:f495a77c2c95 267 {
s1725696 6:f495a77c2c95 268 CurrentState = FAILURE;
s1725696 6:f495a77c2c95 269 StateChanged = true;
s1725696 6:f495a77c2c95 270 }
s1725696 6:f495a77c2c95 271
s1725696 7:ec5add330cb3 272 if(button_demo == true) // condition for OPERATING --> DEMO; button_demo press
s1725696 6:f495a77c2c95 273 {
s1725696 6:f495a77c2c95 274 CurrentState = DEMO;
s1725696 6:f495a77c2c95 275 StateChanged = true;
s1725696 6:f495a77c2c95 276 }
s1725696 6:f495a77c2c95 277
s1725696 6:f495a77c2c95 278 break;
s1725696 6:f495a77c2c95 279
s1725696 6:f495a77c2c95 280 case FAILURE:
s1725696 6:f495a77c2c95 281
s1725696 6:f495a77c2c95 282 if(StateChanged) // so if StateChanged is true
s1725696 6:f495a77c2c95 283 {
s1725696 6:f495a77c2c95 284 // Execute FAILURE mode
s1725696 6:f495a77c2c95 285 pc.printf("State is FAILURE\r\n");
s1725696 6:f495a77c2c95 286 StateChanged = false; // state is still FAILURE
s1725696 6:f495a77c2c95 287 }
s1725696 6:f495a77c2c95 288
s1725696 7:ec5add330cb3 289 if(button_wait == true) // condition for FAILURE --> WAIT; button_wait press (IF THAT IS EVEN POSSIBLE IN THIS STATE?)
s1725696 6:f495a77c2c95 290 {
s1725696 6:f495a77c2c95 291 CurrentState = WAIT;
s1725696 6:f495a77c2c95 292 StateChanged = true;
s1725696 6:f495a77c2c95 293 }
s1725696 6:f495a77c2c95 294
s1725696 6:f495a77c2c95 295 break;
s1725696 6:f495a77c2c95 296
s1725696 6:f495a77c2c95 297 case DEMO:
s1725696 6:f495a77c2c95 298
s1725696 6:f495a77c2c95 299 if(StateChanged) // so if StateChanged is true
s1725696 6:f495a77c2c95 300 {
s1725696 6:f495a77c2c95 301 // Execute DEMO mode
s1725696 6:f495a77c2c95 302 pc.printf("State is DEMO\r\n");
s1725696 6:f495a77c2c95 303 StateChanged = false; // state is still DEMO
s1725696 6:f495a77c2c95 304 }
s1725696 6:f495a77c2c95 305
s1725696 7:ec5add330cb3 306 if(button_wait == true) // condition for DEMO --> WAIT; button_wait press
s1725696 6:f495a77c2c95 307 {
s1725696 6:f495a77c2c95 308 CurrentState = WAIT;
s1725696 6:f495a77c2c95 309 StateChanged = true;
s1725696 6:f495a77c2c95 310 }
s1725696 6:f495a77c2c95 311
s1725696 7:ec5add330cb3 312 if (button_emergency == true) // condition for DEMO --> FAILURE; button_emergency press
s1725696 6:f495a77c2c95 313 {
s1725696 6:f495a77c2c95 314 CurrentState = FAILURE;
s1725696 6:f495a77c2c95 315 StateChanged = true;
s1725696 6:f495a77c2c95 316 }
s1725696 6:f495a77c2c95 317
s1725696 6:f495a77c2c95 318 break;
s1725696 6:f495a77c2c95 319
s1725696 6:f495a77c2c95 320 // no default
s1725696 4:8183e7b228f0 321 }
s1725696 6:f495a77c2c95 322
s1725696 6:f495a77c2c95 323 // while loop does not have to loop every time
s1725696 5:3581013d4505 324 }
s1725696 5:3581013d4505 325
s1725696 0:cb8857cf3ea4 326 }