Aansturing motor met POT, niet werkend
Dependencies: Encoder MODSERIAL mbed
main.cpp
- Committer:
- Tess
- Date:
- 2013-11-04
- Revision:
- 0:6ca6892a17d0
File content as of revision 0:6ca6892a17d0:
#include "mbed.h" #include "encoder.h" #include "MODSERIAL.h" /******************************************************************************* * * * Code can be found at http://mbed.org/users/vsluiter/code/BMT-K9-Regelaar/ * * * ********************************************************************************/ /** keep_in_range -> float in, and keep_in_range if less than min, or larger than max **/ void keep_in_range(float * in, float min, float max); /** variable to show when a new loop can be started */ /** volatile means that it can be changed in an interrupt routine, and that that change is visible in the main loop. */ volatile bool looptimerflag; /** function called by Ticker "looptimer" */ /** variable 'looptimerflag' is set to 'true' each time the looptimer expires.*/ void setlooptimerflag(void) { looptimerflag = true; } int main() { //LOCAL VARIABLES AnalogIn potmeterA(PTC2); AnalogIn potmeterB(PTB2); Encoder motorA(PTD4,PTC8); Encoder motorB(PTD0,PTD2); MODSERIAL pc(USBTX,USBRX); // MODSERIAL to get non-blocking Serial PwmOut pwm_motorA(PTA12); // PWM control to motor PwmOut pwm_motorB(PTA5); // PWM control to motor DigitalOut motordirA(PTD3); // Direction pin DigitalOut motordirB(PTD1); // Direction pin /* variable to store setpoint in */ float setpointA; float setpointB; float setpoint_beginA; float setpoint_beginB; float setpoint_rechtsonderA; float setpoint_rechtsonderB; /* variable to store pwm value in*/ float pwm_to_motorA; float pwm_to_begin_motorA = 0; float pwm_to_begin_motorB = 0; float pwm_to_motorB; float pwm_to_rechtsonder_motorA; float pwm_to_rechtsonder_motorB; /* variable for PD controller*/ const float dt = 0.002; float Kp = 0.001; //0.0208 float Kd = 0.00004342; //0.0006897 float error_t0_A = 0; float error_t0_B = 0; float error_ti_A; float error_ti_B; float P_regelaar_A; float P_regelaar_B; float D_regelaar_A; float D_regelaar_B; float output_regelaar_A; float output_regelaar_B; /* variable to store positions in*/ int32_t positionmotorA_t0; int32_t positionmotorB_t0; int32_t positionmotorA_t_1; int32_t positionmotorB_t_1; int32_t positiondifference_motorA; int32_t positiondifference_motorB; /* inverse kinematica */ float dy; //dy waarde tussen -1 en 1 -1 -vmax; 1 vmax float dx; //dx waarde tussen -1 en 1 -1 -vmax; 1 vmax const float vmax = 0.08; // m/s const float delta_t = 0.005; // 1/samplefrequentie, dus tijd tussen twee meetpunten float X_positie; float Y_positie; float X_positie_begin; float Y_positie_begin; float puls_motorA; float puls_motorB; float kwadraat_X_positie; float kwadraat_Y_positie; float phi_A_pulsen_positie_begin; float phi_B_pulsen_positie_begin; float phi_A_positie_begin; float phi_B_positie_begin; float phi_1; float lengte_arm = 276; // in mm anders rare imaginaire getallen float phi_A; float phi_B; float Puls_motorA; float Puls_motorB; float phi_A_pulsen; float phi_B_pulsen; float pi = 3.14159265; //START OF CODE pc.baud(115200); // Set the baudrate (use this number in RealTerm too!) // In dit stukje code zorgen we ervoor dat de arm gaat draaien naar rechts en stopt als het tegen het frame komt. Eerst motor B botsen dan motor A botsen. // motor B zit onder en motor A zit boven en dus op zijn kop (en dus setpoint moet - zijn). motordirB.write(0); pwm_motorB.write(.1); //0.08 positionmotorB_t0 = motorB.getPosition(); do { wait(0.2); positionmotorB_t_1 = positionmotorB_t0 ; positionmotorB_t0 = motorB.getPosition(); positiondifference_motorB = abs(positionmotorB_t0 - positionmotorB_t_1); } while(positiondifference_motorB > 10); motorB.setPosition(0); pwm_motorB.write(0); wait(1); // willen nu even dat tussen ene actie en andere actie 1 seconde wacht. motordirA.write(1); pwm_motorA.write(.1); positionmotorA_t0 = motorA.getPosition(); do { wait(0.2); positionmotorA_t_1 = positionmotorA_t0 ; positionmotorA_t0 = motorA.getPosition(); positiondifference_motorA = abs(positionmotorA_t0 - positionmotorA_t_1); } while(positiondifference_motorA > 10); motorA.setPosition(0); pwm_motorA.write(0); wait(1); // willen nu even dat tussen ene actie en andere actie 1 seconde wacht. // Hierna willen we de motor van zijn alleruiterste positie naar de x-as hebben. Hiervoor moet motor A eerst op de x-as worden gezet. Hiervoor moet motor A 4.11 graden (63) naar links. motordirA.write(0); pwm_motorA.write(.08); do { wait(0.01); setpoint_beginA = -63; // x-as pwm_to_begin_motorA = (setpoint_beginA - motorA.getPosition()) *.001; // + omdat men met een negatieve hoekverdraaiing werkt. keep_in_range(&pwm_to_begin_motorA, -0.3, 0.3 ); motordirA.write(0); pwm_motorA.write(abs(pwm_to_begin_motorA)); pc.printf("s: %d, %f \n\r", motorA.getPosition(), pwm_to_begin_motorA); } while(pwm_to_begin_motorA >= 0); motorA.setPosition(0); pwm_motorA.write(0); wait(1); // willen nu even dat tussen ene actie en andere actie 1 seconde wacht. // hierna moet motor A naar de rechtsonder A4. Motor A 532 (hoek 59.8 graden). motordirA.write(0); pwm_motorA.write(0.08); do { wait(0.01); setpoint_rechtsonderA = -532; // rechtsonder positie A4 pwm_to_rechtsonder_motorA = (setpoint_rechtsonderA - motorA.getPosition()) *.001; keep_in_range(&pwm_to_rechtsonder_motorA, -0.3, 0.3 ); motordirA.write(0); pwm_motorA.write(abs(pwm_to_rechtsonder_motorA)); } while(pwm_to_rechtsonder_motorA >= 0); pwm_motorA.write(0); wait(1); // Hierna moet motor B 21.6 (192) graden naar links om naar x-as te gaan. motordirB.write(1); pwm_motorB.write(.08); do { wait(0.01); setpoint_beginB = 192; // x-as pwm_to_begin_motorB = (setpoint_beginB - motorB.getPosition()) *.001; keep_in_range(&pwm_to_begin_motorB, -0.3, 0.3 ); motordirB.write(1); pwm_motorB.write(abs(pwm_to_begin_motorB)); } while(pwm_to_begin_motorB >= 0); motorB.setPosition(0); pwm_motorB.write(0); wait(1); // willen nu even dat tussen ene actie en andere actie 1 seconde wacht. // Hierna moet motor B van x-as naar de rechtsonder A4 positie. Motor B 268 (30.2 graden). motordirB.write(1); pwm_motorB.write(0.08); do { wait(0.01); setpoint_rechtsonderB = 268; // rechtsonder positie A4 pwm_to_rechtsonder_motorB = (setpoint_rechtsonderB - motorB.getPosition()) *.001; keep_in_range(&pwm_to_rechtsonder_motorB, -0.3, 0.3 ); motordirB.write(1); pwm_motorB.write(abs(pwm_to_rechtsonder_motorB)); } while(pwm_to_rechtsonder_motorB >= 0); pwm_motorB.write(0); wait(1); // Nu zijn de motoren gekalibreed en staan ze op de startpositie (rechtsonderhoek van A4). // Hierna het script dat EMG wordt omgezet in een positie verandering /*Create a ticker, and let it call the function 'setlooptimerflag' every 0.01s */ Ticker looptimer; looptimer.attach(setlooptimerflag,0.01); //INFINITE LOOP while(1) { while(looptimerflag != true); looptimerflag = false; dx = ((potmeterA.read()-0.5)*2000); dy = ((potmeterB.read()-0.5)*2000); //waarde uit tussen -1 en 1 // inverse kinematica phi_A_pulsen_positie_begin = motorA.getPosition(); phi_B_pulsen_positie_begin = motorB.getPosition(); //pc.printf("s: %f, %d \n\r", phi_A_pulsen_positie_begin, phi_B_pulsen_positie_begin); phi_A_positie_begin = (360/3200.0) * phi_A_pulsen_positie_begin * (pi/180); phi_B_positie_begin = (360/3200.0) * phi_B_pulsen_positie_begin * (pi/180); phi_1 = phi_A_positie_begin - phi_B_positie_begin; X_positie_begin = 2 * lengte_arm * sin(0.5 * phi_1) * cos(90 - 0.5 * phi_A_positie_begin - 0.5 * phi_B_positie_begin); Y_positie_begin = 2 * lengte_arm * sin(0.5 * phi_1) * sin(90 - 0.5 * phi_A_positie_begin - 0.5 * phi_B_positie_begin); X_positie = dx * vmax * delta_t + X_positie_begin; // delta_t eruit want dx tussen -1 en 1 en dimensieloos Y_positie = dy * vmax * delta_t + Y_positie_begin; kwadraat_X_positie = pow(X_positie,2); kwadraat_Y_positie = pow(Y_positie,2); phi_A = pi - acos(sqrt(kwadraat_X_positie+kwadraat_Y_positie)/(2*lengte_arm)) - atan(Y_positie/X_positie); //rad phi_B = pi - phi_A - acos(-(kwadraat_X_positie + kwadraat_Y_positie) / (2 * pow(lengte_arm,2))+1); phi_A_pulsen = (3200/(2*pi)) * phi_A; phi_B_pulsen = (3200/(2*pi)) * phi_B; // motor A moet de hoek altijd binnen 53.4 tot en met 124.3 graden liggen // motor B moet de hoek altijd binnen 30.2 tot en met -16.5 graden liggen keep_in_range(&phi_A_pulsen, -1104, -474); // voor motor moet bereik zijn -1104 tot -474 keep_in_range(&phi_B_pulsen, -146, 268); // voor motor moet bereik zijn -146 tot 268 Puls_motorA = phi_A_pulsen - phi_A_pulsen_positie_begin; Puls_motorB = phi_B_pulsen - phi_B_pulsen_positie_begin; // doe begin berekening pwm_to_motorA = Puls_motorA*.001; pwm_to_motorB = Puls_motorB*.001; keep_in_range(&pwm_to_motorA, -1,1); keep_in_range(&pwm_to_motorB, -1,1); if(pwm_to_motorA > 0) motordirA.write(1); else motordirA.write(0); if(pwm_to_motorB > 0) motordirB.write(1); else motordirB.write(0); pwm_motorA.write(abs(pwm_to_motorA)); pwm_motorB.write(abs(pwm_to_motorB)); /*/PD regelaar voor motor A wait(dt); error_ti_A = phi_A_pulsen - motorA.getPosition(); // puls_motorA - motorA.getPosition(); P_regelaar_A = Kp * error_ti_A; D_regelaar_A = Kd * ((error_ti_A - error_t0_A) / dt); error_t0_A = error_ti_A; output_regelaar_A = P_regelaar_A + D_regelaar_A; //PD regelaar voor motor B wait(dt); error_ti_B = phi_B_pulsen - motorB.getPosition(); //puls_motorB - motorB.getPosition(); P_regelaar_B = Kp * error_ti_B; D_regelaar_B = Kd * ((error_ti_B - error_t0_B) / dt); error_t0_B = error_ti_B; output_regelaar_B = P_regelaar_B + D_regelaar_B; `*/ /* This is a PD-action! store in pwm_to_motor */ /*pwm_to_motorA = output_regelaar_A; pwm_to_motorB = output_regelaar_B; keep_in_range(&pwm_to_motorA, -1,1); keep_in_range(&pwm_to_motorB, -1,1); if(pwm_to_motorA > 0) motordirA.write(1); else motordirA.write(0); if(pwm_to_motorB > 0) motordirB.write(1); else motordirB.write(0); pwm_motorA.write(abs(pwm_to_motorA)); pwm_motorB.write(abs(pwm_to_motorB));*/ } } void keep_in_range(float * in, float min, float max) { *in > min ? *in < max? : *in = max: *in = min; }