niet werkend
Dependencies: mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
main.cpp
- Committer:
- PatrickZieverink
- Date:
- 2019-10-07
- Revision:
- 10:59ca23f66884
- Parent:
- 9:6537eead1241
File content as of revision 10:59ca23f66884:
#include "mbed.h" #include "MODSERIAL.h" #include "FastPWM.h" #include "QEI.h" Serial pc(USBTX, USBRX); //verbinden met pc DigitalOut motor2_direction(D4); //verbinden met motor 2 op board (altijd d4) FastPWM motor2_pwm(D5); //verbinden met motor 2 pwm (altijd d5) DigitalOut motor1_direction(D6); //verbinden met motor 2 op board (altijd d4) FastPWM motor1_pwm(D7); //verbinden met motor 2 pwm (altijd d5) Ticker loop_ticker; //used in main() AnalogIn Pot1(A1); //pot 1 op biorobotics shield AnalogIn Pot2(A0); //pot 2 op biorobotics shield InterruptIn but1(D10); //knop 1 op birorobotics shield InterruptIn but2(D9); //knop 1 op birorobotics shield QEI encoder1 (D11, D12, NC, 8400, QEI::X4_ENCODING); //encoder 1 gebruiken QEI encoder2 (D1, D2, NC, 8400, QEI::X4_ENCODING); //encoder 2 gebruiken //variables enum States {idle, cw, ccw, end, failure}; States current_state; //Definieren van de current_state met als keuzes Idle, cw, ccw, end en failure class motor_state { // Een emmer met variabelen die je hierna motor gaat noemen. public: //Wat is public? float dutycycle1; //pwm of 1st motor float dutycycle2; //pwm of 2nd motor int dir1; //direction of 1st motor int dir2; //direction of 2nd motor }; motor_state motor; //Een emmer met variabelen met motor.iets als naam bijv. motor.dutycycle1 bool state_changed = false; volatile bool but1_pressed = false; volatile bool but2_pressed = false; float pot_1 = 0; float pot_2 = 0; /* float x_movement = 0; float y_movement = 0; float angle = 0; float extention = 0; void calibration() { //kalibratie van de sensoren (de nulstand invoeren) letter = pc.getc() // krijg de letter vanuit de keyboard 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. y_movement = 1; } while (letter == a) { x_movement = -1; } while (letter == s){ y_movement = -1; } while (letter == d) { x_movement = 1; } robot_kinematics() // hoe dat dit precies moet geen idee. Dit lijkt me een goed begin of iig een begin x_movement = 0; y_movement = 0; } void robot_kinematics() { //hoe de motoren moeten draaien als er een x of y richting als input is. [motor.dir1, motor.dir2; motor.dutycycle1, motor.dutycycle2] = H^-1 *[x_movement, y_movement; angle] // Zoiets } */ // the funtions needed to run the program void measure_signals() { /* letter = pc.getc() // krijg de letter vanuit de keyboard if (letter == w){ y_movement = 1; } if (letter == a) { x_movement = -1; } if (letter == s){ y_movement = -1; } if (letter == d) { x_movement = 1; } 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 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) */ pot_1 = Pot1.read(); pot_1 = 1; /// pc.printf("pot_1 = %f",pot_1); // Dit doet hij niet, waarom niet? Er scheelt niet veel aan, maar toch motor.dutycycle1 = pot_1*0.75; //pod_read * 0.75 (dus max 75%) pot_2 = Pot2.read(); /// pc.printf("pot_2 = %f",pot_2); // Dit doet hij niet, waarom niet? Er scheelt niet veel aan, maar toch motor.dutycycle2 = pot_1*0.75; //pod_read * 0.75 (dus max 75%) return; } void do_nothing() { motor.dutycycle1 = 0; motor.dutycycle2 = 0; if (but1_pressed == true) { //this moves the program from the idle to cw state current_state = cw; state_changed = true; //to show next state it can initialize pc.printf("Changed state from idle to cw\r\n"); but1_pressed = false; //reset button 1 } } void rotate_cw() { if (state_changed) { pc.printf("State changed to CW\r\n"); state_changed = false; //reset this so it wont print next loop } motor.dir1 = 1; //todo: check if this is actually clockwise motor.dir2 = 1; //todo: check if this is actually clockwise if (!state_changed && but1_pressed) { //state wasnt just changed, button 1 has been pressed -> change state current_state = ccw; state_changed = true; but1_pressed = false; //reset this } } void rotate_ccw() { //similar to rotate_cw() if (state_changed) { pc.printf("State changed to CCW\r\n"); state_changed = false; } motor.dir1 = 1; motor.dir2 = 1; if (!state_changed && but1_pressed) { //state niet veranderd, button 1 gepressd -> state verandert current_state = cw; state_changed = true; but1_pressed = false; } } void motor_controller() { motor1_pwm.period(0.0000625f); // 1/frequency van waarop hij draait motor1_direction = motor.dir1; // Zet de richting goed motor1_pwm.write(motor.dutycycle1); // Zet het op de snelheid van motor.dutycycle1 motor2_pwm.period(0.0000625f); // 1/frequency van waarop hij draait motor2_direction = motor.dir2; motor2_pwm.write(motor.dutycycle2); return; } void output() {return;} void but1_interrupt () { but1_pressed = true; pc.printf("Button 1 pressed \n\r"); } void but2_interrupt () { but2_pressed = true; pc.printf("Button 2 pressed \n\r"); } void state_machine() { if (but2_pressed){ // Is dit de goede locatie hiervoor? current_state = idle; but2_pressed = false; pc.printf("Do_nothing happened due to pressing button 2 \n\r"); } //run current state switch (current_state) { case idle: // hoezo de :? do_nothing(); break; case cw: rotate_cw(); break; case ccw: rotate_ccw(); break; case end: break; case failure: break; } } void main_loop() { measure_signals(); state_machine(); motor_controller(); output(); } int main() { pc.baud(115200); pc.printf("Executing main()... \r\n"); current_state = idle; motor.dir1 = 0; motor.dir2 = 0; but1.fall(&but1_interrupt); but2.fall(&but2_interrupt); loop_ticker.attach(&main_loop, 0.1f); //main loop at 1kHz (niet per tiende seconde?) pc.printf("Main_loop is running\n \r"); while (true) {} }