Stef Kuiper / Mbed 2 deprecated biorobotics_four_scorers_DEMO

Werkend met ledjes

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@11:f83e3bf7febf, 2019-10-07 (annotated)

Committer:
joostbonekamp
Date:
Mon Oct 07 08:38:07 2019 +0000
Revision:
11:f83e3bf7febf
Parent:
10:b8c60fd468f1
Child:
12:88cbc65f2563
removed old stuff

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 */
joostbonekamp 11:f83e3bf7febf 102
PatrickZieverink 8:6f6a4dc12036 103 return;
PatrickZieverink 8:6f6a4dc12036 104 }
joostbonekamp 5:aa8b5d5e632f 105
joostbonekamp 5:aa8b5d5e632f 106 void do_nothing() {
PatrickZieverink 8:6f6a4dc12036 107 motor.speed1 = 0;
PatrickZieverink 8:6f6a4dc12036 108 motor.speed2 = 0;
PatrickZieverink 7:78bc59c7753c 109
joostbonekamp 10:b8c60fd468f1 110
joostbonekamp 10:b8c60fd468f1 111 //state guard
joostbonekamp 10:b8c60fd468f1 112 if (but1_pressed) { //this moves the program from the idle to cw state
joostbonekamp 5:aa8b5d5e632f 113 current_state = cw;
joostbonekamp 6:354a6509405f 114 state_changed = true; //to show next state it can initialize
joostbonekamp 5:aa8b5d5e632f 115 pc.printf("Changed state from idle to cw\r\n");
PatrickZieverink 8:6f6a4dc12036 116 but1_pressed = false; //reset button 1
joostbonekamp 5:aa8b5d5e632f 117 }
PatrickZieverink 7:78bc59c7753c 118
joostbonekamp 5:aa8b5d5e632f 119 }
PatrickZieverink 7:78bc59c7753c 120
PatrickZieverink 7:78bc59c7753c 121
joostbonekamp 5:aa8b5d5e632f 122 void rotate_cw() {
joostbonekamp 5:aa8b5d5e632f 123 if (state_changed) {
joostbonekamp 5:aa8b5d5e632f 124 pc.printf("State changed to CW\r\n");
joostbonekamp 6:354a6509405f 125 state_changed = false; //reset this so it wont print next loop
joostbonekamp 5:aa8b5d5e632f 126 }
PatrickZieverink 8:6f6a4dc12036 127 motor.dir1 = 1; //todo: check if this is actually clockwise
PatrickZieverink 7:78bc59c7753c 128 motor.dir2 = 1; //todo: check if this is actually clockwise
PatrickZieverink 7:78bc59c7753c 129
joostbonekamp 10:b8c60fd468f1 130 motor.pwm1 = x_input; //nog niet echt de x, maar de rotatie, moet later met kinematics gefixt worden
joostbonekamp 10:b8c60fd468f1 131 motor.pwm2 = y_input; // ook nog niet echt de y dus
joostbonekamp 10:b8c60fd468f1 132 //state guard
PatrickZieverink 8:6f6a4dc12036 133 if (!state_changed && but1_pressed) { //state wasnt just changed, button 1 has been pressed -> change state
joostbonekamp 5:aa8b5d5e632f 134 current_state = ccw;
joostbonekamp 5:aa8b5d5e632f 135 state_changed = true;
PatrickZieverink 8:6f6a4dc12036 136 but1_pressed = false; //reset this
joostbonekamp 5:aa8b5d5e632f 137 }
PatrickZieverink 8:6f6a4dc12036 138
joostbonekamp 5:aa8b5d5e632f 139 }
PatrickZieverink 7:78bc59c7753c 140
joostbonekamp 5:aa8b5d5e632f 141 void rotate_ccw() {
joostbonekamp 6:354a6509405f 142 //similar to rotate_cw()
joostbonekamp 5:aa8b5d5e632f 143 if (state_changed) {
joostbonekamp 5:aa8b5d5e632f 144 pc.printf("State changed to CCW\r\n");
joostbonekamp 5:aa8b5d5e632f 145 state_changed = false;
joostbonekamp 5:aa8b5d5e632f 146 }
PatrickZieverink 8:6f6a4dc12036 147 motor.dir1 = 0;
joostbonekamp 5:aa8b5d5e632f 148 motor.dir2 = 0;
joostbonekamp 10:b8c60fd468f1 149
joostbonekamp 10:b8c60fd468f1 150 motor.pwm1 = x_input; //nog niet echt de x
joostbonekamp 10:b8c60fd468f1 151 motor.pwm2 = y_input; //nog niet echt de y
joostbonekamp 10:b8c60fd468f1 152
joostbonekamp 10:b8c60fd468f1 153 //state guard
PatrickZieverink 8:6f6a4dc12036 154 if (!state_changed && but1_pressed) { //state niet veranderd, button 1 gepressd -> state verandert
joostbonekamp 5:aa8b5d5e632f 155 current_state = cw;
joostbonekamp 5:aa8b5d5e632f 156 state_changed = true;
PatrickZieverink 8:6f6a4dc12036 157 but1_pressed = false;
joostbonekamp 5:aa8b5d5e632f 158 }
joostbonekamp 5:aa8b5d5e632f 159 }
joostbonekamp 5:aa8b5d5e632f 160
PatrickZieverink 8:6f6a4dc12036 161 void motor_controller() {
joostbonekamp 10:b8c60fd468f1 162
PatrickZieverink 8:6f6a4dc12036 163 motor1_direction = motor.dir1; // Zet de richting goed
joostbonekamp 10:b8c60fd468f1 164 motor1_pwm.write(motor.pwm1); // Zet het op de snelheid van motor.speed1
PatrickZieverink 8:6f6a4dc12036 165
joostbonekamp 10:b8c60fd468f1 166
PatrickZieverink 8:6f6a4dc12036 167 motor2_direction = motor.dir2;
joostbonekamp 10:b8c60fd468f1 168 motor2_pwm.write(motor.pwm2);
PatrickZieverink 8:6f6a4dc12036 169 return;
joostbonekamp 10:b8c60fd468f1 170 }
PatrickZieverink 8:6f6a4dc12036 171
joostbonekamp 5:aa8b5d5e632f 172 void output() {return;}
joostbonekamp 5:aa8b5d5e632f 173
PatrickZieverink 8:6f6a4dc12036 174 void but1_interrupt () {
PatrickZieverink 8:6f6a4dc12036 175 but1_pressed = true;
PatrickZieverink 8:6f6a4dc12036 176 pc.printf("Button 1 pressed \n\r");
PatrickZieverink 8:6f6a4dc12036 177 }
PatrickZieverink 8:6f6a4dc12036 178
PatrickZieverink 8:6f6a4dc12036 179 void but2_interrupt () {
PatrickZieverink 8:6f6a4dc12036 180 but2_pressed = true;
PatrickZieverink 8:6f6a4dc12036 181 pc.printf("Button 2 pressed \n\r");
joostbonekamp 5:aa8b5d5e632f 182 }
joostbonekamp 5:aa8b5d5e632f 183
joostbonekamp 5:aa8b5d5e632f 184 void state_machine() {
joostbonekamp 5:aa8b5d5e632f 185
PatrickZieverink 8:6f6a4dc12036 186 if (but2_pressed){ // Is dit de goede locatie hiervoor?
PatrickZieverink 8:6f6a4dc12036 187 current_state = idle;
PatrickZieverink 8:6f6a4dc12036 188 but2_pressed = false;
PatrickZieverink 8:6f6a4dc12036 189 pc.printf("Do_nothing happened due to pressing button 2 \n\r");
PatrickZieverink 8:6f6a4dc12036 190 }
PatrickZieverink 8:6f6a4dc12036 191
joostbonekamp 5:aa8b5d5e632f 192 //run current state
joostbonekamp 5:aa8b5d5e632f 193 switch (current_state) {
PatrickZieverink 7:78bc59c7753c 194 case idle: // hoezo de :?
joostbonekamp 5:aa8b5d5e632f 195 do_nothing();
joostbonekamp 5:aa8b5d5e632f 196 break;
joostbonekamp 5:aa8b5d5e632f 197 case cw:
joostbonekamp 5:aa8b5d5e632f 198 rotate_cw();
joostbonekamp 5:aa8b5d5e632f 199 break;
joostbonekamp 5:aa8b5d5e632f 200 case ccw:
joostbonekamp 5:aa8b5d5e632f 201 rotate_ccw();
joostbonekamp 5:aa8b5d5e632f 202 break;
joostbonekamp 5:aa8b5d5e632f 203 case end:
joostbonekamp 5:aa8b5d5e632f 204 break;
joostbonekamp 5:aa8b5d5e632f 205 case failure:
joostbonekamp 5:aa8b5d5e632f 206 break;
joostbonekamp 5:aa8b5d5e632f 207 }
joostbonekamp 5:aa8b5d5e632f 208 }
joostbonekamp 5:aa8b5d5e632f 209
PatrickZieverink 8:6f6a4dc12036 210
joostbonekamp 5:aa8b5d5e632f 211 void main_loop() {
joostbonekamp 5:aa8b5d5e632f 212 measure_signals();
joostbonekamp 5:aa8b5d5e632f 213 state_machine();
joostbonekamp 5:aa8b5d5e632f 214 motor_controller();
joostbonekamp 5:aa8b5d5e632f 215 output();
joostbonekamp 5:aa8b5d5e632f 216 }
joostbonekamp 3:e3d12393adb1 217
joostbonekamp 2:bbaa6fca2ab1 218 int main() {
joostbonekamp 3:e3d12393adb1 219 pc.baud(115200);
PatrickZieverink 8:6f6a4dc12036 220 pc.printf("Executing main()... \r\n");
joostbonekamp 5:aa8b5d5e632f 221 current_state = idle;
joostbonekamp 3:e3d12393adb1 222
joostbonekamp 5:aa8b5d5e632f 223 motor.dir1 = 0;
joostbonekamp 5:aa8b5d5e632f 224 motor.dir2 = 0;
joostbonekamp 11:f83e3bf7febf 225 motor2_pwm.period(1/160000f); // 1/frequency van waarop hij draait
joostbonekamp 11:f83e3bf7febf 226 motor1_pwm.period(1/160000f); // 1/frequency van waarop hij draait
PatrickZieverink 7:78bc59c7753c 227
PatrickZieverink 8:6f6a4dc12036 228 but1.fall(&but1_interrupt);
PatrickZieverink 8:6f6a4dc12036 229 but2.fall(&but2_interrupt);
PatrickZieverink 9:6537eead1241 230 loop_ticker.attach(&main_loop, 0.1f); //main loop at 1kHz (niet per tiende seconde?)
PatrickZieverink 8:6f6a4dc12036 231 pc.printf("Main_loop is running\n \r");
joostbonekamp 5:aa8b5d5e632f 232 while (true) {}
joostbonekamp 6:354a6509405f 233 }