Stef Kuiper / Mbed 2 deprecated biorobotics_four_scorers_DEMO

Werkend met ledjes

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@9:6537eead1241, 2019-10-05 (annotated)

Committer:
PatrickZieverink
Date:
Sat Oct 05 16:38:45 2019 +0000
Revision:
9:6537eead1241
Parent:
8:6f6a4dc12036
Child:
10:b8c60fd468f1
Niets veranderd wat aanstaat, alleen heel veel in concept erin gezet waarvan ik denk dat het wel handig is.

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
PatrickZieverink 7:78bc59c7753c 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)
PatrickZieverink 8:6f6a4dc12036 9 DigitalOut motor1_direction(D6); //verbinden met motor 2 op board (altijd d4)
PatrickZieverink 8:6f6a4dc12036 10 FastPWM motor1_pwm(D7); //verbinden met motor 2 pwm (altijd d5)
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
PatrickZieverink 8:6f6a4dc12036 14 InterruptIn but1(D10); //knop 1 op birorobotics shield
PatrickZieverink 8:6f6a4dc12036 15 InterruptIn but2(D9); //knop 1 op birorobotics shield
PatrickZieverink 8:6f6a4dc12036 16 QEI encoder1 (D11, D12, NC, 8400, QEI::X4_ENCODING); //encoder 1 gebruiken
PatrickZieverink 8:6f6a4dc12036 17 QEI encoder2 (D1, D2, NC, 8400, QEI::X4_ENCODING); //encoder 2 gebruiken
joostbonekamp 3:e3d12393adb1 18
joostbonekamp 5:aa8b5d5e632f 19 //variables
joostbonekamp 5:aa8b5d5e632f 20 enum States {idle, cw, ccw, end, failure};
PatrickZieverink 8:6f6a4dc12036 21 States current_state; //Definieren van de current_state met als keuzes Idle, cw, ccw, end en failure
PatrickZieverink 8:6f6a4dc12036 22
PatrickZieverink 8:6f6a4dc12036 23 class motor_state { // Een emmer met variabelen die je hierna motor gaat noemen.
PatrickZieverink 8:6f6a4dc12036 24 public: //Wat is public?
joostbonekamp 5:aa8b5d5e632f 25 float pwm1; //pwm of 1st motor
joostbonekamp 5:aa8b5d5e632f 26 float pwm2; //pwm of 2nd motor
joostbonekamp 5:aa8b5d5e632f 27 int dir1; //direction of 1st motor
joostbonekamp 5:aa8b5d5e632f 28 int dir2; //direction of 2nd motor
PatrickZieverink 8:6f6a4dc12036 29 float speed1; // speed motor 1
PatrickZieverink 8:6f6a4dc12036 30 double speed2; // speed motor 2
joostbonekamp 5:aa8b5d5e632f 31 };
PatrickZieverink 8:6f6a4dc12036 32 motor_state motor; //Een emmer met variabelen met motor.iets als naam bijv. motor.speed1
joostbonekamp 5:aa8b5d5e632f 33
joostbonekamp 5:aa8b5d5e632f 34 bool state_changed = false;
PatrickZieverink 8:6f6a4dc12036 35 volatile bool but1_pressed = false;
PatrickZieverink 8:6f6a4dc12036 36 volatile bool but2_pressed = false;
PatrickZieverink 8:6f6a4dc12036 37 float pot_1 = 0;
PatrickZieverink 8:6f6a4dc12036 38 float pot_2 = 0;
joostbonekamp 5:aa8b5d5e632f 39
PatrickZieverink 9:6537eead1241 40 /*
PatrickZieverink 9:6537eead1241 41 float x_movement = 0;
PatrickZieverink 9:6537eead1241 42 float y_movement = 0;
PatrickZieverink 9:6537eead1241 43 float angle = 0;
PatrickZieverink 9:6537eead1241 44 float extention = 0;
PatrickZieverink 9:6537eead1241 45
PatrickZieverink 9:6537eead1241 46 void calibration() { //kalibratie van de sensoren (de nulstand invoeren)
PatrickZieverink 9:6537eead1241 47 letter = pc.getc() // krijg de letter vanuit de keyboard
PatrickZieverink 9:6537eead1241 48 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 49 y_movement = 1;
PatrickZieverink 9:6537eead1241 50 }
PatrickZieverink 9:6537eead1241 51 while (letter == a) {
PatrickZieverink 9:6537eead1241 52 x_movement = -1;
PatrickZieverink 9:6537eead1241 53 }
PatrickZieverink 9:6537eead1241 54 while (letter == s){
PatrickZieverink 9:6537eead1241 55 y_movement = -1;
PatrickZieverink 9:6537eead1241 56 }
PatrickZieverink 9:6537eead1241 57 while (letter == d) {
PatrickZieverink 9:6537eead1241 58 x_movement = 1;
PatrickZieverink 9:6537eead1241 59 }
PatrickZieverink 9:6537eead1241 60 robot_kinematics() // hoe dat dit precies moet geen idee. Dit lijkt me een goed begin of iig een begin
PatrickZieverink 9:6537eead1241 61 x_movement = 0;
PatrickZieverink 9:6537eead1241 62 y_movement = 0;
PatrickZieverink 9:6537eead1241 63
PatrickZieverink 9:6537eead1241 64
PatrickZieverink 9:6537eead1241 65
PatrickZieverink 9:6537eead1241 66 }
PatrickZieverink 9:6537eead1241 67 void robot_kinematics() { //hoe de motoren moeten draaien als er een x of y richting als input is.
PatrickZieverink 9:6537eead1241 68 [motor.dir1, motor.dir2; motor.speed1, motor.speed2] = H^-1 *[x_movement, y_movement; angle] // Zoiets
PatrickZieverink 9:6537eead1241 69
PatrickZieverink 9:6537eead1241 70
PatrickZieverink 9:6537eead1241 71 }
PatrickZieverink 9:6537eead1241 72 */
PatrickZieverink 9:6537eead1241 73
PatrickZieverink 8:6f6a4dc12036 74 // the funtions needed to run the program
PatrickZieverink 8:6f6a4dc12036 75 void measure_signals() {
PatrickZieverink 9:6537eead1241 76
PatrickZieverink 9:6537eead1241 77 /*
PatrickZieverink 9:6537eead1241 78 letter = pc.getc() // krijg de letter vanuit de keyboard
PatrickZieverink 9:6537eead1241 79 if (letter == w){
PatrickZieverink 9:6537eead1241 80 y_movement = 1;
PatrickZieverink 9:6537eead1241 81 }
PatrickZieverink 9:6537eead1241 82 if (letter == a) {
PatrickZieverink 9:6537eead1241 83 x_movement = -1;
PatrickZieverink 9:6537eead1241 84 }
PatrickZieverink 9:6537eead1241 85 if (letter == s){
PatrickZieverink 9:6537eead1241 86 y_movement = -1;
PatrickZieverink 9:6537eead1241 87 }
PatrickZieverink 9:6537eead1241 88 if (letter == d) {
PatrickZieverink 9:6537eead1241 89 x_movement = 1;
PatrickZieverink 9:6537eead1241 90 }
PatrickZieverink 9:6537eead1241 91
PatrickZieverink 9:6537eead1241 92 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 93 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 94
PatrickZieverink 9:6537eead1241 95
PatrickZieverink 9:6537eead1241 96 */
PatrickZieverink 9:6537eead1241 97
PatrickZieverink 8:6f6a4dc12036 98 pot_1 = Pot1.read();
PatrickZieverink 8:6f6a4dc12036 99 pc.printf("pot_1 = %f",pot_1); // Dit doet hij niet, waarom niet? Er scheelt niet veel aan, maar toch
PatrickZieverink 8:6f6a4dc12036 100 motor.speed1 = pot_1*0.75; //pod_read * 0.75 (dus max 75%)
PatrickZieverink 8:6f6a4dc12036 101
PatrickZieverink 8:6f6a4dc12036 102 pot_2 = Pot2.read();
PatrickZieverink 8:6f6a4dc12036 103 pc.printf("pot_2 = %f",pot_2); // Dit doet hij niet, waarom niet? Er scheelt niet veel aan, maar toch
PatrickZieverink 8:6f6a4dc12036 104 motor.speed2 = pot_2*0.75; //pod_read * 0.75 (dus max 75%)
PatrickZieverink 8:6f6a4dc12036 105 return;
PatrickZieverink 8:6f6a4dc12036 106 }
joostbonekamp 5:aa8b5d5e632f 107
joostbonekamp 5:aa8b5d5e632f 108 void do_nothing() {
PatrickZieverink 8:6f6a4dc12036 109 motor.speed1 = 0;
PatrickZieverink 8:6f6a4dc12036 110 motor.speed2 = 0;
PatrickZieverink 7:78bc59c7753c 111
PatrickZieverink 8:6f6a4dc12036 112 if (but1_pressed == true) { //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
PatrickZieverink 8:6f6a4dc12036 130 if (!state_changed && but1_pressed) { //state wasnt just changed, button 1 has been pressed -> change state
joostbonekamp 5:aa8b5d5e632f 131 current_state = ccw;
joostbonekamp 5:aa8b5d5e632f 132 state_changed = true;
PatrickZieverink 8:6f6a4dc12036 133 but1_pressed = false; //reset this
joostbonekamp 5:aa8b5d5e632f 134 }
PatrickZieverink 8:6f6a4dc12036 135
joostbonekamp 5:aa8b5d5e632f 136 }
PatrickZieverink 7:78bc59c7753c 137
joostbonekamp 5:aa8b5d5e632f 138 void rotate_ccw() {
joostbonekamp 6:354a6509405f 139 //similar to rotate_cw()
joostbonekamp 5:aa8b5d5e632f 140 if (state_changed) {
joostbonekamp 5:aa8b5d5e632f 141 pc.printf("State changed to CCW\r\n");
joostbonekamp 5:aa8b5d5e632f 142 state_changed = false;
joostbonekamp 5:aa8b5d5e632f 143 }
PatrickZieverink 8:6f6a4dc12036 144 motor.dir1 = 0;
joostbonekamp 5:aa8b5d5e632f 145 motor.dir2 = 0;
PatrickZieverink 7:78bc59c7753c 146
PatrickZieverink 8:6f6a4dc12036 147 if (!state_changed && but1_pressed) { //state niet veranderd, button 1 gepressd -> state verandert
joostbonekamp 5:aa8b5d5e632f 148 current_state = cw;
joostbonekamp 5:aa8b5d5e632f 149 state_changed = true;
PatrickZieverink 8:6f6a4dc12036 150 but1_pressed = false;
joostbonekamp 5:aa8b5d5e632f 151 }
joostbonekamp 5:aa8b5d5e632f 152 }
joostbonekamp 5:aa8b5d5e632f 153
PatrickZieverink 8:6f6a4dc12036 154 void motor_controller() {
PatrickZieverink 8:6f6a4dc12036 155 motor1_pwm.period(0.0000625‬f); // 1/frequency van waarop hij draait
PatrickZieverink 8:6f6a4dc12036 156 motor1_direction = motor.dir1; // Zet de richting goed
PatrickZieverink 8:6f6a4dc12036 157 motor1_pwm.write(motor.speed1); // Zet het op de snelheid van motor.speed1
PatrickZieverink 8:6f6a4dc12036 158
PatrickZieverink 8:6f6a4dc12036 159 motor2_pwm.period(0.0000625f); // 1/frequency van waarop hij draait
PatrickZieverink 8:6f6a4dc12036 160 motor2_direction = motor.dir2;
PatrickZieverink 8:6f6a4dc12036 161 motor2_pwm.write(motor.speed2);
PatrickZieverink 8:6f6a4dc12036 162 return;
PatrickZieverink 8:6f6a4dc12036 163 }
PatrickZieverink 8:6f6a4dc12036 164
joostbonekamp 5:aa8b5d5e632f 165 void output() {return;}
joostbonekamp 5:aa8b5d5e632f 166
PatrickZieverink 8:6f6a4dc12036 167 void but1_interrupt () {
PatrickZieverink 8:6f6a4dc12036 168 but1_pressed = true;
PatrickZieverink 8:6f6a4dc12036 169 pc.printf("Button 1 pressed \n\r");
PatrickZieverink 8:6f6a4dc12036 170 }
PatrickZieverink 8:6f6a4dc12036 171
PatrickZieverink 8:6f6a4dc12036 172 void but2_interrupt () {
PatrickZieverink 8:6f6a4dc12036 173 but2_pressed = true;
PatrickZieverink 8:6f6a4dc12036 174 pc.printf("Button 2 pressed \n\r");
joostbonekamp 5:aa8b5d5e632f 175 }
joostbonekamp 5:aa8b5d5e632f 176
joostbonekamp 5:aa8b5d5e632f 177 void state_machine() {
joostbonekamp 5:aa8b5d5e632f 178
PatrickZieverink 8:6f6a4dc12036 179 if (but2_pressed){ // Is dit de goede locatie hiervoor?
PatrickZieverink 8:6f6a4dc12036 180 current_state = idle;
PatrickZieverink 8:6f6a4dc12036 181 but2_pressed = false;
PatrickZieverink 8:6f6a4dc12036 182 pc.printf("Do_nothing happened due to pressing button 2 \n\r");
PatrickZieverink 8:6f6a4dc12036 183 }
PatrickZieverink 8:6f6a4dc12036 184
joostbonekamp 5:aa8b5d5e632f 185 //run current state
joostbonekamp 5:aa8b5d5e632f 186 switch (current_state) {
PatrickZieverink 7:78bc59c7753c 187 case idle: // hoezo de :?
joostbonekamp 5:aa8b5d5e632f 188 do_nothing();
joostbonekamp 5:aa8b5d5e632f 189 break;
joostbonekamp 5:aa8b5d5e632f 190 case cw:
joostbonekamp 5:aa8b5d5e632f 191 rotate_cw();
joostbonekamp 5:aa8b5d5e632f 192 break;
joostbonekamp 5:aa8b5d5e632f 193 case ccw:
joostbonekamp 5:aa8b5d5e632f 194 rotate_ccw();
joostbonekamp 5:aa8b5d5e632f 195 break;
joostbonekamp 5:aa8b5d5e632f 196 case end:
joostbonekamp 5:aa8b5d5e632f 197 break;
joostbonekamp 5:aa8b5d5e632f 198 case failure:
joostbonekamp 5:aa8b5d5e632f 199 break;
joostbonekamp 5:aa8b5d5e632f 200 }
joostbonekamp 5:aa8b5d5e632f 201 }
joostbonekamp 5:aa8b5d5e632f 202
PatrickZieverink 8:6f6a4dc12036 203
joostbonekamp 5:aa8b5d5e632f 204 void main_loop() {
joostbonekamp 5:aa8b5d5e632f 205 measure_signals();
joostbonekamp 5:aa8b5d5e632f 206 state_machine();
joostbonekamp 5:aa8b5d5e632f 207 motor_controller();
joostbonekamp 5:aa8b5d5e632f 208 output();
joostbonekamp 5:aa8b5d5e632f 209 }
joostbonekamp 3:e3d12393adb1 210
joostbonekamp 2:bbaa6fca2ab1 211 int main() {
joostbonekamp 3:e3d12393adb1 212 pc.baud(115200);
PatrickZieverink 8:6f6a4dc12036 213 pc.printf("Executing main()... \r\n");
joostbonekamp 5:aa8b5d5e632f 214 current_state = idle;
joostbonekamp 3:e3d12393adb1 215
joostbonekamp 5:aa8b5d5e632f 216 motor.dir1 = 0;
joostbonekamp 5:aa8b5d5e632f 217 motor.dir2 = 0;
PatrickZieverink 7:78bc59c7753c 218
PatrickZieverink 8:6f6a4dc12036 219 but1.fall(&but1_interrupt);
PatrickZieverink 8:6f6a4dc12036 220 but2.fall(&but2_interrupt);
PatrickZieverink 9:6537eead1241 221 loop_ticker.attach(&main_loop, 0.1f); //main loop at 1kHz (niet per tiende seconde?)
PatrickZieverink 8:6f6a4dc12036 222 pc.printf("Main_loop is running\n \r");
joostbonekamp 5:aa8b5d5e632f 223 while (true) {}
joostbonekamp 6:354a6509405f 224 }