Sem Bertelink / Mbed 2 deprecated MOD09_Biorobotics_Groep_14

mag niet van hendrik D:

Dependencies:   mbed MatrixMath QEI HIDScope Matrix biquadFilter MODSERIAL FastPWM

main.cpp@19:5ac8b7af77a3, 2019-10-04 (annotated)

Committer:
Hendrikvg
Date:
Fri Oct 04 09:37:33 2019 +0000
Revision:
19:5ac8b7af77a3
Parent:
18:50c04dd523ea
Child:
20:ac1b4ffa3323
Crappy 3 state machine, maar dan met kloppende PWM namen

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