Patrick Zieverink / Mbed 2 deprecated biorobotics_four_scorers_Patrick

Werkt dit?

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

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

Committer:
PatrickZieverink
Date:
Mon Oct 07 08:45:03 2019 +0000
Revision:
11:1166af209e55
Parent:
10:b8c60fd468f1 
Versie waarin weinig werkt;

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
PatrickZieverink 11:1166af209e55 28 float dutycycle1; //pwm of 1st motor
PatrickZieverink 11:1166af209e55 29 float dutycycle2; //pwm of 2nd motor
joostbonekamp 10:b8c60fd468f1 30 int dir1; //direction of 1st motor
joostbonekamp 10:b8c60fd468f1 31 int dir2; //direction of 2nd motor
PatrickZieverink 11:1166af209e55 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 11:1166af209e55 47 float extension = 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 11:1166af209e55 63 while (int pc.getc() != 'p') {
PatrickZieverink 11:1166af209e55 64 robot_kinematics() // hoe dat dit precies moet geen idee. Dit lijkt me een goed begin of iig een begin
PatrickZieverink 11:1166af209e55 65 }
PatrickZieverink 9:6537eead1241 66 x_movement = 0;
PatrickZieverink 9:6537eead1241 67 y_movement = 0;
PatrickZieverink 9:6537eead1241 68
PatrickZieverink 9:6537eead1241 69
PatrickZieverink 9:6537eead1241 70
PatrickZieverink 9:6537eead1241 71 }
PatrickZieverink 9:6537eead1241 72 void robot_kinematics() { //hoe de motoren moeten draaien als er een x of y richting als input is.
PatrickZieverink 11:1166af209e55 73 [motor.dir1, motor.dir2; motor.speed1, motor.speed2] = H^-1 *[x_movement, y_movement; angle, extension] // Zoiets
PatrickZieverink 9:6537eead1241 74
PatrickZieverink 9:6537eead1241 75
PatrickZieverink 9:6537eead1241 76 }
PatrickZieverink 9:6537eead1241 77 */
PatrickZieverink 9:6537eead1241 78
PatrickZieverink 8:6f6a4dc12036 79 // the funtions needed to run the program
PatrickZieverink 8:6f6a4dc12036 80 void measure_signals() {
PatrickZieverink 11:1166af209e55 81 /*
joostbonekamp 10:b8c60fd468f1 82 x_input = joystick_x.read();
PatrickZieverink 11:1166af209e55 83 pc.printf("X reading is %f" x_input);
joostbonekamp 10:b8c60fd468f1 84 y_input = joystick_y.read();
PatrickZieverink 11:1166af209e55 85 pc.printf("Y reading is %f", y_input);
joostbonekamp 10:b8c60fd468f1 86
PatrickZieverink 11:1166af209e55 87
PatrickZieverink 9:6537eead1241 88 letter = pc.getc() // krijg de letter vanuit de keyboard
PatrickZieverink 9:6537eead1241 89 if (letter == w){
PatrickZieverink 9:6537eead1241 90 y_movement = 1;
PatrickZieverink 9:6537eead1241 91 }
PatrickZieverink 9:6537eead1241 92 if (letter == a) {
PatrickZieverink 9:6537eead1241 93 x_movement = -1;
PatrickZieverink 9:6537eead1241 94 }
PatrickZieverink 9:6537eead1241 95 if (letter == s){
PatrickZieverink 9:6537eead1241 96 y_movement = -1;
PatrickZieverink 9:6537eead1241 97 }
PatrickZieverink 9:6537eead1241 98 if (letter == d) {
PatrickZieverink 9:6537eead1241 99 x_movement = 1;
PatrickZieverink 9:6537eead1241 100 }
PatrickZieverink 9:6537eead1241 101
PatrickZieverink 9:6537eead1241 102 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 11:1166af209e55 103 extension = 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 104
PatrickZieverink 9:6537eead1241 105
PatrickZieverink 9:6537eead1241 106 */
PatrickZieverink 9:6537eead1241 107
PatrickZieverink 8:6f6a4dc12036 108 pot_1 = Pot1.read();
PatrickZieverink 11:1166af209e55 109 motor.dutycycle1 = pot_1*0.75; //pod_read * 0.75 (dus max 75%)
PatrickZieverink 8:6f6a4dc12036 110
PatrickZieverink 8:6f6a4dc12036 111 pot_2 = Pot2.read();
PatrickZieverink 11:1166af209e55 112 motor.dutycycle2 = pot_2*0.75; //pod_read * 0.75 (dus max 75%)
PatrickZieverink 8:6f6a4dc12036 113 return;
PatrickZieverink 8:6f6a4dc12036 114 }
joostbonekamp 5:aa8b5d5e632f 115
joostbonekamp 5:aa8b5d5e632f 116 void do_nothing() {
PatrickZieverink 11:1166af209e55 117 motor.dutycycle1 = 0;
PatrickZieverink 11:1166af209e55 118 motor.dutycycle2 = 0;
PatrickZieverink 7:78bc59c7753c 119
joostbonekamp 10:b8c60fd468f1 120
joostbonekamp 10:b8c60fd468f1 121 //state guard
joostbonekamp 10:b8c60fd468f1 122 if (but1_pressed) { //this moves the program from the idle to cw state
joostbonekamp 5:aa8b5d5e632f 123 current_state = cw;
joostbonekamp 6:354a6509405f 124 state_changed = true; //to show next state it can initialize
joostbonekamp 5:aa8b5d5e632f 125 pc.printf("Changed state from idle to cw\r\n");
PatrickZieverink 8:6f6a4dc12036 126 but1_pressed = false; //reset button 1
joostbonekamp 5:aa8b5d5e632f 127 }
PatrickZieverink 7:78bc59c7753c 128
joostbonekamp 5:aa8b5d5e632f 129 }
PatrickZieverink 7:78bc59c7753c 130
PatrickZieverink 7:78bc59c7753c 131
joostbonekamp 5:aa8b5d5e632f 132 void rotate_cw() {
joostbonekamp 5:aa8b5d5e632f 133 if (state_changed) {
joostbonekamp 5:aa8b5d5e632f 134 pc.printf("State changed to CW\r\n");
joostbonekamp 6:354a6509405f 135 state_changed = false; //reset this so it wont print next loop
joostbonekamp 5:aa8b5d5e632f 136 }
PatrickZieverink 8:6f6a4dc12036 137 motor.dir1 = 1; //todo: check if this is actually clockwise
PatrickZieverink 7:78bc59c7753c 138 motor.dir2 = 1; //todo: check if this is actually clockwise
PatrickZieverink 7:78bc59c7753c 139
PatrickZieverink 11:1166af209e55 140 /// motor.dutycycle1 = x_input; //nog niet echt de x, maar de rotatie, moet later met kinematics gefixt worden
PatrickZieverink 11:1166af209e55 141 /// motor.dutycycle2 = y_input; // ook nog niet echt de y dus
PatrickZieverink 11:1166af209e55 142
joostbonekamp 10:b8c60fd468f1 143 //state guard
PatrickZieverink 8:6f6a4dc12036 144 if (!state_changed && but1_pressed) { //state wasnt just changed, button 1 has been pressed -> change state
joostbonekamp 5:aa8b5d5e632f 145 current_state = ccw;
joostbonekamp 5:aa8b5d5e632f 146 state_changed = true;
PatrickZieverink 8:6f6a4dc12036 147 but1_pressed = false; //reset this
joostbonekamp 5:aa8b5d5e632f 148 }
PatrickZieverink 8:6f6a4dc12036 149
joostbonekamp 5:aa8b5d5e632f 150 }
PatrickZieverink 7:78bc59c7753c 151
joostbonekamp 5:aa8b5d5e632f 152 void rotate_ccw() {
joostbonekamp 6:354a6509405f 153 //similar to rotate_cw()
joostbonekamp 5:aa8b5d5e632f 154 if (state_changed) {
joostbonekamp 5:aa8b5d5e632f 155 pc.printf("State changed to CCW\r\n");
joostbonekamp 5:aa8b5d5e632f 156 state_changed = false;
joostbonekamp 5:aa8b5d5e632f 157 }
PatrickZieverink 8:6f6a4dc12036 158 motor.dir1 = 0;
joostbonekamp 5:aa8b5d5e632f 159 motor.dir2 = 0;
joostbonekamp 10:b8c60fd468f1 160
PatrickZieverink 11:1166af209e55 161 /// motor.dutycycle1 = x_input; //nog niet echt de x
PatrickZieverink 11:1166af209e55 162 /// motor.dutycycle2 = y_input; //nog niet echt de y
joostbonekamp 10:b8c60fd468f1 163
joostbonekamp 10:b8c60fd468f1 164 //state guard
PatrickZieverink 8:6f6a4dc12036 165 if (!state_changed && but1_pressed) { //state niet veranderd, button 1 gepressd -> state verandert
joostbonekamp 5:aa8b5d5e632f 166 current_state = cw;
joostbonekamp 5:aa8b5d5e632f 167 state_changed = true;
PatrickZieverink 8:6f6a4dc12036 168 but1_pressed = false;
joostbonekamp 5:aa8b5d5e632f 169 }
joostbonekamp 5:aa8b5d5e632f 170 }
joostbonekamp 5:aa8b5d5e632f 171
PatrickZieverink 8:6f6a4dc12036 172 void motor_controller() {
joostbonekamp 10:b8c60fd468f1 173
PatrickZieverink 8:6f6a4dc12036 174 motor1_direction = motor.dir1; // Zet de richting goed
PatrickZieverink 11:1166af209e55 175 motor1_pwm.write(motor.dutycycle1); // Zet het op de snelheid van motor.speed1
PatrickZieverink 11:1166af209e55 176
joostbonekamp 10:b8c60fd468f1 177
PatrickZieverink 8:6f6a4dc12036 178 motor2_direction = motor.dir2;
PatrickZieverink 11:1166af209e55 179 motor2_pwm.write(motor.dutycycle2);
PatrickZieverink 8:6f6a4dc12036 180 return;
joostbonekamp 10:b8c60fd468f1 181 }
PatrickZieverink 8:6f6a4dc12036 182
joostbonekamp 5:aa8b5d5e632f 183 void output() {return;}
joostbonekamp 5:aa8b5d5e632f 184
PatrickZieverink 8:6f6a4dc12036 185 void but1_interrupt () {
PatrickZieverink 8:6f6a4dc12036 186 but1_pressed = true;
PatrickZieverink 8:6f6a4dc12036 187 pc.printf("Button 1 pressed \n\r");
PatrickZieverink 8:6f6a4dc12036 188 }
PatrickZieverink 8:6f6a4dc12036 189
PatrickZieverink 8:6f6a4dc12036 190 void but2_interrupt () {
PatrickZieverink 8:6f6a4dc12036 191 but2_pressed = true;
PatrickZieverink 8:6f6a4dc12036 192 pc.printf("Button 2 pressed \n\r");
joostbonekamp 5:aa8b5d5e632f 193 }
joostbonekamp 5:aa8b5d5e632f 194
joostbonekamp 5:aa8b5d5e632f 195 void state_machine() {
joostbonekamp 5:aa8b5d5e632f 196
PatrickZieverink 8:6f6a4dc12036 197 if (but2_pressed){ // Is dit de goede locatie hiervoor?
PatrickZieverink 8:6f6a4dc12036 198 current_state = idle;
PatrickZieverink 8:6f6a4dc12036 199 but2_pressed = false;
PatrickZieverink 8:6f6a4dc12036 200 pc.printf("Do_nothing happened due to pressing button 2 \n\r");
PatrickZieverink 8:6f6a4dc12036 201 }
PatrickZieverink 8:6f6a4dc12036 202
joostbonekamp 5:aa8b5d5e632f 203 //run current state
joostbonekamp 5:aa8b5d5e632f 204 switch (current_state) {
PatrickZieverink 7:78bc59c7753c 205 case idle: // hoezo de :?
joostbonekamp 5:aa8b5d5e632f 206 do_nothing();
joostbonekamp 5:aa8b5d5e632f 207 break;
joostbonekamp 5:aa8b5d5e632f 208 case cw:
joostbonekamp 5:aa8b5d5e632f 209 rotate_cw();
joostbonekamp 5:aa8b5d5e632f 210 break;
joostbonekamp 5:aa8b5d5e632f 211 case ccw:
joostbonekamp 5:aa8b5d5e632f 212 rotate_ccw();
joostbonekamp 5:aa8b5d5e632f 213 break;
joostbonekamp 5:aa8b5d5e632f 214 case end:
joostbonekamp 5:aa8b5d5e632f 215 break;
joostbonekamp 5:aa8b5d5e632f 216 case failure:
joostbonekamp 5:aa8b5d5e632f 217 break;
joostbonekamp 5:aa8b5d5e632f 218 }
joostbonekamp 5:aa8b5d5e632f 219 }
joostbonekamp 5:aa8b5d5e632f 220
PatrickZieverink 8:6f6a4dc12036 221
joostbonekamp 5:aa8b5d5e632f 222 void main_loop() {
joostbonekamp 5:aa8b5d5e632f 223 measure_signals();
joostbonekamp 5:aa8b5d5e632f 224 state_machine();
joostbonekamp 5:aa8b5d5e632f 225 motor_controller();
joostbonekamp 5:aa8b5d5e632f 226 output();
joostbonekamp 5:aa8b5d5e632f 227 }
joostbonekamp 3:e3d12393adb1 228
joostbonekamp 2:bbaa6fca2ab1 229 int main() {
joostbonekamp 3:e3d12393adb1 230 pc.baud(115200);
PatrickZieverink 8:6f6a4dc12036 231 pc.printf("Executing main()... \r\n");
joostbonekamp 5:aa8b5d5e632f 232 current_state = idle;
joostbonekamp 3:e3d12393adb1 233
joostbonekamp 5:aa8b5d5e632f 234 motor.dir1 = 0;
joostbonekamp 5:aa8b5d5e632f 235 motor.dir2 = 0;
joostbonekamp 10:b8c60fd468f1 236 motor2_pwm.period(0.0000625f); // 1/frequency van waarop hij draait
joostbonekamp 10:b8c60fd468f1 237 motor1_pwm.period(0.0000625f); // 1/frequency van waarop hij draait
PatrickZieverink 7:78bc59c7753c 238
PatrickZieverink 8:6f6a4dc12036 239 but1.fall(&but1_interrupt);
PatrickZieverink 8:6f6a4dc12036 240 but2.fall(&but2_interrupt);
PatrickZieverink 9:6537eead1241 241 loop_ticker.attach(&main_loop, 0.1f); //main loop at 1kHz (niet per tiende seconde?)
PatrickZieverink 8:6f6a4dc12036 242 pc.printf("Main_loop is running\n \r");
joostbonekamp 5:aa8b5d5e632f 243 while (true) {}
joostbonekamp 6:354a6509405f 244 }