Hidde van der Bijl / Mbed 2 deprecated biorobotics_four_scorers

Kinematics

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

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

Committer:
hidde1104
Date:
Mon Oct 07 13:11:24 2019 +0000
Revision:
11:51ae2da8da55
Parent:
10:b8c60fd468f1 
Kinematics;

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"
hidde1104 11:51ae2da8da55 5 #define PI 3.14159265
RobertoO 0:67c50348f842 6
joostbonekamp 10:b8c60fd468f1 7 Serial pc(USBTX, USBRX); //verbinden met pc
joostbonekamp 4:36e32ddf2443 8 DigitalOut motor2_direction(D4); //verbinden met motor 2 op board (altijd d4)
joostbonekamp 5:aa8b5d5e632f 9 FastPWM motor2_pwm(D5); //verbinden met motor 2 pwm (altijd d5)
joostbonekamp 10:b8c60fd468f1 10 DigitalOut motor1_direction(D6); //verbinden met motor 2 op board (altijd d6)
joostbonekamp 10:b8c60fd468f1 11 FastPWM motor1_pwm(D7); //verbinden met motor 2 pwm (altijd d7)
joostbonekamp 5:aa8b5d5e632f 12 Ticker loop_ticker; //used in main()
PatrickZieverink 8:6f6a4dc12036 13 AnalogIn Pot1(A1); //pot 1 op biorobotics shield
PatrickZieverink 8:6f6a4dc12036 14 AnalogIn Pot2(A0); //pot 2 op biorobotics shield
joostbonekamp 10:b8c60fd468f1 15 AnalogIn joystick_x(A2); //input joystick
joostbonekamp 10:b8c60fd468f1 16 AnalogIn joystick_y(A3); //input joystick
joostbonekamp 10:b8c60fd468f1 17 InterruptIn but1(D10); //knop 1 op birorobotics shield
joostbonekamp 10:b8c60fd468f1 18 InterruptIn but2(D9); //knop 1 op birorobotics shield
PatrickZieverink 8:6f6a4dc12036 19 QEI encoder1 (D11, D12, NC, 8400, QEI::X4_ENCODING); //encoder 1 gebruiken
joostbonekamp 10:b8c60fd468f1 20 QEI encoder2 (D1, D2, NC, 8400, QEI::X4_ENCODING); //encoder 2 gebruiken
joostbonekamp 10:b8c60fd468f1 21
joostbonekamp 3:e3d12393adb1 22
joostbonekamp 5:aa8b5d5e632f 23 //variables
joostbonekamp 5:aa8b5d5e632f 24 enum States {idle, cw, ccw, end, failure};
PatrickZieverink 8:6f6a4dc12036 25 States current_state; //Definieren van de current_state met als keuzes Idle, cw, ccw, end en failure
PatrickZieverink 8:6f6a4dc12036 26
PatrickZieverink 8:6f6a4dc12036 27 class motor_state { // Een emmer met variabelen die je hierna motor gaat noemen.
joostbonekamp 10:b8c60fd468f1 28 public: //variabelen kunnen worden gebruikt door externe functies
joostbonekamp 10:b8c60fd468f1 29 float pwm1; //pwm of 1st motor
joostbonekamp 10:b8c60fd468f1 30 float pwm2; //pwm of 2nd motor
joostbonekamp 10:b8c60fd468f1 31 int dir1; //direction of 1st motor
joostbonekamp 10:b8c60fd468f1 32 int dir2; //direction of 2nd motor
joostbonekamp 5:aa8b5d5e632f 33 };
joostbonekamp 10:b8c60fd468f1 34 motor_state motor; //Een emmer met variabelen met motor.iets als naam bijv. motor.speed1
joostbonekamp 5:aa8b5d5e632f 35
joostbonekamp 10:b8c60fd468f1 36 bool state_changed = false; // wordt gebruikt in de state functies, om te kijken of de state net begint
PatrickZieverink 8:6f6a4dc12036 37 volatile bool but1_pressed = false;
PatrickZieverink 8:6f6a4dc12036 38 volatile bool but2_pressed = false;
joostbonekamp 10:b8c60fd468f1 39 float pot_1;
joostbonekamp 10:b8c60fd468f1 40 float pot_2;
joostbonekamp 10:b8c60fd468f1 41 float x_input;
joostbonekamp 10:b8c60fd468f1 42 float y_input;
joostbonekamp 5:aa8b5d5e632f 43
PatrickZieverink 9:6537eead1241 44 /*
PatrickZieverink 9:6537eead1241 45 float x_movement = 0;
PatrickZieverink 9:6537eead1241 46 float y_movement = 0;
PatrickZieverink 9:6537eead1241 47 float angle = 0;
PatrickZieverink 9:6537eead1241 48 float extention = 0;
PatrickZieverink 9:6537eead1241 49
PatrickZieverink 9:6537eead1241 50 void calibration() { //kalibratie van de sensoren (de nulstand invoeren)
PatrickZieverink 9:6537eead1241 51 letter = pc.getc() // krijg de letter vanuit de keyboard
PatrickZieverink 9:6537eead1241 52 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 53 y_movement = 1;
PatrickZieverink 9:6537eead1241 54 }
PatrickZieverink 9:6537eead1241 55 while (letter == a) {
PatrickZieverink 9:6537eead1241 56 x_movement = -1;
PatrickZieverink 9:6537eead1241 57 }
PatrickZieverink 9:6537eead1241 58 while (letter == s){
PatrickZieverink 9:6537eead1241 59 y_movement = -1;
PatrickZieverink 9:6537eead1241 60 }
PatrickZieverink 9:6537eead1241 61 while (letter == d) {
PatrickZieverink 9:6537eead1241 62 x_movement = 1;
PatrickZieverink 9:6537eead1241 63 }
PatrickZieverink 9:6537eead1241 64 robot_kinematics() // hoe dat dit precies moet geen idee. Dit lijkt me een goed begin of iig een begin
PatrickZieverink 9:6537eead1241 65 x_movement = 0;
PatrickZieverink 9:6537eead1241 66 y_movement = 0;
PatrickZieverink 9:6537eead1241 67
PatrickZieverink 9:6537eead1241 68
PatrickZieverink 9:6537eead1241 69
PatrickZieverink 9:6537eead1241 70 }
PatrickZieverink 9:6537eead1241 71 void robot_kinematics() { //hoe de motoren moeten draaien als er een x of y richting als input is.
PatrickZieverink 9:6537eead1241 72 [motor.dir1, motor.dir2; motor.speed1, motor.speed2] = H^-1 *[x_movement, y_movement; angle] // Zoiets
PatrickZieverink 9:6537eead1241 73
PatrickZieverink 9:6537eead1241 74
PatrickZieverink 9:6537eead1241 75 }
PatrickZieverink 9:6537eead1241 76 */
PatrickZieverink 9:6537eead1241 77
PatrickZieverink 8:6f6a4dc12036 78 // the funtions needed to run the program
PatrickZieverink 8:6f6a4dc12036 79 void measure_signals() {
joostbonekamp 10:b8c60fd468f1 80 x_input = joystick_x.read();
joostbonekamp 10:b8c60fd468f1 81 y_input = joystick_y.read();
joostbonekamp 10:b8c60fd468f1 82
PatrickZieverink 9:6537eead1241 83 /*
PatrickZieverink 9:6537eead1241 84 letter = pc.getc() // krijg de letter vanuit de keyboard
PatrickZieverink 9:6537eead1241 85 if (letter == w){
PatrickZieverink 9:6537eead1241 86 y_movement = 1;
PatrickZieverink 9:6537eead1241 87 }
PatrickZieverink 9:6537eead1241 88 if (letter == a) {
PatrickZieverink 9:6537eead1241 89 x_movement = -1;
PatrickZieverink 9:6537eead1241 90 }
PatrickZieverink 9:6537eead1241 91 if (letter == s){
PatrickZieverink 9:6537eead1241 92 y_movement = -1;
PatrickZieverink 9:6537eead1241 93 }
PatrickZieverink 9:6537eead1241 94 if (letter == d) {
PatrickZieverink 9:6537eead1241 95 x_movement = 1;
PatrickZieverink 9:6537eead1241 96 }
PatrickZieverink 9:6537eead1241 97
PatrickZieverink 9:6537eead1241 98 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 99 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 100
PatrickZieverink 9:6537eead1241 101
PatrickZieverink 9:6537eead1241 102 */
PatrickZieverink 9:6537eead1241 103
PatrickZieverink 8:6f6a4dc12036 104 pot_1 = Pot1.read();
PatrickZieverink 8:6f6a4dc12036 105 pc.printf("pot_1 = %f",pot_1); // Dit doet hij niet, waarom niet? Er scheelt niet veel aan, maar toch
PatrickZieverink 8:6f6a4dc12036 106 motor.speed1 = pot_1*0.75; //pod_read * 0.75 (dus max 75%)
PatrickZieverink 8:6f6a4dc12036 107
PatrickZieverink 8:6f6a4dc12036 108 pot_2 = Pot2.read();
PatrickZieverink 8:6f6a4dc12036 109 pc.printf("pot_2 = %f",pot_2); // Dit doet hij niet, waarom niet? Er scheelt niet veel aan, maar toch
PatrickZieverink 8:6f6a4dc12036 110 motor.speed2 = pot_2*0.75; //pod_read * 0.75 (dus max 75%)
PatrickZieverink 8:6f6a4dc12036 111 return;
PatrickZieverink 8:6f6a4dc12036 112 }
joostbonekamp 5:aa8b5d5e632f 113
joostbonekamp 5:aa8b5d5e632f 114 void do_nothing() {
PatrickZieverink 8:6f6a4dc12036 115 motor.speed1 = 0;
PatrickZieverink 8:6f6a4dc12036 116 motor.speed2 = 0;
PatrickZieverink 7:78bc59c7753c 117
joostbonekamp 10:b8c60fd468f1 118
joostbonekamp 10:b8c60fd468f1 119 //state guard
joostbonekamp 10:b8c60fd468f1 120 if (but1_pressed) { //this moves the program from the idle to cw state
joostbonekamp 5:aa8b5d5e632f 121 current_state = cw;
joostbonekamp 6:354a6509405f 122 state_changed = true; //to show next state it can initialize
joostbonekamp 5:aa8b5d5e632f 123 pc.printf("Changed state from idle to cw\r\n");
PatrickZieverink 8:6f6a4dc12036 124 but1_pressed = false; //reset button 1
joostbonekamp 5:aa8b5d5e632f 125 }
PatrickZieverink 7:78bc59c7753c 126
joostbonekamp 5:aa8b5d5e632f 127 }
PatrickZieverink 7:78bc59c7753c 128
hidde1104 11:51ae2da8da55 129 void kinematics1() { //
hidde1104 11:51ae2da8da55 130 double length_1;
hidde1104 11:51ae2da8da55 131 volatile double theta;
hidde1104 11:51ae2da8da55 132 volatile double length_2;
hidde1104 11:51ae2da8da55 133
hidde1104 11:51ae2da8da55 134 class H_matrix { //rows_columns (als je het niet ziet, gebruik dat de onderste rij 0 0 1 is)
hidde1104 11:51ae2da8da55 135 public:
hidde1104 11:51ae2da8da55 136 int h1_1_1 = 1;
hidde1104 11:51ae2da8da55 137 int h1_1_2 = 0;
hidde1104 11:51ae2da8da55 138 float h1_1_3 = sin(theta*PI/180)*(length_1+length_2); //sinus en cosinus zijn standaard gedefinieërd in radians, maar theta is hier dus in graden
hidde1104 11:51ae2da8da55 139 int h1_2_1 = 0;
hidde1104 11:51ae2da8da55 140 int h1_2_2 = 1;
hidde1104 11:51ae2da8da55 141 float h1_2_3 = cos(theta*PI/180)*(length_1+length_2);
hidde1104 11:51ae2da8da55 142 int h1_3_1 = 0;
hidde1104 11:51ae2da8da55 143 int h1_3_2 = 0;
hidde1104 11:51ae2da8da55 144 int h1_3_3 = 1;
hidde1104 11:51ae2da8da55 145 }
hidde1104 11:51ae2da8da55 146 H_matrix H;
hidde1104 11:51ae2da8da55 147
hidde1104 11:51ae2da8da55 148 class Position_vector1 { //positie vector gezien vanuit het uiteinde van de complete arm (coördinatiestelsel draait niet mee met de arm)
hidde1104 11:51ae2da8da55 149 public:
hidde1104 11:51ae2da8da55 150 float p1_1_1
hidde1104 11:51ae2da8da55 151 float p1_2_1
hidde1104 11:51ae2da8da55 152 float p1_3_1
hidde1104 11:51ae2da8da55 153 }
hidde1104 11:51ae2da8da55 154 class Position_vector2 {
hidde1104 11:51ae2da8da55 155 public:
hidde1104 11:51ae2da8da55 156 float p2_1_1 = h1_1_1*p1_1_1 + h1_1_2*p1_2_1 + h1_1_3*p1_3_1
hidde1104 11:51ae2da8da55 157 float p2_2_1 = h1_2_1*p1_1_1 + h1_2_2*p1_2_1 + h1_2_3*p1_3_1
hidde1104 11:51ae2da8da55 158 float p2_3_1 = h1_3_1*p1_1_1 + h1_3_2*p1_2_1 + h1_3_3*p1_3_1
hidde1104 11:51ae2da8da55 159 }
hidde1104 11:51ae2da8da55 160 }
PatrickZieverink 7:78bc59c7753c 161
hidde1104 11:51ae2da8da55 162
hidde1104 11:51ae2da8da55 163 void kinematics2() { //
hidde1104 11:51ae2da8da55 164 double length_1;
hidde1104 11:51ae2da8da55 165 volatile double theta;
hidde1104 11:51ae2da8da55 166 volatile double length_2;
hidde1104 11:51ae2da8da55 167
hidde1104 11:51ae2da8da55 168 class H_matrix2 { //rows_columns (als je het niet ziet, gebruik dat de onderste rij 0 0 1 is)
hidde1104 11:51ae2da8da55 169 public:
hidde1104 11:51ae2da8da55 170 int h2_1_1 = 1;
hidde1104 11:51ae2da8da55 171 int h2_1_2 = 0;
hidde1104 11:51ae2da8da55 172 float h2_1_3 = -sin(theta*PI/180)*(length_1+length_2); //sinus en cosinus zijn standaard gedefinieërd in radians, maar theta is hier dus in graden
hidde1104 11:51ae2da8da55 173 int h2_2_1 = 0;
hidde1104 11:51ae2da8da55 174 int h2_2_2 = 1;
hidde1104 11:51ae2da8da55 175 float h2_2_3 = -cos(theta*PI/180)*(length_1+length_2);
hidde1104 11:51ae2da8da55 176 int h2_3_1 = 0;
hidde1104 11:51ae2da8da55 177 int h2_3_2 = 0;
hidde1104 11:51ae2da8da55 178 int h2_3_3 = 1;
hidde1104 11:51ae2da8da55 179 }
hidde1104 11:51ae2da8da55 180 H_matrix H;
hidde1104 11:51ae2da8da55 181
hidde1104 11:51ae2da8da55 182 class Position_vector3 { //positie vector gezien vanuit het onderste draaipunt
hidde1104 11:51ae2da8da55 183 public:
hidde1104 11:51ae2da8da55 184 float p3_1_1
hidde1104 11:51ae2da8da55 185 float p3_2_1
hidde1104 11:51ae2da8da55 186 float p3_3_1
hidde1104 11:51ae2da8da55 187 }
hidde1104 11:51ae2da8da55 188 class Position_vector4 {
hidde1104 11:51ae2da8da55 189 public:
hidde1104 11:51ae2da8da55 190 float p4_1_1 = h2_1_1*p3_1_1 + h2_1_2*p3_2_1 + h2_1_3*p3_3_1
hidde1104 11:51ae2da8da55 191 float p4_2_1 = h2_2_1*p1_1_1 + h2_2_2*p1_2_1 + h2_2_3*p3_3_1
hidde1104 11:51ae2da8da55 192 float p4_3_1 = h2_3_1*p1_1_1 + h2_3_2*p1_2_1 + h2_3_3*p3_3_1
hidde1104 11:51ae2da8da55 193 }
hidde1104 11:51ae2da8da55 194 H_matrix2 H2
hidde1104 11:51ae2da8da55 195 }
hidde1104 11:51ae2da8da55 196
joostbonekamp 5:aa8b5d5e632f 197 void rotate_cw() {
joostbonekamp 5:aa8b5d5e632f 198 if (state_changed) {
joostbonekamp 5:aa8b5d5e632f 199 pc.printf("State changed to CW\r\n");
joostbonekamp 6:354a6509405f 200 state_changed = false; //reset this so it wont print next loop
joostbonekamp 5:aa8b5d5e632f 201 }
PatrickZieverink 8:6f6a4dc12036 202 motor.dir1 = 1; //todo: check if this is actually clockwise
PatrickZieverink 7:78bc59c7753c 203 motor.dir2 = 1; //todo: check if this is actually clockwise
PatrickZieverink 7:78bc59c7753c 204
joostbonekamp 10:b8c60fd468f1 205 motor.pwm1 = x_input; //nog niet echt de x, maar de rotatie, moet later met kinematics gefixt worden
joostbonekamp 10:b8c60fd468f1 206 motor.pwm2 = y_input; // ook nog niet echt de y dus
joostbonekamp 10:b8c60fd468f1 207 //state guard
PatrickZieverink 8:6f6a4dc12036 208 if (!state_changed && but1_pressed) { //state wasnt just changed, button 1 has been pressed -> change state
joostbonekamp 5:aa8b5d5e632f 209 current_state = ccw;
joostbonekamp 5:aa8b5d5e632f 210 state_changed = true;
PatrickZieverink 8:6f6a4dc12036 211 but1_pressed = false; //reset this
joostbonekamp 5:aa8b5d5e632f 212 }
PatrickZieverink 8:6f6a4dc12036 213
joostbonekamp 5:aa8b5d5e632f 214 }
PatrickZieverink 7:78bc59c7753c 215
joostbonekamp 5:aa8b5d5e632f 216 void rotate_ccw() {
joostbonekamp 6:354a6509405f 217 //similar to rotate_cw()
joostbonekamp 5:aa8b5d5e632f 218 if (state_changed) {
joostbonekamp 5:aa8b5d5e632f 219 pc.printf("State changed to CCW\r\n");
joostbonekamp 5:aa8b5d5e632f 220 state_changed = false;
joostbonekamp 5:aa8b5d5e632f 221 }
PatrickZieverink 8:6f6a4dc12036 222 motor.dir1 = 0;
joostbonekamp 5:aa8b5d5e632f 223 motor.dir2 = 0;
joostbonekamp 10:b8c60fd468f1 224
joostbonekamp 10:b8c60fd468f1 225 motor.pwm1 = x_input; //nog niet echt de x
joostbonekamp 10:b8c60fd468f1 226 motor.pwm2 = y_input; //nog niet echt de y
joostbonekamp 10:b8c60fd468f1 227
joostbonekamp 10:b8c60fd468f1 228 //state guard
PatrickZieverink 8:6f6a4dc12036 229 if (!state_changed && but1_pressed) { //state niet veranderd, button 1 gepressd -> state verandert
joostbonekamp 5:aa8b5d5e632f 230 current_state = cw;
joostbonekamp 5:aa8b5d5e632f 231 state_changed = true;
PatrickZieverink 8:6f6a4dc12036 232 but1_pressed = false;
joostbonekamp 5:aa8b5d5e632f 233 }
joostbonekamp 5:aa8b5d5e632f 234 }
joostbonekamp 5:aa8b5d5e632f 235
PatrickZieverink 8:6f6a4dc12036 236 void motor_controller() {
joostbonekamp 10:b8c60fd468f1 237
PatrickZieverink 8:6f6a4dc12036 238 motor1_direction = motor.dir1; // Zet de richting goed
joostbonekamp 10:b8c60fd468f1 239 motor1_pwm.write(motor.pwm1); // Zet het op de snelheid van motor.speed1
PatrickZieverink 8:6f6a4dc12036 240
joostbonekamp 10:b8c60fd468f1 241
PatrickZieverink 8:6f6a4dc12036 242 motor2_direction = motor.dir2;
joostbonekamp 10:b8c60fd468f1 243 motor2_pwm.write(motor.pwm2);
PatrickZieverink 8:6f6a4dc12036 244 return;
joostbonekamp 10:b8c60fd468f1 245 }
PatrickZieverink 8:6f6a4dc12036 246
joostbonekamp 5:aa8b5d5e632f 247 void output() {return;}
joostbonekamp 5:aa8b5d5e632f 248
PatrickZieverink 8:6f6a4dc12036 249 void but1_interrupt () {
PatrickZieverink 8:6f6a4dc12036 250 but1_pressed = true;
PatrickZieverink 8:6f6a4dc12036 251 pc.printf("Button 1 pressed \n\r");
PatrickZieverink 8:6f6a4dc12036 252 }
PatrickZieverink 8:6f6a4dc12036 253
PatrickZieverink 8:6f6a4dc12036 254 void but2_interrupt () {
PatrickZieverink 8:6f6a4dc12036 255 but2_pressed = true;
PatrickZieverink 8:6f6a4dc12036 256 pc.printf("Button 2 pressed \n\r");
joostbonekamp 5:aa8b5d5e632f 257 }
joostbonekamp 5:aa8b5d5e632f 258
joostbonekamp 5:aa8b5d5e632f 259 void state_machine() {
joostbonekamp 5:aa8b5d5e632f 260
PatrickZieverink 8:6f6a4dc12036 261 if (but2_pressed){ // Is dit de goede locatie hiervoor?
PatrickZieverink 8:6f6a4dc12036 262 current_state = idle;
PatrickZieverink 8:6f6a4dc12036 263 but2_pressed = false;
PatrickZieverink 8:6f6a4dc12036 264 pc.printf("Do_nothing happened due to pressing button 2 \n\r");
PatrickZieverink 8:6f6a4dc12036 265 }
PatrickZieverink 8:6f6a4dc12036 266
joostbonekamp 5:aa8b5d5e632f 267 //run current state
joostbonekamp 5:aa8b5d5e632f 268 switch (current_state) {
PatrickZieverink 7:78bc59c7753c 269 case idle: // hoezo de :?
joostbonekamp 5:aa8b5d5e632f 270 do_nothing();
joostbonekamp 5:aa8b5d5e632f 271 break;
joostbonekamp 5:aa8b5d5e632f 272 case cw:
joostbonekamp 5:aa8b5d5e632f 273 rotate_cw();
joostbonekamp 5:aa8b5d5e632f 274 break;
joostbonekamp 5:aa8b5d5e632f 275 case ccw:
joostbonekamp 5:aa8b5d5e632f 276 rotate_ccw();
joostbonekamp 5:aa8b5d5e632f 277 break;
joostbonekamp 5:aa8b5d5e632f 278 case end:
joostbonekamp 5:aa8b5d5e632f 279 break;
joostbonekamp 5:aa8b5d5e632f 280 case failure:
joostbonekamp 5:aa8b5d5e632f 281 break;
joostbonekamp 5:aa8b5d5e632f 282 }
joostbonekamp 5:aa8b5d5e632f 283 }
joostbonekamp 5:aa8b5d5e632f 284
PatrickZieverink 8:6f6a4dc12036 285
joostbonekamp 5:aa8b5d5e632f 286 void main_loop() {
joostbonekamp 5:aa8b5d5e632f 287 measure_signals();
joostbonekamp 5:aa8b5d5e632f 288 state_machine();
joostbonekamp 5:aa8b5d5e632f 289 motor_controller();
joostbonekamp 5:aa8b5d5e632f 290 output();
joostbonekamp 5:aa8b5d5e632f 291 }
joostbonekamp 3:e3d12393adb1 292
joostbonekamp 2:bbaa6fca2ab1 293 int main() {
joostbonekamp 3:e3d12393adb1 294 pc.baud(115200);
PatrickZieverink 8:6f6a4dc12036 295 pc.printf("Executing main()... \r\n");
joostbonekamp 5:aa8b5d5e632f 296 current_state = idle;
joostbonekamp 3:e3d12393adb1 297
joostbonekamp 5:aa8b5d5e632f 298 motor.dir1 = 0;
joostbonekamp 5:aa8b5d5e632f 299 motor.dir2 = 0;
joostbonekamp 10:b8c60fd468f1 300 motor2_pwm.period(0.0000625f); // 1/frequency van waarop hij draait
joostbonekamp 10:b8c60fd468f1 301 motor1_pwm.period(0.0000625f); // 1/frequency van waarop hij draait
PatrickZieverink 7:78bc59c7753c 302
PatrickZieverink 8:6f6a4dc12036 303 but1.fall(&but1_interrupt);
PatrickZieverink 8:6f6a4dc12036 304 but2.fall(&but2_interrupt);
PatrickZieverink 9:6537eead1241 305 loop_ticker.attach(&main_loop, 0.1f); //main loop at 1kHz (niet per tiende seconde?)
PatrickZieverink 8:6f6a4dc12036 306 pc.printf("Main_loop is running\n \r");
joostbonekamp 5:aa8b5d5e632f 307 while (true) {}
joostbonekamp 6:354a6509405f 308 }