Moet dit er bij
Dependencies: mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 6:e7e39d116ed0
- Parent:
- 5:4d8b85b7cfc4
- Child:
- 7:676a83def149
diff -r 4d8b85b7cfc4 -r e7e39d116ed0 main.cpp --- a/main.cpp Tue Oct 29 15:57:06 2019 +0000 +++ b/main.cpp Tue Oct 29 16:11:21 2019 +0000 @@ -38,8 +38,6 @@ DigitalOut motor1Direction(D7); FastPWM motor1Power(D6); -volatile int motortoggle = 1; //Toggle to stop motors - //Motorcontrol bool motordir1; bool motordir2; @@ -85,7 +83,7 @@ HIDScope scope(6); //Going to send x channels of data. To access data go to 'http:/localhost:18082/' after starting HIDScope application. //State maschine -enum Motor_States{motor_wait , motor_cal1_start , motor_cal1 , motor_cal2_start , motor_cal2 , motor_encoderset}; +enum Motor_States{motor_wait , motor_cal1_start , motor_cal1 , motor_cal2_start , motor_cal2 , motor_cal_manual , motor_encoderset}; Motor_States motor_curr_state; bool motor_state_changed = true; bool motor_cal1_done = false; @@ -122,6 +120,10 @@ button1_pressed = true; } +void button2Press() +{ + button2_pressed = true; +} // Ticker Functions void readEncoder() @@ -146,7 +148,7 @@ motor1angle = (counts1 * factorin / gearratio); // Angle of motor shaft in rad omega1 = deltaCounts1 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s controlsignal1=0.10f; - motor1Power.write(abs(controlsignal1*motortoggle)); + motor1Power.write(abs(controlsignal1)); motor1Direction=0; // State transition guard @@ -169,7 +171,7 @@ motor1angle = (counts1 * factorin / gearratio); // Angle of motor shaft in rad omega1 = deltaCounts1 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s controlsignal1=0.0980f; - motor1Power.write(abs(controlsignal1*motortoggle)); + motor1Power.write(abs(controlsignal1)); motor1Direction=0; // State transition guard @@ -190,13 +192,13 @@ motor1angle = (counts1 * factorin / gearratio); // Angle of motor shaft in rad omega1 = deltaCounts1 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s controlsignal1=0.10f; - motor1Power.write(abs(controlsignal1*motortoggle)); + motor1Power.write(abs(controlsignal1)); motor1Direction=0; motor2angle = (counts2 * factorin / gearratio); // Angle of motor shaft in rad omega2 = deltaCounts2 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s controlsignal2=0.10f; - motor2Power.write(abs(controlsignal2*motortoggle)); + motor2Power.write(abs(controlsignal2)); motor2Direction=0; // State transition guard @@ -218,13 +220,13 @@ motor1angle = (counts1 * factorin / gearratio); // Angle of motor shaft in rad omega1 = deltaCounts1 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s controlsignal1=0.10f; - motor1Power.write(abs(controlsignal1*motortoggle)); + motor1Power.write(abs(controlsignal1)); motor1Direction=0; motor2angle = (counts2 * factorin / gearratio); // Angle of motor shaft in rad omega2 = deltaCounts2 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s controlsignal2=0.10f; - motor2Power.write(abs(controlsignal2*motortoggle)); + motor2Power.write(abs(controlsignal2)); motor2Direction=0; // State transition guard @@ -263,19 +265,40 @@ // Do nothing until end condition is met // State transition guard - if ( button1_pressed ) { +if ( button1_pressed ) { button1_pressed = false; motor_curr_state = motor_cal1_start; //Beginnen met calibratie motor_state_changed = true; // More functions - } + } +else if ( button2_pressed ) { + button2_pressed = false; + motor_curr_state = motor_cal_manual; //Beginnen met calibratie + motor_state_changed = true; + // More functions + } + } -void toggleMotor() -{ - motortoggle = !motortoggle; +void do_motor_cal_manual(){ + // Entry function + if ( motor_state_changed == true ) { + motor_state_changed = false; + // More functions + } + + //Op dit moment moet je de robotarm handmatig naar zijn beginpositie brengen, + //en op het knopje klikken om door te gaan + +if ( button2_pressed ) { + button2_pressed = false; + motor_curr_state = motor_encoderset; + motor_state_changed = true; + // More functions + } } + void motor_state_machine() { switch(motor_curr_state) { @@ -297,6 +320,9 @@ case motor_encoderset: do_motor_Encoder_Set(); break; + case motor_cal_manual: + do_motor_cal_manual(); + break; } } @@ -323,6 +349,7 @@ tickGlobal.attach( &tickGlobalFunc, Ts ); button1.fall(&button1Press); + button2.fall(&button2Press); while (true) { pc.printf("Omega1: %f Omega 2: %f controlsignal1: %f \r\n", omega1, omega2, controlsignal1);