![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Robot tryout
Dependencies: mbed QEI biquadFilter MODSERIAL FastPWM ttmath Math
Diff: Motor_tryout.cpp
- Revision:
- 18:ab0fe311e7f3
- Parent:
- 17:6da57acb7bea
- Child:
- 19:9c8ab7922191
diff -r 6da57acb7bea -r ab0fe311e7f3 Motor_tryout.cpp --- a/Motor_tryout.cpp Mon Oct 14 09:05:05 2019 +0000 +++ b/Motor_tryout.cpp Mon Oct 21 11:01:18 2019 +0000 @@ -1,70 +1,245 @@ #include "mbed.h" #include "MODSERIAL.h" +#include "QEI1.h" +#include "QEI2.h" #include "QEI.h" +#include "Math.h" +#include "ttmath.h" + MODSERIAL pc(USBTX, USBRX); +//Serial term (USBTX, USBRX); PwmOut motor1_pwm(PTC2); DigitalOut motor1_dir(PTC3); -PwmOut motor3_pwm(PTA1); -DigitalOut motor3_dir(PTB9); +PwmOut motor2_pwm(PTA2); +DigitalOut motor2_dir(PTB23); +PwmOut motor3_pwm(PTC4); +DigitalOut motor3_dir(PTC12); -QEI Encoder(D12,D13,NC,64,QEI::X4_ENCODING); +QEI1 Encoder1(D12,D13,NC,64,QEI1::X4_ENCODING); +QEI2 Encoder2(D10,D11,NC,64,QEI2::X4_ENCODING); +QEI Encoder(D2,D3,NC,64,QEI::X4_ENCODING); int quit; -int counts = 0; +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; +const float re = 174.0; +const float rf = 50.0; +const float pi = 3.14159265358979323846; +float y2; +float y1; +float z1; +float z2; +float rje2; +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; + float z1 = 0.0; + float z2 = z0; + float rje2 = re*re - x0*x0; + float rje = sqrt(rje2); + 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) { + beta = beta + pi; + } + float theta1 = (beta - alpha)*180.0/pi; + int check = 0; + 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; + } +return theta1, theta2, theta3; + +} + + Ticker pulse; void pulseget() { - counts = Encoder.getPulses(); - if(counts > limit_pos) - { - quit = 1; - } + counts1 = Encoder1.getPulses(); + counts2 = Encoder2.getPulses(); + counts3 = Encoder.getPulses(); } -Ticker show; -void pulseshow() +int anglestep(float angle) { - pc.printf("The counts is %i",counts); + float steps; + steps = angle / 360 * 8400; + return steps; } - // DE MAIN FUNCTIE int main(void) -{ - pc.baud(115200); +{ + pc.baud(115200); + char cc = pc.getc(); + pc.printf("\n\r cc check"); + //char key = term.getc(); + pc.printf("\n\r term check"); + + float angle = 360; while(true) { - quit = 0; - counts = 0; - { - pc.printf("in while"); + 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; + +char cc = pc.getc(); + + +if (cc=='a') { + pc.printf("\n\rleft"); + z0=z0-1.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(0); // positief - motor1_pwm.write(0.8); // write Duty Cycle + 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(0); // positief - motor3_pwm.write(0.8); // write Duty Cycle + motor3_dir.write(1); // positief + motor3_pwm.write(0.7); // write Duty Cycle + + +} +if (cc=='d') { + pc.printf("\n\rright"); + + z0=z0-10.0f; + + theta1 = delta_calcinverse(x0,y0,z0); + theta1 = delta_calcangleyz(x0,y0,z0); - counts = Encoder.getPulses(); - pc.printf("The counts is %i\n\r",counts); + theta1 = theta1 - oldtheta1; + theta2 = theta2 - oldtheta2; + theta3 = theta3 - oldtheta3; + + float steps1 = anglestep(theta1); + float steps2 = anglestep(theta2); + float steps3 = anglestep(theta3); - //pulse.attach(pulseget,1.0/10000); - // show.attach(pulseshow,1.0/10000); + 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(0); // positief + motor2_dir.write(1); // positief + motor3_dir.write(0); // positief + + if (steps1<0) { + motor1_dir.write(1); // positief + } - //if(quit == 1) - //{ - // pulse.detach(); - // motor1_pwm.write(0); - // motor3_pwm.write(0); - // pc.printf("The motor is off with counts is %i\n\r",counts); - // break; - // } - wait(1.0/10); + 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 (abs(counts1)<=abs(steps1) || abs(counts2)<=abs(steps2) || abs(counts3)<=abs(steps3)) { + pc.printf("while"); + if(abs(counts1)>=abs(steps1)) { + motor1_pwm.write(0); + } + if (abs(counts2)>=abs(steps2)) { + motor2_pwm.write(0); + } + if (abs(counts3)>=abs(steps3)) { + motor3_pwm.write(0); + } } - } \ No newline at end of file + +} + + + wait(1.0/100); + } +} \ No newline at end of file