Stef Kuiper / Mbed 2 deprecated biorobotics_four_scorers_DEMO

Werkend met ledjes

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@10:b8c60fd468f1, 2019-10-06 (annotated)

Committer:
joostbonekamp
Date:
Sun Oct 06 13:14:40 2019 +0000
Revision:
10:b8c60fd468f1
Parent:
9:6537eead1241
Child:
11:f83e3bf7febf
Child:
13:51ae2da8da55
added joystick interface (to be tested)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
RobertoO 0:67c50348f842 1 #include "mbed.h"
RobertoO 1:b862262a9d14 2 #include "MODSERIAL.h"
joostbonekamp 2:bbaa6fca2ab1 3 #include "FastPWM.h"
joostbonekamp 2:bbaa6fca2ab1 4 #include "QEI.h"
RobertoO 0:67c50348f842 5
joostbonekamp 10:b8c60fd468f1 6 Serial pc(USBTX, USBRX); //verbinden met pc
joostbonekamp 4:36e32ddf2443 7 DigitalOut motor2_direction(D4); //verbinden met motor 2 op board (altijd d4)
joostbonekamp 5:aa8b5d5e632f 8 FastPWM motor2_pwm(D5); //verbinden met motor 2 pwm (altijd d5)
joostbonekamp 10:b8c60fd468f1 9 DigitalOut motor1_direction(D6); //verbinden met motor 2 op board (altijd d6)
joostbonekamp 10:b8c60fd468f1 10 FastPWM motor1_pwm(D7); //verbinden met motor 2 pwm (altijd d7)
joostbonekamp 5:aa8b5d5e632f 11 Ticker loop_ticker; //used in main()
PatrickZieverink 8:6f6a4dc12036 12 AnalogIn Pot1(A1); //pot 1 op biorobotics shield
PatrickZieverink 8:6f6a4dc12036 13 AnalogIn Pot2(A0); //pot 2 op biorobotics shield
joostbonekamp 10:b8c60fd468f1 14 AnalogIn joystick_x(A2); //input joystick
joostbonekamp 10:b8c60fd468f1 15 AnalogIn joystick_y(A3); //input joystick
joostbonekamp 10:b8c60fd468f1 16 InterruptIn but1(D10); //knop 1 op birorobotics shield
joostbonekamp 10:b8c60fd468f1 17 InterruptIn but2(D9); //knop 1 op birorobotics shield
PatrickZieverink 8:6f6a4dc12036 18 QEI encoder1 (D11, D12, NC, 8400, QEI::X4_ENCODING); //encoder 1 gebruiken
joostbonekamp 10:b8c60fd468f1 19 QEI encoder2 (D1, D2, NC, 8400, QEI::X4_ENCODING); //encoder 2 gebruiken
joostbonekamp 10:b8c60fd468f1 20
joostbonekamp 3:e3d12393adb1 21
joostbonekamp 5:aa8b5d5e632f 22 //variables
joostbonekamp 5:aa8b5d5e632f 23 enum States {idle, cw, ccw, end, failure};
PatrickZieverink 8:6f6a4dc12036 24 States current_state; //Definieren van de current_state met als keuzes Idle, cw, ccw, end en failure
PatrickZieverink 8:6f6a4dc12036 25
PatrickZieverink 8:6f6a4dc12036 26 class motor_state { // Een emmer met variabelen die je hierna motor gaat noemen.
joostbonekamp 10:b8c60fd468f1 27 public: //variabelen kunnen worden gebruikt door externe functies
joostbonekamp 10:b8c60fd468f1 28 float pwm1; //pwm of 1st motor
joostbonekamp 10:b8c60fd468f1 29 float pwm2; //pwm of 2nd motor
joostbonekamp 10:b8c60fd468f1 30 int dir1; //direction of 1st motor
joostbonekamp 10:b8c60fd468f1 31 int dir2; //direction of 2nd motor
joostbonekamp 5:aa8b5d5e632f 32 };
joostbonekamp 10:b8c60fd468f1 33 motor_state motor; //Een emmer met variabelen met motor.iets als naam bijv. motor.speed1
joostbonekamp 5:aa8b5d5e632f 34
joostbonekamp 10:b8c60fd468f1 35 bool state_changed = false; // wordt gebruikt in de state functies, om te kijken of de state net begint
PatrickZieverink 8:6f6a4dc12036 36 volatile bool but1_pressed = false;
PatrickZieverink 8:6f6a4dc12036 37 volatile bool but2_pressed = false;
joostbonekamp 10:b8c60fd468f1 38 float pot_1;
joostbonekamp 10:b8c60fd468f1 39 float pot_2;
joostbonekamp 10:b8c60fd468f1 40 float x_input;
joostbonekamp 10:b8c60fd468f1 41 float y_input;
joostbonekamp 5:aa8b5d5e632f 42
PatrickZieverink 9:6537eead1241 43 /*
PatrickZieverink 9:6537eead1241 44 float x_movement = 0;
PatrickZieverink 9:6537eead1241 45 float y_movement = 0;
PatrickZieverink 9:6537eead1241 46 float angle = 0;
PatrickZieverink 9:6537eead1241 47 float extention = 0;
PatrickZieverink 9:6537eead1241 48
PatrickZieverink 9:6537eead1241 49 void calibration() { //kalibratie van de sensoren (de nulstand invoeren)
PatrickZieverink 9:6537eead1241 50 letter = pc.getc() // krijg de letter vanuit de keyboard
PatrickZieverink 9:6537eead1241 51 while (letter == w){ // of hier een while loop moet ja of nee, geen idee, lijkt me beter dan een if naar mijn mening. Eigenlijk wil je iets op de rising en falling edge hebben om zo het begin en het eind erin te kunne zetten.
PatrickZieverink 9:6537eead1241 52 y_movement = 1;
PatrickZieverink 9:6537eead1241 53 }
PatrickZieverink 9:6537eead1241 54 while (letter == a) {
PatrickZieverink 9:6537eead1241 55 x_movement = -1;
PatrickZieverink 9:6537eead1241 56 }
PatrickZieverink 9:6537eead1241 57 while (letter == s){
PatrickZieverink 9:6537eead1241 58 y_movement = -1;
PatrickZieverink 9:6537eead1241 59 }
PatrickZieverink 9:6537eead1241 60 while (letter == d) {
PatrickZieverink 9:6537eead1241 61 x_movement = 1;
PatrickZieverink 9:6537eead1241 62 }
PatrickZieverink 9:6537eead1241 63 robot_kinematics() // hoe dat dit precies moet geen idee. Dit lijkt me een goed begin of iig een begin
PatrickZieverink 9:6537eead1241 64 x_movement = 0;
PatrickZieverink 9:6537eead1241 65 y_movement = 0;
PatrickZieverink 9:6537eead1241 66
PatrickZieverink 9:6537eead1241 67
PatrickZieverink 9:6537eead1241 68
PatrickZieverink 9:6537eead1241 69 }
PatrickZieverink 9:6537eead1241 70 void robot_kinematics() { //hoe de motoren moeten draaien als er een x of y richting als input is.
PatrickZieverink 9:6537eead1241 71 [motor.dir1, motor.dir2; motor.speed1, motor.speed2] = H^-1 *[x_movement, y_movement; angle] // Zoiets
PatrickZieverink 9:6537eead1241 72
PatrickZieverink 9:6537eead1241 73
PatrickZieverink 9:6537eead1241 74 }
PatrickZieverink 9:6537eead1241 75 */
PatrickZieverink 9:6537eead1241 76
PatrickZieverink 8:6f6a4dc12036 77 // the funtions needed to run the program
PatrickZieverink 8:6f6a4dc12036 78 void measure_signals() {
joostbonekamp 10:b8c60fd468f1 79 x_input = joystick_x.read();
joostbonekamp 10:b8c60fd468f1 80 y_input = joystick_y.read();
joostbonekamp 10:b8c60fd468f1 81
PatrickZieverink 9:6537eead1241 82 /*
PatrickZieverink 9:6537eead1241 83 letter = pc.getc() // krijg de letter vanuit de keyboard
PatrickZieverink 9:6537eead1241 84 if (letter == w){
PatrickZieverink 9:6537eead1241 85 y_movement = 1;
PatrickZieverink 9:6537eead1241 86 }
PatrickZieverink 9:6537eead1241 87 if (letter == a) {
PatrickZieverink 9:6537eead1241 88 x_movement = -1;
PatrickZieverink 9:6537eead1241 89 }
PatrickZieverink 9:6537eead1241 90 if (letter == s){
PatrickZieverink 9:6537eead1241 91 y_movement = -1;
PatrickZieverink 9:6537eead1241 92 }
PatrickZieverink 9:6537eead1241 93 if (letter == d) {
PatrickZieverink 9:6537eead1241 94 x_movement = 1;
PatrickZieverink 9:6537eead1241 95 }
PatrickZieverink 9:6537eead1241 96
PatrickZieverink 9:6537eead1241 97 angle = (encoder1.getPulses()-angle_0)/8400*360; //kijkt op welke stand encoder 1 nu staat (dat hoort dus de basis motor te zijn!) /8400 signalen per rotatie * 360 graden
PatrickZieverink 9:6537eead1241 98 extention = encoder2.getPulses()-extention_0/8400*8*5.05; //kijkt hoever de arm is uitgeschoven (/8400 per rotatie *8mm per rotatie aan verschuiving * 5.05 de gear ratio)
PatrickZieverink 9:6537eead1241 99
PatrickZieverink 9:6537eead1241 100
PatrickZieverink 9:6537eead1241 101 */
PatrickZieverink 9:6537eead1241 102
PatrickZieverink 8:6f6a4dc12036 103 pot_1 = Pot1.read();
PatrickZieverink 8:6f6a4dc12036 104 pc.printf("pot_1 = %f",pot_1); // Dit doet hij niet, waarom niet? Er scheelt niet veel aan, maar toch
PatrickZieverink 8:6f6a4dc12036 105 motor.speed1 = pot_1*0.75; //pod_read * 0.75 (dus max 75%)
PatrickZieverink 8:6f6a4dc12036 106
PatrickZieverink 8:6f6a4dc12036 107 pot_2 = Pot2.read();
PatrickZieverink 8:6f6a4dc12036 108 pc.printf("pot_2 = %f",pot_2); // Dit doet hij niet, waarom niet? Er scheelt niet veel aan, maar toch
PatrickZieverink 8:6f6a4dc12036 109 motor.speed2 = pot_2*0.75; //pod_read * 0.75 (dus max 75%)
PatrickZieverink 8:6f6a4dc12036 110 return;
PatrickZieverink 8:6f6a4dc12036 111 }
joostbonekamp 5:aa8b5d5e632f 112
joostbonekamp 5:aa8b5d5e632f 113 void do_nothing() {
PatrickZieverink 8:6f6a4dc12036 114 motor.speed1 = 0;
PatrickZieverink 8:6f6a4dc12036 115 motor.speed2 = 0;
PatrickZieverink 7:78bc59c7753c 116
joostbonekamp 10:b8c60fd468f1 117
joostbonekamp 10:b8c60fd468f1 118 //state guard
joostbonekamp 10:b8c60fd468f1 119 if (but1_pressed) { //this moves the program from the idle to cw state
joostbonekamp 5:aa8b5d5e632f 120 current_state = cw;
joostbonekamp 6:354a6509405f 121 state_changed = true; //to show next state it can initialize
joostbonekamp 5:aa8b5d5e632f 122 pc.printf("Changed state from idle to cw\r\n");
PatrickZieverink 8:6f6a4dc12036 123 but1_pressed = false; //reset button 1
joostbonekamp 5:aa8b5d5e632f 124 }
PatrickZieverink 7:78bc59c7753c 125
joostbonekamp 5:aa8b5d5e632f 126 }
PatrickZieverink 7:78bc59c7753c 127
PatrickZieverink 7:78bc59c7753c 128
joostbonekamp 5:aa8b5d5e632f 129 void rotate_cw() {
joostbonekamp 5:aa8b5d5e632f 130 if (state_changed) {
joostbonekamp 5:aa8b5d5e632f 131 pc.printf("State changed to CW\r\n");
joostbonekamp 6:354a6509405f 132 state_changed = false; //reset this so it wont print next loop
joostbonekamp 5:aa8b5d5e632f 133 }
PatrickZieverink 8:6f6a4dc12036 134 motor.dir1 = 1; //todo: check if this is actually clockwise
PatrickZieverink 7:78bc59c7753c 135 motor.dir2 = 1; //todo: check if this is actually clockwise
PatrickZieverink 7:78bc59c7753c 136
joostbonekamp 10:b8c60fd468f1 137 motor.pwm1 = x_input; //nog niet echt de x, maar de rotatie, moet later met kinematics gefixt worden
joostbonekamp 10:b8c60fd468f1 138 motor.pwm2 = y_input; // ook nog niet echt de y dus
joostbonekamp 10:b8c60fd468f1 139 //state guard
PatrickZieverink 8:6f6a4dc12036 140 if (!state_changed && but1_pressed) { //state wasnt just changed, button 1 has been pressed -> change state
joostbonekamp 5:aa8b5d5e632f 141 current_state = ccw;
joostbonekamp 5:aa8b5d5e632f 142 state_changed = true;
PatrickZieverink 8:6f6a4dc12036 143 but1_pressed = false; //reset this
joostbonekamp 5:aa8b5d5e632f 144 }
PatrickZieverink 8:6f6a4dc12036 145
joostbonekamp 5:aa8b5d5e632f 146 }
PatrickZieverink 7:78bc59c7753c 147
joostbonekamp 5:aa8b5d5e632f 148 void rotate_ccw() {
joostbonekamp 6:354a6509405f 149 //similar to rotate_cw()
joostbonekamp 5:aa8b5d5e632f 150 if (state_changed) {
joostbonekamp 5:aa8b5d5e632f 151 pc.printf("State changed to CCW\r\n");
joostbonekamp 5:aa8b5d5e632f 152 state_changed = false;
joostbonekamp 5:aa8b5d5e632f 153 }
PatrickZieverink 8:6f6a4dc12036 154 motor.dir1 = 0;
joostbonekamp 5:aa8b5d5e632f 155 motor.dir2 = 0;
joostbonekamp 10:b8c60fd468f1 156
joostbonekamp 10:b8c60fd468f1 157 motor.pwm1 = x_input; //nog niet echt de x
joostbonekamp 10:b8c60fd468f1 158 motor.pwm2 = y_input; //nog niet echt de y
joostbonekamp 10:b8c60fd468f1 159
joostbonekamp 10:b8c60fd468f1 160 //state guard
PatrickZieverink 8:6f6a4dc12036 161 if (!state_changed && but1_pressed) { //state niet veranderd, button 1 gepressd -> state verandert
joostbonekamp 5:aa8b5d5e632f 162 current_state = cw;
joostbonekamp 5:aa8b5d5e632f 163 state_changed = true;
PatrickZieverink 8:6f6a4dc12036 164 but1_pressed = false;
joostbonekamp 5:aa8b5d5e632f 165 }
joostbonekamp 5:aa8b5d5e632f 166 }
joostbonekamp 5:aa8b5d5e632f 167
PatrickZieverink 8:6f6a4dc12036 168 void motor_controller() {
joostbonekamp 10:b8c60fd468f1 169
PatrickZieverink 8:6f6a4dc12036 170 motor1_direction = motor.dir1; // Zet de richting goed
joostbonekamp 10:b8c60fd468f1 171 motor1_pwm.write(motor.pwm1); // Zet het op de snelheid van motor.speed1
PatrickZieverink 8:6f6a4dc12036 172
joostbonekamp 10:b8c60fd468f1 173
PatrickZieverink 8:6f6a4dc12036 174 motor2_direction = motor.dir2;
joostbonekamp 10:b8c60fd468f1 175 motor2_pwm.write(motor.pwm2);
PatrickZieverink 8:6f6a4dc12036 176 return;
joostbonekamp 10:b8c60fd468f1 177 }
PatrickZieverink 8:6f6a4dc12036 178
joostbonekamp 5:aa8b5d5e632f 179 void output() {return;}
joostbonekamp 5:aa8b5d5e632f 180
PatrickZieverink 8:6f6a4dc12036 181 void but1_interrupt () {
PatrickZieverink 8:6f6a4dc12036 182 but1_pressed = true;
PatrickZieverink 8:6f6a4dc12036 183 pc.printf("Button 1 pressed \n\r");
PatrickZieverink 8:6f6a4dc12036 184 }
PatrickZieverink 8:6f6a4dc12036 185
PatrickZieverink 8:6f6a4dc12036 186 void but2_interrupt () {
PatrickZieverink 8:6f6a4dc12036 187 but2_pressed = true;
PatrickZieverink 8:6f6a4dc12036 188 pc.printf("Button 2 pressed \n\r");
joostbonekamp 5:aa8b5d5e632f 189 }
joostbonekamp 5:aa8b5d5e632f 190
joostbonekamp 5:aa8b5d5e632f 191 void state_machine() {
joostbonekamp 5:aa8b5d5e632f 192
PatrickZieverink 8:6f6a4dc12036 193 if (but2_pressed){ // Is dit de goede locatie hiervoor?
PatrickZieverink 8:6f6a4dc12036 194 current_state = idle;
PatrickZieverink 8:6f6a4dc12036 195 but2_pressed = false;
PatrickZieverink 8:6f6a4dc12036 196 pc.printf("Do_nothing happened due to pressing button 2 \n\r");
PatrickZieverink 8:6f6a4dc12036 197 }
PatrickZieverink 8:6f6a4dc12036 198
joostbonekamp 5:aa8b5d5e632f 199 //run current state
joostbonekamp 5:aa8b5d5e632f 200 switch (current_state) {
PatrickZieverink 7:78bc59c7753c 201 case idle: // hoezo de :?
joostbonekamp 5:aa8b5d5e632f 202 do_nothing();
joostbonekamp 5:aa8b5d5e632f 203 break;
joostbonekamp 5:aa8b5d5e632f 204 case cw:
joostbonekamp 5:aa8b5d5e632f 205 rotate_cw();
joostbonekamp 5:aa8b5d5e632f 206 break;
joostbonekamp 5:aa8b5d5e632f 207 case ccw:
joostbonekamp 5:aa8b5d5e632f 208 rotate_ccw();
joostbonekamp 5:aa8b5d5e632f 209 break;
joostbonekamp 5:aa8b5d5e632f 210 case end:
joostbonekamp 5:aa8b5d5e632f 211 break;
joostbonekamp 5:aa8b5d5e632f 212 case failure:
joostbonekamp 5:aa8b5d5e632f 213 break;
joostbonekamp 5:aa8b5d5e632f 214 }
joostbonekamp 5:aa8b5d5e632f 215 }
joostbonekamp 5:aa8b5d5e632f 216
PatrickZieverink 8:6f6a4dc12036 217
joostbonekamp 5:aa8b5d5e632f 218 void main_loop() {
joostbonekamp 5:aa8b5d5e632f 219 measure_signals();
joostbonekamp 5:aa8b5d5e632f 220 state_machine();
joostbonekamp 5:aa8b5d5e632f 221 motor_controller();
joostbonekamp 5:aa8b5d5e632f 222 output();
joostbonekamp 5:aa8b5d5e632f 223 }
joostbonekamp 3:e3d12393adb1 224
joostbonekamp 2:bbaa6fca2ab1 225 int main() {
joostbonekamp 3:e3d12393adb1 226 pc.baud(115200);
PatrickZieverink 8:6f6a4dc12036 227 pc.printf("Executing main()... \r\n");
joostbonekamp 5:aa8b5d5e632f 228 current_state = idle;
joostbonekamp 3:e3d12393adb1 229
joostbonekamp 5:aa8b5d5e632f 230 motor.dir1 = 0;
joostbonekamp 5:aa8b5d5e632f 231 motor.dir2 = 0;
joostbonekamp 10:b8c60fd468f1 232 motor2_pwm.period(0.0000625f); // 1/frequency van waarop hij draait
joostbonekamp 10:b8c60fd468f1 233 motor1_pwm.period(0.0000625f); // 1/frequency van waarop hij draait
PatrickZieverink 7:78bc59c7753c 234
PatrickZieverink 8:6f6a4dc12036 235 but1.fall(&but1_interrupt);
PatrickZieverink 8:6f6a4dc12036 236 but2.fall(&but2_interrupt);
PatrickZieverink 9:6537eead1241 237 loop_ticker.attach(&main_loop, 0.1f); //main loop at 1kHz (niet per tiende seconde?)
PatrickZieverink 8:6f6a4dc12036 238 pc.printf("Main_loop is running\n \r");
joostbonekamp 5:aa8b5d5e632f 239 while (true) {}
joostbonekamp 6:354a6509405f 240 }