Sem Bertelink / Mbed 2 deprecated MOD09_Biorobotics_Groep_14lol

lololololol

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@18:50c04dd523ea, 2019-09-23 (annotated)

Committer:
Hendrikvg
Date:
Mon Sep 23 14:43:01 2019 +0000
Revision:
18:50c04dd523ea
Parent:
17:cacf9e75eda7
Child:
19:5ac8b7af77a3
Met pin configuration comments

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