Sem Bertelink / Mbed 2 deprecated MOD09_Biorobotics_Groep_14lol

lololololol

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@17:cacf9e75eda7, 2019-09-23 (annotated)

Committer:
Hendrikvg
Date:
Mon Sep 23 14:21:30 2019 +0000
Revision:
17:cacf9e75eda7
Parent:
16:40183eeadb6d
Child:
18:50c04dd523ea
Finite State Machine met 3 states: rust, CW, CCW. Snelheid en felheid LED instelbaar dmv potentiometer. Encoder werkt voor de draaing van de end effector van de motor. Ticker functie werkt.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Hendrikvg 17:cacf9e75eda7 1 #include "QEI.h"
Hendrikvg 15:80b3ac2b8448 2 #include "mbed.h"
Hendrikvg 14:20f11bb58244 3 #include "FastPWM.h"
Hendrikvg 17:cacf9e75eda7 4 #include "HIDScope.h"
Hendrikvg 16:40183eeadb6d 5 #include "MODSERIAL.h"
Hendrikvg 9:12b9865e7373 6
Hendrikvg 15:80b3ac2b8448 7 MODSERIAL pc(USBTX, USBRX);
Hendrikvg 15:80b3ac2b8448 8 DigitalOut ledr(LED_RED);
Hendrikvg 15:80b3ac2b8448 9 DigitalOut ledg(LED_GREEN);
Hendrikvg 15:80b3ac2b8448 10 DigitalOut ledb(LED_BLUE);
Hendrikvg 16:40183eeadb6d 11 Ticker ReduceEmission;
Hendrikvg 15:80b3ac2b8448 12 Timer R;
Hendrikvg 15:80b3ac2b8448 13 Timer G;
Hendrikvg 15:80b3ac2b8448 14 Timer B;
Hendrikvg 15:80b3ac2b8448 15
Hendrikvg 9:12b9865e7373 16 InterruptIn BUT1(D1);
Hendrikvg 9:12b9865e7373 17 InterruptIn BUT2(D0);
Hendrikvg 14:20f11bb58244 18 FastPWM lichtje(D3);
Hendrikvg 9:12b9865e7373 19 AnalogIn ain(A0);
Hendrikvg 14:20f11bb58244 20 DigitalOut direction(D6);
Hendrikvg 14:20f11bb58244 21 DigitalOut speed(D7);
RobertoO 0:67c50348f842 22
Hendrikvg 17:cacf9e75eda7 23 HIDScope scope(2);
Hendrikvg 17:cacf9e75eda7 24 QEI encoder(D12,D13,NC,64,QEI::X4_ENCODING);
Hendrikvg 17:cacf9e75eda7 25 Ticker RW_scope;
Hendrikvg 17:cacf9e75eda7 26
Hendrikvg 15:80b3ac2b8448 27 // variables
Hendrikvg 15:80b3ac2b8448 28
Hendrikvg 15:80b3ac2b8448 29 volatile char command = 'r';
Hendrikvg 15:80b3ac2b8448 30 enum states {red, green, blue};
Hendrikvg 15:80b3ac2b8448 31 states CurrentState = red;
Hendrikvg 15:80b3ac2b8448 32 bool StateChanged = true;
Hendrikvg 15:80b3ac2b8448 33
Hendrikvg 16:40183eeadb6d 34 volatile int on_time_ms; // The time the LED should be on, in microseconds
Hendrikvg 16:40183eeadb6d 35 volatile int off_time_ms;
Hendrikvg 15:80b3ac2b8448 36
Hendrikvg 17:cacf9e75eda7 37 int degrees;
Hendrikvg 17:cacf9e75eda7 38 volatile double x;
Hendrikvg 17:cacf9e75eda7 39 volatile double x_prev=0;
Hendrikvg 17:cacf9e75eda7 40 volatile double y;
Hendrikvg 17:cacf9e75eda7 41
Hendrikvg 9:12b9865e7373 42 int n=5;
Hendrikvg 2:d9b0ebf3fcca 43
Hendrikvg 15:80b3ac2b8448 44 // functions
Hendrikvg 15:80b3ac2b8448 45
Hendrikvg 9:12b9865e7373 46 void plus()
Hendrikvg 9:12b9865e7373 47 {
Hendrikvg 9:12b9865e7373 48 n++; // n=n+1
Hendrikvg 15:80b3ac2b8448 49 if (n>10) {
Hendrikvg 9:12b9865e7373 50 n=10;
Hendrikvg 9:12b9865e7373 51 }
Hendrikvg 15:80b3ac2b8448 52 }
Hendrikvg 15:80b3ac2b8448 53
Hendrikvg 9:12b9865e7373 54 void min()
Hendrikvg 8:d1794f225fff 55 {
Hendrikvg 9:12b9865e7373 56 n--;
Hendrikvg 15:80b3ac2b8448 57 if (n<0) {
Hendrikvg 9:12b9865e7373 58 n=0;
Hendrikvg 7:d307e31f7391 59 }
Hendrikvg 15:80b3ac2b8448 60 }
Hendrikvg 7:d307e31f7391 61
Hendrikvg 16:40183eeadb6d 62 void TurnLedRed()
Hendrikvg 16:40183eeadb6d 63 {
Hendrikvg 16:40183eeadb6d 64 ledr = 0;
Hendrikvg 16:40183eeadb6d 65 ledg = 1;
Hendrikvg 16:40183eeadb6d 66 ledb = 1;
Hendrikvg 16:40183eeadb6d 67 }
Hendrikvg 16:40183eeadb6d 68
Hendrikvg 16:40183eeadb6d 69 void TurnLedGreen()
Hendrikvg 16:40183eeadb6d 70 {
Hendrikvg 16:40183eeadb6d 71 ledr = 1;
Hendrikvg 16:40183eeadb6d 72 ledg = 0;
Hendrikvg 16:40183eeadb6d 73 ledb = 1;
Hendrikvg 16:40183eeadb6d 74 }
Hendrikvg 16:40183eeadb6d 75
Hendrikvg 16:40183eeadb6d 76 void TurnLedBlue()
Hendrikvg 16:40183eeadb6d 77 {
Hendrikvg 16:40183eeadb6d 78 ledr = 1;
Hendrikvg 16:40183eeadb6d 79 ledg = 1;
Hendrikvg 16:40183eeadb6d 80 ledb = 0;
Hendrikvg 16:40183eeadb6d 81 }
Hendrikvg 16:40183eeadb6d 82
Hendrikvg 16:40183eeadb6d 83 void PulseWidthModulation() // Bepaalt de duty cycle gebaseerd op de potentiometer en zet de motor dan op die snelheid mbh PWM
Hendrikvg 16:40183eeadb6d 84 {
Hendrikvg 16:40183eeadb6d 85 on_time_ms = (int) (ain.read()*(1.0/50)*1.0e3);
Hendrikvg 16:40183eeadb6d 86 off_time_ms = (int) ((1-ain.read())*(1.0/50)*1.0e3);
Hendrikvg 16:40183eeadb6d 87 direction = 1;
Hendrikvg 16:40183eeadb6d 88 wait_ms(on_time_ms);
Hendrikvg 16:40183eeadb6d 89 direction = 0;
Hendrikvg 16:40183eeadb6d 90 wait_ms(off_time_ms);
Hendrikvg 16:40183eeadb6d 91 }
Hendrikvg 16:40183eeadb6d 92
Hendrikvg 16:40183eeadb6d 93 void EnergySaving() // Functie voor de "ReduceEmissions" ticker, zet de motor uit en LED op rood
Hendrikvg 16:40183eeadb6d 94 {
Hendrikvg 16:40183eeadb6d 95 command = 'r';
Hendrikvg 16:40183eeadb6d 96 }
Hendrikvg 16:40183eeadb6d 97
Hendrikvg 16:40183eeadb6d 98 void CheckCommandFromTerminal(void) // Functie voor de in de main loop
Hendrikvg 16:40183eeadb6d 99 {
Hendrikvg 16:40183eeadb6d 100 command = pc.getcNb();
Hendrikvg 16:40183eeadb6d 101 }
Hendrikvg 16:40183eeadb6d 102
Hendrikvg 16:40183eeadb6d 103 void WhileRed()
Hendrikvg 16:40183eeadb6d 104 {
Hendrikvg 16:40183eeadb6d 105 if (command == 'g') {
Hendrikvg 16:40183eeadb6d 106 R.stop();
Hendrikvg 16:40183eeadb6d 107 pc.printf("The LED has been red for %f seconds!\n\r", R.read());
Hendrikvg 16:40183eeadb6d 108 CurrentState= green;
Hendrikvg 16:40183eeadb6d 109 StateChanged= true;
Hendrikvg 16:40183eeadb6d 110 }
Hendrikvg 16:40183eeadb6d 111 if (command == 'b') {
Hendrikvg 16:40183eeadb6d 112 R.stop();
Hendrikvg 16:40183eeadb6d 113 pc.printf("The LED has been red for %f seconds!\n\r", R.read());
Hendrikvg 16:40183eeadb6d 114 CurrentState= blue;
Hendrikvg 16:40183eeadb6d 115 StateChanged= true;
Hendrikvg 16:40183eeadb6d 116 }
Hendrikvg 16:40183eeadb6d 117 }
Hendrikvg 16:40183eeadb6d 118
Hendrikvg 16:40183eeadb6d 119 void WhileGreen()
Hendrikvg 16:40183eeadb6d 120 {
Hendrikvg 16:40183eeadb6d 121 PulseWidthModulation();
Hendrikvg 16:40183eeadb6d 122 if (command == 'r') {
Hendrikvg 16:40183eeadb6d 123 G.stop();
Hendrikvg 16:40183eeadb6d 124 pc.printf("The LED has been green for %f seconds!\n\r", G.read());
Hendrikvg 16:40183eeadb6d 125 CurrentState= red;
Hendrikvg 16:40183eeadb6d 126 StateChanged= true;
Hendrikvg 16:40183eeadb6d 127 }
Hendrikvg 16:40183eeadb6d 128 if (command == 'b') {
Hendrikvg 16:40183eeadb6d 129 G.stop();
Hendrikvg 16:40183eeadb6d 130 pc.printf("The LED has been green for %f seconds!\n\r", G.read());
Hendrikvg 16:40183eeadb6d 131 CurrentState= blue;
Hendrikvg 16:40183eeadb6d 132 StateChanged= true;
Hendrikvg 16:40183eeadb6d 133 }
Hendrikvg 16:40183eeadb6d 134 }
Hendrikvg 16:40183eeadb6d 135
Hendrikvg 16:40183eeadb6d 136 void WhileBlue()
Hendrikvg 16:40183eeadb6d 137 {
Hendrikvg 16:40183eeadb6d 138 PulseWidthModulation();
Hendrikvg 16:40183eeadb6d 139 if (command == 'r') {
Hendrikvg 16:40183eeadb6d 140 B.stop();
Hendrikvg 16:40183eeadb6d 141 pc.printf("The LED has been blue for %f seconds!\n\r", B.read());
Hendrikvg 16:40183eeadb6d 142 CurrentState= red;
Hendrikvg 16:40183eeadb6d 143 StateChanged= true;
Hendrikvg 16:40183eeadb6d 144 }
Hendrikvg 16:40183eeadb6d 145 if (command == 'g') {
Hendrikvg 16:40183eeadb6d 146 B.stop();
Hendrikvg 16:40183eeadb6d 147 pc.printf("The LED has been blue for %f seconds!\n\r", B.read());
Hendrikvg 16:40183eeadb6d 148 CurrentState= green;
Hendrikvg 16:40183eeadb6d 149 StateChanged= true;
Hendrikvg 16:40183eeadb6d 150 }
Hendrikvg 16:40183eeadb6d 151 }
Hendrikvg 16:40183eeadb6d 152
Hendrikvg 17:cacf9e75eda7 153 void ReadEncoderAndWriteScope()
Hendrikvg 17:cacf9e75eda7 154 {
Hendrikvg 17:cacf9e75eda7 155 degrees = ((360/64)*encoder.getPulses())/131.25; // 360/64 voor de 64 CPR Encoder, 131.25 omdat de gear ratio 131.25:1 is. Hiermee is 1 omwenteling 360.
Hendrikvg 17:cacf9e75eda7 156 x = degrees;
Hendrikvg 17:cacf9e75eda7 157 scope.set(0,x);
Hendrikvg 17:cacf9e75eda7 158 y = (x_prev + x)/2.0;
Hendrikvg 17:cacf9e75eda7 159 scope.set(1,y);
Hendrikvg 17:cacf9e75eda7 160 x_prev=x;
Hendrikvg 17:cacf9e75eda7 161 scope.send();
Hendrikvg 17:cacf9e75eda7 162 }
Hendrikvg 17:cacf9e75eda7 163
Hendrikvg 16:40183eeadb6d 164 void StateMachine(void)
Hendrikvg 16:40183eeadb6d 165 {
Hendrikvg 16:40183eeadb6d 166 switch(CurrentState) {
Hendrikvg 16:40183eeadb6d 167 case red:
Hendrikvg 16:40183eeadb6d 168 if (StateChanged) {
Hendrikvg 16:40183eeadb6d 169 pc.printf("Initiating turning LED red\n\r");
Hendrikvg 16:40183eeadb6d 170 StateChanged= false;
Hendrikvg 16:40183eeadb6d 171 TurnLedRed();
Hendrikvg 16:40183eeadb6d 172 R.start();
Hendrikvg 16:40183eeadb6d 173 direction = 0;
Hendrikvg 16:40183eeadb6d 174 pc.printf("LED is now red!\n\r");
Hendrikvg 16:40183eeadb6d 175 }
Hendrikvg 16:40183eeadb6d 176 WhileRed();
Hendrikvg 16:40183eeadb6d 177 break;
Hendrikvg 16:40183eeadb6d 178 case green:
Hendrikvg 16:40183eeadb6d 179 if (StateChanged) {
Hendrikvg 16:40183eeadb6d 180 pc.printf("Initiating turning LED green\n\r");
Hendrikvg 16:40183eeadb6d 181 StateChanged= false;
Hendrikvg 16:40183eeadb6d 182 TurnLedGreen();
Hendrikvg 16:40183eeadb6d 183 G.start();
Hendrikvg 16:40183eeadb6d 184 speed = 0;
Hendrikvg 16:40183eeadb6d 185 pc.printf("LED is now green!\n\r");
Hendrikvg 16:40183eeadb6d 186 }
Hendrikvg 16:40183eeadb6d 187 WhileGreen();
Hendrikvg 16:40183eeadb6d 188 break;
Hendrikvg 16:40183eeadb6d 189 case blue:
Hendrikvg 16:40183eeadb6d 190 if (StateChanged) {
Hendrikvg 16:40183eeadb6d 191 pc.printf("Initiating turning LED blue\n\r");
Hendrikvg 16:40183eeadb6d 192 StateChanged= false;
Hendrikvg 16:40183eeadb6d 193 TurnLedBlue();
Hendrikvg 16:40183eeadb6d 194 B.start();
Hendrikvg 16:40183eeadb6d 195 speed = 255;
Hendrikvg 16:40183eeadb6d 196 pc.printf("LED is now blue!\n\r");
Hendrikvg 16:40183eeadb6d 197 }
Hendrikvg 16:40183eeadb6d 198 WhileBlue();
Hendrikvg 16:40183eeadb6d 199 break;
Hendrikvg 16:40183eeadb6d 200 default:
Hendrikvg 16:40183eeadb6d 201 pc.printf("Unknown or unimplemented state reached!\n\r");
Hendrikvg 16:40183eeadb6d 202 }
Hendrikvg 16:40183eeadb6d 203 }
Hendrikvg 16:40183eeadb6d 204
Hendrikvg 15:80b3ac2b8448 205 // main
Hendrikvg 15:80b3ac2b8448 206
Hendrikvg 15:80b3ac2b8448 207 int main()
Hendrikvg 15:80b3ac2b8448 208 {
RobertoO 0:67c50348f842 209 pc.baud(115200);
Hendrikvg 17:cacf9e75eda7 210 pc.printf("Hello World!\n\r");
Hendrikvg 17:cacf9e75eda7 211 RW_scope.attach(&ReadEncoderAndWriteScope, 0.1);
Hendrikvg 16:40183eeadb6d 212 ReduceEmission.attach(EnergySaving,20);
Hendrikvg 9:12b9865e7373 213 BUT1.fall(plus);
Hendrikvg 9:12b9865e7373 214 BUT2.fall(min);
Hendrikvg 15:80b3ac2b8448 215 while(true) {
Hendrikvg 15:80b3ac2b8448 216 CheckCommandFromTerminal();
Hendrikvg 15:80b3ac2b8448 217 StateMachine();
Hendrikvg 15:80b3ac2b8448 218 lichtje.write(ain.read()); // duty cycle
Hendrikvg 9:12b9865e7373 219 }
Hendrikvg 2:d9b0ebf3fcca 220 }