Patrick Zieverink / Mbed 2 deprecated biorobotics_four_scorers_Patrick

Werkt dit?

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@8:6f6a4dc12036, 2019-10-04 (annotated)

Committer:
PatrickZieverink
Date:
Fri Oct 04 17:44:30 2019 +0000
Revision:
8:6f6a4dc12036
Parent:
7:78bc59c7753c
Child:
9:6537eead1241
Werkend met motoren, of ze beide werken geen idee;

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 8:6f6a4dc12036 40 // the funtions needed to run the program
PatrickZieverink 8:6f6a4dc12036 41 void measure_signals() {
PatrickZieverink 8:6f6a4dc12036 42 pot_1 = Pot1.read();
PatrickZieverink 8:6f6a4dc12036 43 pc.printf("pot_1 = %f",pot_1); // Dit doet hij niet, waarom niet? Er scheelt niet veel aan, maar toch
PatrickZieverink 8:6f6a4dc12036 44 motor.speed1 = pot_1*0.75; //pod_read * 0.75 (dus max 75%)
PatrickZieverink 8:6f6a4dc12036 45
PatrickZieverink 8:6f6a4dc12036 46 pot_2 = Pot2.read();
PatrickZieverink 8:6f6a4dc12036 47 pc.printf("pot_2 = %f",pot_2); // Dit doet hij niet, waarom niet? Er scheelt niet veel aan, maar toch
PatrickZieverink 8:6f6a4dc12036 48 motor.speed2 = pot_2*0.75; //pod_read * 0.75 (dus max 75%)
PatrickZieverink 8:6f6a4dc12036 49 return;
PatrickZieverink 8:6f6a4dc12036 50 }
joostbonekamp 5:aa8b5d5e632f 51
joostbonekamp 5:aa8b5d5e632f 52 void do_nothing() {
PatrickZieverink 8:6f6a4dc12036 53 motor.speed1 = 0;
PatrickZieverink 8:6f6a4dc12036 54 motor.speed2 = 0;
PatrickZieverink 7:78bc59c7753c 55
PatrickZieverink 8:6f6a4dc12036 56 if (but1_pressed == true) { //this moves the program from the idle to cw state
joostbonekamp 5:aa8b5d5e632f 57 current_state = cw;
joostbonekamp 6:354a6509405f 58 state_changed = true; //to show next state it can initialize
joostbonekamp 5:aa8b5d5e632f 59 pc.printf("Changed state from idle to cw\r\n");
PatrickZieverink 8:6f6a4dc12036 60 but1_pressed = false; //reset button 1
joostbonekamp 5:aa8b5d5e632f 61 }
PatrickZieverink 7:78bc59c7753c 62
joostbonekamp 5:aa8b5d5e632f 63 }
PatrickZieverink 7:78bc59c7753c 64
PatrickZieverink 7:78bc59c7753c 65
joostbonekamp 5:aa8b5d5e632f 66 void rotate_cw() {
joostbonekamp 5:aa8b5d5e632f 67 if (state_changed) {
joostbonekamp 5:aa8b5d5e632f 68 pc.printf("State changed to CW\r\n");
joostbonekamp 6:354a6509405f 69 state_changed = false; //reset this so it wont print next loop
joostbonekamp 5:aa8b5d5e632f 70 }
PatrickZieverink 8:6f6a4dc12036 71 motor.dir1 = 1; //todo: check if this is actually clockwise
PatrickZieverink 7:78bc59c7753c 72 motor.dir2 = 1; //todo: check if this is actually clockwise
PatrickZieverink 7:78bc59c7753c 73
PatrickZieverink 8:6f6a4dc12036 74 if (!state_changed && but1_pressed) { //state wasnt just changed, button 1 has been pressed -> change state
joostbonekamp 5:aa8b5d5e632f 75 current_state = ccw;
joostbonekamp 5:aa8b5d5e632f 76 state_changed = true;
PatrickZieverink 8:6f6a4dc12036 77 but1_pressed = false; //reset this
joostbonekamp 5:aa8b5d5e632f 78 }
PatrickZieverink 8:6f6a4dc12036 79
joostbonekamp 5:aa8b5d5e632f 80 }
PatrickZieverink 7:78bc59c7753c 81
joostbonekamp 5:aa8b5d5e632f 82 void rotate_ccw() {
joostbonekamp 6:354a6509405f 83 //similar to rotate_cw()
joostbonekamp 5:aa8b5d5e632f 84 if (state_changed) {
joostbonekamp 5:aa8b5d5e632f 85 pc.printf("State changed to CCW\r\n");
joostbonekamp 5:aa8b5d5e632f 86 state_changed = false;
joostbonekamp 5:aa8b5d5e632f 87 }
PatrickZieverink 8:6f6a4dc12036 88 motor.dir1 = 0;
joostbonekamp 5:aa8b5d5e632f 89 motor.dir2 = 0;
PatrickZieverink 7:78bc59c7753c 90
PatrickZieverink 8:6f6a4dc12036 91 if (!state_changed && but1_pressed) { //state niet veranderd, button 1 gepressd -> state verandert
joostbonekamp 5:aa8b5d5e632f 92 current_state = cw;
joostbonekamp 5:aa8b5d5e632f 93 state_changed = true;
PatrickZieverink 8:6f6a4dc12036 94 but1_pressed = false;
joostbonekamp 5:aa8b5d5e632f 95 }
joostbonekamp 5:aa8b5d5e632f 96 }
joostbonekamp 5:aa8b5d5e632f 97
PatrickZieverink 8:6f6a4dc12036 98 void motor_controller() {
PatrickZieverink 8:6f6a4dc12036 99 motor1_pwm.period(0.0000625‬f); // 1/frequency van waarop hij draait
PatrickZieverink 8:6f6a4dc12036 100 motor1_direction = motor.dir1; // Zet de richting goed
PatrickZieverink 8:6f6a4dc12036 101 motor1_pwm.write(motor.speed1); // Zet het op de snelheid van motor.speed1
PatrickZieverink 8:6f6a4dc12036 102
PatrickZieverink 8:6f6a4dc12036 103 motor2_pwm.period(0.0000625f); // 1/frequency van waarop hij draait
PatrickZieverink 8:6f6a4dc12036 104 motor2_direction = motor.dir2;
PatrickZieverink 8:6f6a4dc12036 105 motor2_pwm.write(motor.speed2);
PatrickZieverink 8:6f6a4dc12036 106 return;
PatrickZieverink 8:6f6a4dc12036 107 }
PatrickZieverink 8:6f6a4dc12036 108
joostbonekamp 5:aa8b5d5e632f 109 void output() {return;}
joostbonekamp 5:aa8b5d5e632f 110
PatrickZieverink 8:6f6a4dc12036 111 void but1_interrupt () {
PatrickZieverink 8:6f6a4dc12036 112 but1_pressed = true;
PatrickZieverink 8:6f6a4dc12036 113 pc.printf("Button 1 pressed \n\r");
PatrickZieverink 8:6f6a4dc12036 114 }
PatrickZieverink 8:6f6a4dc12036 115
PatrickZieverink 8:6f6a4dc12036 116 void but2_interrupt () {
PatrickZieverink 8:6f6a4dc12036 117 but2_pressed = true;
PatrickZieverink 8:6f6a4dc12036 118 pc.printf("Button 2 pressed \n\r");
joostbonekamp 5:aa8b5d5e632f 119 }
joostbonekamp 5:aa8b5d5e632f 120
joostbonekamp 5:aa8b5d5e632f 121 void state_machine() {
joostbonekamp 5:aa8b5d5e632f 122
PatrickZieverink 8:6f6a4dc12036 123 if (but2_pressed){ // Is dit de goede locatie hiervoor?
PatrickZieverink 8:6f6a4dc12036 124 current_state = idle;
PatrickZieverink 8:6f6a4dc12036 125 but2_pressed = false;
PatrickZieverink 8:6f6a4dc12036 126 pc.printf("Do_nothing happened due to pressing button 2 \n\r");
PatrickZieverink 8:6f6a4dc12036 127 }
PatrickZieverink 8:6f6a4dc12036 128
joostbonekamp 5:aa8b5d5e632f 129 //run current state
joostbonekamp 5:aa8b5d5e632f 130 switch (current_state) {
PatrickZieverink 7:78bc59c7753c 131 case idle: // hoezo de :?
joostbonekamp 5:aa8b5d5e632f 132 do_nothing();
joostbonekamp 5:aa8b5d5e632f 133 break;
joostbonekamp 5:aa8b5d5e632f 134 case cw:
joostbonekamp 5:aa8b5d5e632f 135 rotate_cw();
joostbonekamp 5:aa8b5d5e632f 136 break;
joostbonekamp 5:aa8b5d5e632f 137 case ccw:
joostbonekamp 5:aa8b5d5e632f 138 rotate_ccw();
joostbonekamp 5:aa8b5d5e632f 139 break;
joostbonekamp 5:aa8b5d5e632f 140 case end:
joostbonekamp 5:aa8b5d5e632f 141 break;
joostbonekamp 5:aa8b5d5e632f 142 case failure:
joostbonekamp 5:aa8b5d5e632f 143 break;
joostbonekamp 5:aa8b5d5e632f 144 }
joostbonekamp 5:aa8b5d5e632f 145 }
joostbonekamp 5:aa8b5d5e632f 146
PatrickZieverink 8:6f6a4dc12036 147
joostbonekamp 5:aa8b5d5e632f 148 void main_loop() {
joostbonekamp 5:aa8b5d5e632f 149 measure_signals();
joostbonekamp 5:aa8b5d5e632f 150 state_machine();
joostbonekamp 5:aa8b5d5e632f 151 motor_controller();
joostbonekamp 5:aa8b5d5e632f 152 output();
joostbonekamp 5:aa8b5d5e632f 153 }
joostbonekamp 3:e3d12393adb1 154
joostbonekamp 2:bbaa6fca2ab1 155 int main() {
joostbonekamp 3:e3d12393adb1 156 pc.baud(115200);
PatrickZieverink 8:6f6a4dc12036 157 pc.printf("Executing main()... \r\n");
joostbonekamp 5:aa8b5d5e632f 158 current_state = idle;
joostbonekamp 3:e3d12393adb1 159
joostbonekamp 5:aa8b5d5e632f 160 motor.dir1 = 0;
joostbonekamp 5:aa8b5d5e632f 161 motor.dir2 = 0;
PatrickZieverink 7:78bc59c7753c 162
PatrickZieverink 8:6f6a4dc12036 163 but1.fall(&but1_interrupt);
PatrickZieverink 8:6f6a4dc12036 164 but2.fall(&but2_interrupt);
PatrickZieverink 7:78bc59c7753c 165 loop_ticker.attach(&main_loop, 0.1f); //main loop at 1kHz
PatrickZieverink 8:6f6a4dc12036 166 pc.printf("Main_loop is running\n \r");
joostbonekamp 5:aa8b5d5e632f 167 while (true) {}
joostbonekamp 6:354a6509405f 168 }