Kilian Buitenhuis / Mbed 2 deprecated State_machine

changed the location of all constants to Constants.h

Dependencies:   HIDScope QEI biquadFilter mbed

Fork of State_machine by Casper Kroon

main.cpp@1:afb820c6fc0d, 2018-10-30 (annotated)

Committer:
CasperK
Date:
Tue Oct 30 14:26:15 2018 +0000
Revision:
1:afb820c6fc0d
Parent:
0:1b2c842eca42
Child:
2:3f67b4833256
Added killswitch, and position calibration. Motors now driven from ticker

Who changed what in which revision?

UserRevisionLine numberNew contents of line
CasperK 0:1b2c842eca42 1 #include "mbed.h"
CasperK 1:afb820c6fc0d 2 #include "QEI.h"
CasperK 1:afb820c6fc0d 3 #include "HIDScope.h"
CasperK 1:afb820c6fc0d 4 #include "MODSERIAL.h"
CasperK 1:afb820c6fc0d 5
CasperK 1:afb820c6fc0d 6 PwmOut pwmpin1(D6);
CasperK 1:afb820c6fc0d 7 PwmOut pwmpin2(D5);
CasperK 1:afb820c6fc0d 8 AnalogIn potmeter1(A5);
CasperK 1:afb820c6fc0d 9 AnalogIn potmeter2(A4);
CasperK 1:afb820c6fc0d 10 DigitalIn button1(D2);
CasperK 1:afb820c6fc0d 11 DigitalIn button2(D3);
CasperK 1:afb820c6fc0d 12 DigitalOut directionpin1(D4);
CasperK 1:afb820c6fc0d 13 DigitalOut directionpin2(D7);
CasperK 1:afb820c6fc0d 14 QEI motor1(D13,D12,NC, 32);
CasperK 1:afb820c6fc0d 15 QEI motor2(D11,D10,NC, 32);
CasperK 1:afb820c6fc0d 16
CasperK 1:afb820c6fc0d 17 DigitalIn kill_switch(SW2); //position has to be changed
CasperK 1:afb820c6fc0d 18 DigitalIn next_switch(SW3); //name and position should be replaced
CasperK 1:afb820c6fc0d 19
CasperK 1:afb820c6fc0d 20 //for testing purposes
CasperK 1:afb820c6fc0d 21 DigitalOut ledred(LED_RED);
CasperK 1:afb820c6fc0d 22 DigitalOut ledgreen(LED_GREEN);
CasperK 1:afb820c6fc0d 23 DigitalOut ledblue(LED_BLUE);
CasperK 1:afb820c6fc0d 24
CasperK 1:afb820c6fc0d 25 enum states{PositionCalibration, EmgCalibration, Movement, KILL};
CasperK 1:afb820c6fc0d 26 states CurrentState;
CasperK 1:afb820c6fc0d 27 MODSERIAL pc(USBTX, USBRX);
CasperK 1:afb820c6fc0d 28 //HIDScope scope(2);
CasperK 1:afb820c6fc0d 29
CasperK 1:afb820c6fc0d 30 Ticker MotorsTicker;
CasperK 1:afb820c6fc0d 31
CasperK 1:afb820c6fc0d 32 volatile float pwm_value1 = 0.0;
CasperK 1:afb820c6fc0d 33 volatile float pwm_value2 = 0.0;
CasperK 0:1b2c842eca42 34
CasperK 1:afb820c6fc0d 35 void positionCalibration() {
CasperK 1:afb820c6fc0d 36 while(!button1){
CasperK 1:afb820c6fc0d 37 directionpin2 = true;
CasperK 1:afb820c6fc0d 38 pwm_value2 = 1.0f;
CasperK 1:afb820c6fc0d 39 }
CasperK 1:afb820c6fc0d 40 pwm_value2 = 0.0f;
CasperK 1:afb820c6fc0d 41 while(!button2){
CasperK 1:afb820c6fc0d 42 directionpin2 = true;
CasperK 1:afb820c6fc0d 43 pwm_value2 = 0.7f;
CasperK 1:afb820c6fc0d 44 //wait(0.01f);
CasperK 1:afb820c6fc0d 45 }
CasperK 1:afb820c6fc0d 46 pwm_value2 = 0.0f;
CasperK 1:afb820c6fc0d 47
CasperK 1:afb820c6fc0d 48 // pwm_value1 = potmeter1;
CasperK 1:afb820c6fc0d 49 // pwm_value2 = potmeter2;
CasperK 1:afb820c6fc0d 50
CasperK 1:afb820c6fc0d 51 if (!next_switch) {
CasperK 1:afb820c6fc0d 52 CurrentState = EmgCalibration;
CasperK 1:afb820c6fc0d 53 pc.printf("current state = EmgCalibration\n\r");
CasperK 1:afb820c6fc0d 54 }
CasperK 1:afb820c6fc0d 55 }
CasperK 1:afb820c6fc0d 56
CasperK 1:afb820c6fc0d 57 void emgCalibration() {
CasperK 1:afb820c6fc0d 58 ledblue = !ledblue;
CasperK 1:afb820c6fc0d 59 wait(0.5f);
CasperK 1:afb820c6fc0d 60
CasperK 1:afb820c6fc0d 61 if (!next_switch) {
CasperK 1:afb820c6fc0d 62 CurrentState = Movement;
CasperK 1:afb820c6fc0d 63 pc.printf("current state = Movement\n\r");
CasperK 1:afb820c6fc0d 64 }
CasperK 1:afb820c6fc0d 65 }
CasperK 1:afb820c6fc0d 66
CasperK 1:afb820c6fc0d 67 void movement() {
CasperK 1:afb820c6fc0d 68
CasperK 1:afb820c6fc0d 69 }
CasperK 1:afb820c6fc0d 70
CasperK 1:afb820c6fc0d 71 void move_motors() {
CasperK 1:afb820c6fc0d 72 pwmpin1 = pwm_value1;
CasperK 1:afb820c6fc0d 73 pwmpin2 = pwm_value2;
CasperK 1:afb820c6fc0d 74 }
CasperK 0:1b2c842eca42 75
CasperK 0:1b2c842eca42 76 int main()
CasperK 0:1b2c842eca42 77 {
CasperK 1:afb820c6fc0d 78 pc.baud(115200);
CasperK 1:afb820c6fc0d 79 pc.printf(" ** program reset **\n\r");
CasperK 1:afb820c6fc0d 80 pwmpin1.period_us(60);
CasperK 1:afb820c6fc0d 81 pwmpin2.period_us(60);
CasperK 1:afb820c6fc0d 82 directionpin1 = true;
CasperK 1:afb820c6fc0d 83 directionpin2 = true;
CasperK 1:afb820c6fc0d 84 ledred = true;
CasperK 1:afb820c6fc0d 85 ledgreen = true;
CasperK 1:afb820c6fc0d 86 ledblue = true;
CasperK 1:afb820c6fc0d 87
CasperK 1:afb820c6fc0d 88 MotorsTicker.attach(&move_motors, 0.02f);
CasperK 1:afb820c6fc0d 89 CurrentState = PositionCalibration;
CasperK 1:afb820c6fc0d 90 pc.printf("current state = PositionCalibration\n\r");
CasperK 0:1b2c842eca42 91
CasperK 0:1b2c842eca42 92 while (true) {
CasperK 1:afb820c6fc0d 93 switch(CurrentState) {
CasperK 0:1b2c842eca42 94 case PositionCalibration:
CasperK 1:afb820c6fc0d 95 ledred = true;
CasperK 1:afb820c6fc0d 96 ledgreen = false;
CasperK 1:afb820c6fc0d 97 ledblue = true;
CasperK 1:afb820c6fc0d 98 positionCalibration();
CasperK 0:1b2c842eca42 99
CasperK 1:afb820c6fc0d 100 if (!kill_switch) {
CasperK 1:afb820c6fc0d 101 CurrentState = KILL;
CasperK 1:afb820c6fc0d 102 pc.printf("current state = KILL\n\r");
CasperK 1:afb820c6fc0d 103 }
CasperK 1:afb820c6fc0d 104 break;
CasperK 1:afb820c6fc0d 105
CasperK 0:1b2c842eca42 106 case EmgCalibration:
CasperK 1:afb820c6fc0d 107 ledred = true;
CasperK 1:afb820c6fc0d 108 ledgreen = true;
CasperK 1:afb820c6fc0d 109 // ledblue = false;
CasperK 1:afb820c6fc0d 110 emgCalibration();
CasperK 1:afb820c6fc0d 111
CasperK 1:afb820c6fc0d 112 if (!kill_switch) {
CasperK 1:afb820c6fc0d 113 CurrentState = KILL;
CasperK 1:afb820c6fc0d 114 pc.printf("current state = KILL\n\r");
CasperK 0:1b2c842eca42 115 }
CasperK 1:afb820c6fc0d 116 break;
CasperK 0:1b2c842eca42 117
CasperK 0:1b2c842eca42 118 case Movement:
CasperK 1:afb820c6fc0d 119 ledred = true;
CasperK 1:afb820c6fc0d 120 ledgreen = false;
CasperK 1:afb820c6fc0d 121 ledblue = false;
CasperK 1:afb820c6fc0d 122 movement();
CasperK 1:afb820c6fc0d 123
CasperK 1:afb820c6fc0d 124 if (!kill_switch) {
CasperK 1:afb820c6fc0d 125 CurrentState = KILL;
CasperK 1:afb820c6fc0d 126 pc.printf("current state = KILL\n\r");
CasperK 0:1b2c842eca42 127 }
CasperK 1:afb820c6fc0d 128 break;
CasperK 0:1b2c842eca42 129
CasperK 0:1b2c842eca42 130 case KILL:
CasperK 1:afb820c6fc0d 131 bool u = true;
CasperK 1:afb820c6fc0d 132 ledgreen = true;
CasperK 1:afb820c6fc0d 133 ledblue = true;
CasperK 1:afb820c6fc0d 134 ledred = false;
CasperK 1:afb820c6fc0d 135 //turnoffmotors(); //placeholder
CasperK 1:afb820c6fc0d 136 //flashsos() //placeholder
CasperK 1:afb820c6fc0d 137 wait(1.0f);
CasperK 1:afb820c6fc0d 138 while(u) {
CasperK 1:afb820c6fc0d 139 if (!kill_switch) {
CasperK 1:afb820c6fc0d 140 u = false;
CasperK 1:afb820c6fc0d 141 ledred = true;
CasperK 1:afb820c6fc0d 142 wait(1.0f);
CasperK 1:afb820c6fc0d 143 CurrentState = PositionCalibration;
CasperK 1:afb820c6fc0d 144 pc.printf("current state = PositionCalibration\n\r");
CasperK 1:afb820c6fc0d 145 }
CasperK 0:1b2c842eca42 146 }
CasperK 1:afb820c6fc0d 147 break;
CasperK 1:afb820c6fc0d 148 }
CasperK 1:afb820c6fc0d 149 wait(0.2f);
CasperK 0:1b2c842eca42 150 }
CasperK 0:1b2c842eca42 151 }