![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Robot tryout
Dependencies: mbed QEI biquadFilter MODSERIAL FastPWM ttmath Math
Diff: Motor_tryout.cpp
- Revision:
- 19:9c8ab7922191
- Parent:
- 18:ab0fe311e7f3
- Child:
- 21:c826abca79c3
--- a/Motor_tryout.cpp Mon Oct 21 11:01:18 2019 +0000 +++ b/Motor_tryout.cpp Fri Oct 25 07:18:00 2019 +0000 @@ -19,16 +19,18 @@ QEI2 Encoder2(D10,D11,NC,64,QEI2::X4_ENCODING); QEI Encoder(D2,D3,NC,64,QEI::X4_ENCODING); +int check; int quit; +int limit_pos = 8400; +float steps; +int g = 0; + +bool check1; +bool check2; +bool check3; int counts1 = 0; int counts2 = 0; int counts3 = 0; -int limit_pos = 8400; -int x; -int y; -int z; -float steps; -int g = 0; const float le = 15.0; const float f = 37.5; @@ -43,17 +45,18 @@ float rje; float r2; float r; + float z0=-172; float y0=0; float x0=0; -int check; + float theta1; float theta2; float theta3; float oldtheta1=0; float oldtheta2=0; float oldtheta3=0; -int direction; + float delta_calcangleyz(float x0, float y0, float z0) { float y2 = y0 + le; float y1 = f; @@ -64,51 +67,41 @@ float r2 = (y1-y2)*(y1-y2) + (z1-z0)*(z1-z0); float r = sqrt(r2); - - if ((r+rje<rf) || (r + rf <rje) || (rf+rje<r)) - { - int check = 1; - pc.printf("\n\rPunt bestaat niet"); - } - else - { - float alpha = acos((r2 + rf*rf -rje2)/(2*rf*r)); - float beta = atan((z1-z2)/(y1-y2)); - - if(beta<0) { + if ((r+rje<rf) || (r + rf <rje) || (rf+rje<r)) { + int check = 1; + pc.printf("\n\rPunt bestaat niet"); + } + else { + float alpha = acos((r2 + rf*rf -rje2)/(2*rf*r)); + float beta = atan((z1-z2)/(y1-y2)); + if(beta<0) { beta = beta + pi; } - float theta1 = (beta - alpha)*180.0/pi; - int check = 0; - return theta1; + float theta1 = (beta - alpha)*180.0/pi; + return theta1; } } float delta_calcinverse(float x0, float y0, float z0) { - theta1 = theta2 = theta3 = 0; - theta1 = delta_calcangleyz(x0, y0, z0); - if (check == 0) - { - theta2 = delta_calcangleyz(x0*cos(2/3*pi) + y0*sin(2/3*pi), y0*cos(2/3*pi)-x0*sin(2/3*pi), z0); - theta3 = delta_calcangleyz(x0*cos(2/3*pi) - y0*sin(2/3*pi), y0*cos(2/3*pi)+x0*sin(2/3*pi), z0); - //return theta2; - //return theta3; - } +theta1 = theta2 = theta3 = 0; +theta1 = delta_calcangleyz(x0, y0, z0); + + if (check == 0) { + theta2 = delta_calcangleyz(x0*cos(2/3*pi) + y0*sin(2/3*pi), y0*cos(2/3*pi)-x0*sin(2/3*pi), z0); + theta3 = delta_calcangleyz(x0*cos(2/3*pi) - y0*sin(2/3*pi), y0*cos(2/3*pi)+x0*sin(2/3*pi), z0); + } + return theta1, theta2, theta3; - } - Ticker pulse; -void pulseget() -{ +void pulseget() { counts1 = Encoder1.getPulses(); counts2 = Encoder2.getPulses(); counts3 = Encoder.getPulses(); } -int anglestep(float angle) -{ +float anglestep(float angle) { float steps; steps = angle / 360 * 8400; return steps; @@ -120,99 +113,51 @@ pc.baud(115200); char cc = pc.getc(); - pc.printf("\n\r cc check"); - //char key = term.getc(); - pc.printf("\n\r term check"); + + while(true) { + counts1 = 0; + counts2 = 0; + counts3 = 0; - float angle = 360; - while(true) - { - counts1 = 0; - counts2 = 0; - counts3 = 0; - - float steps1; - float steps2; - float steps3; - - counts1 = Encoder1.getPulses(); - counts2 = Encoder2.getPulses(); - counts3 = Encoder.getPulses(); - - delta_calcinverse(x0,y0,z0); - - oldtheta1 = theta1; - oldtheta2 = theta2; - oldtheta3 = theta3; - + delta_calcinverse(x0,y0,z0); + + oldtheta1 = theta1; + oldtheta2 = theta2; + oldtheta3 = theta3; + char cc = pc.getc(); if (cc=='a') { - pc.printf("\n\rleft"); - z0=z0-1.0f; - + check1=true; + check2=true; + check3=true; + + z0=z0+2.0f; + theta1 = delta_calcinverse(x0,y0,z0); - theta1 = delta_calcangleyz(x0,y0,z0); theta1 = theta1 - oldtheta1; theta2 = theta2 - oldtheta2; - theta3 = theta3 - oldtheta3; - - steps1 = anglestep(theta1); - steps2 = anglestep(theta2); - steps3 = anglestep(theta3); - - int frequency_pwm = 10000; //10 kHz PWM - motor1_pwm.period(1.0/(double)frequency_pwm); // T=1/f - motor1_dir.write(1); // positief - motor1_pwm.write(0.7); // write Duty Cycle - - motor2_pwm.period(1.0/(double)frequency_pwm); // T=1/f - motor2_dir.write(0); // positief - motor2_pwm.write(0.7); // write Duty Cycle - - motor3_pwm.period(1.0/(double)frequency_pwm); // T=1/f - motor3_dir.write(1); // positief - motor3_pwm.write(0.7); // write Duty Cycle - - -} -if (cc=='d') { - pc.printf("\n\rright"); + theta3 = theta3 - oldtheta3; - z0=z0-10.0f; - - theta1 = delta_calcinverse(x0,y0,z0); - theta1 = delta_calcangleyz(x0,y0,z0); - - theta1 = theta1 - oldtheta1; - theta2 = theta2 - oldtheta2; - theta3 = theta3 - oldtheta3; - float steps1 = anglestep(theta1); float steps2 = anglestep(theta2); float steps3 = anglestep(theta3); - pc.printf("\n\r the difference in angles are (%f, %f, %f)", theta1, theta2, theta3); - pc.printf("\n\r the steps are (%f, %f, %f)", steps1, steps2, steps3); + motor1_dir.write(1); // positief + motor2_dir.write(0); // positief + motor3_dir.write(1); // positief + if (steps1>0) { motor1_dir.write(0); // positief - motor2_dir.write(1); // positief - motor3_dir.write(0); // positief - - if (steps1<0) { - motor1_dir.write(1); // positief } - - if (steps1<0) { - motor2_dir.write(0); // positief - - } - - if (steps1<0) { - motor3_dir.write(1); // positief - } + if (steps1>0) { + motor2_dir.write(1); // positief + } + if (steps1>0) { + motor3_dir.write(0); // positief + } int frequency_pwm = 10000; //10 kHz PWM motor1_pwm.period(1.0/(double)frequency_pwm); // T=1/f @@ -223,17 +168,107 @@ motor3_pwm.period(1.0/(double)frequency_pwm); // T=1/f motor3_pwm.write(0.7); // write Duty Cycle + + wait(1.0/100); + - while (abs(counts1)<=abs(steps1) || abs(counts2)<=abs(steps2) || abs(counts3)<=abs(steps3)) { - pc.printf("while"); + while (check1 || check2 || check3) { + counts1 = Encoder1.getPulses(); + counts2 = Encoder2.getPulses(); + counts3 = Encoder.getPulses(); + + wait(1.0/1000); + //pc.printf("while"); if(abs(counts1)>=abs(steps1)) { motor1_pwm.write(0); + check1=false; + counts1=0; } if (abs(counts2)>=abs(steps2)) { motor2_pwm.write(0); + check2=false; + counts2=0; } if (abs(counts3)>=abs(steps3)) { motor3_pwm.write(0); + check3=false; + counts3=0; + } + } +} + +if (cc=='d') { + + check1=true; + check2=true; + check3=true; + + x0=x0-2.0f; + + theta1 = delta_calcinverse(x0,y0,z0); + pc.printf("\n\r de hoeken zijn(%f, %f, %f)", theta1, theta2, theta3); + pc.printf("\n\r coordinaten(%f, %f, %f)", x0, y0, z0); + + theta1 = theta1 - oldtheta1; + theta2 = theta2 - oldtheta2; + theta3 = theta3 - oldtheta3; + + float steps1 = anglestep(theta1); + float steps2 = anglestep(theta2); + float steps3 = anglestep(theta3); + + //pc.printf("\n\r (%f, %f, %f)", steps1, steps2, steps3); + + + motor1_dir.write(0); // positief + motor2_dir.write(1); // positief + motor3_dir.write(0); // positief + + if (steps1<0) { + motor1_dir.write(1); // positief + } + if (steps1<0) { + motor2_dir.write(0); // positief + } + if (steps1<0) { + motor3_dir.write(1); // positief + } + + int frequency_pwm = 10000; //10 kHz PWM + motor1_pwm.period(1.0/(double)frequency_pwm); // T=1/f + motor1_pwm.write(0.7); // write Duty Cycle + + motor2_pwm.period(1.0/(double)frequency_pwm); // T=1/f + motor2_pwm.write(0.7); // write Duty Cycle + + motor3_pwm.period(1.0/(double)frequency_pwm); // T=1/f + motor3_pwm.write(0.7); // write Duty Cycle + + while (check1 || check2 || check3) { + counts1 = Encoder1.getPulses(); + counts2 = Encoder2.getPulses(); + counts3 = Encoder.getPulses(); + + wait(1.0/500); + //pc.printf("while"); + if(abs(counts1)>=abs(steps1)) { + motor1_pwm.write(0); + check1=false; + counts1=0; + wait(1.0/100); + } + if (abs(counts2)>=abs(steps2)) { + motor2_pwm.write(0); + check2=false; + counts2=0; + wait(1.0/100); + } + if (abs(counts3)>=abs(steps3)) { + motor3_pwm.write(0); + check3=false; + //pc.printf("\n\rsteps %f, counts %f", steps3, counts3); + counts3=0; + wait(1.0/100); } }