![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Signa-bot code for project BioRobotics, at University of Twente.
Dependencies: mbed QEI MODSERIAL FastPWM ttmath Math
Diff: Motor_tryout.cpp
- Revision:
- 32:60a71dcfdb7a
- Parent:
- 31:3c13f1c35ee5
- Child:
- 33:88fbf14d8aaf
--- a/Motor_tryout.cpp Thu Oct 31 12:42:24 2019 +0000 +++ b/Motor_tryout.cpp Thu Oct 31 14:30:59 2019 +0000 @@ -57,9 +57,6 @@ float z0=-172; float y0=0; float x0=0; -float x00; -float y00; -float z00; float theta1; float theta2; @@ -97,49 +94,126 @@ float derivative2 = 0; float derivative3 = 0; -//bool timer = false; +float angle1 = 0; +float angle2 = 0; +float angle3 = 0; + +bool timer = false; +float time_sin = 0; + + +void delta_calctheta1 () { + 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; + } + theta1 = (beta - alpha); + } +} + +void delta_calctheta2 () { + float xref = x0*cospi+y0*sinpi; + float yref = y0*cospi-x0*sinpi; + float zref = z0; + float y2 = yref + le; + float y1 = f; + float z1 = 0.0; + float z2 = zref; + float rje2 = re*re - xref*xref; + float rje = sqrt(rje2); + float r2 = (y1-y2)*(y1-y2) + (z1-zref)*(z1-zref); + 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; + } + theta2 = (beta - alpha); + } +} + +void delta_calctheta3 () { + float xreff = x0*cospi-y0*sinpi; + float yreff = y0*cospi+y0*sinpi; + float zreff = z0; + float y2 = yreff + le; + float y1 = f; + float z1 = 0.0; + float z2 = zreff; + float rje2 = re*re - xreff*xreff; + float rje = sqrt(rje2); + float r2 = (y1-y2)*(y1-y2) + (z1-zreff)*(z1-zreff); + 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; + } + theta3 = (beta - alpha); + } +} + + + Ticker motor_timer; -void motor() -{ +void motor(){ + counts1 = Encoder1.getPulses(); counts2 = Encoder2.getPulses(); counts3 = Encoder3.getPulses(); - -// angle1 = counts1/(8400.0)*2.0*pie; -// angle2 = counts2/(8400.0)*2.0*pie; -// angle3 = counts3/(8400.0)*2.0*pie; - -// sinewave = sin(time_sin); + + z0 = -172.0 + 10* sin(time_sin); - lasterror1 = error1; - lasterror2 = error2; - lasterror3 = error3; + delta_calctheta1 (); + delta_calctheta2 (); + delta_calctheta3 (); - error1 = counts1 - steps1; - error2 = counts2 - steps2; - error3 = counts3 - steps3; + angle1 = counts1/(8400.0)*2.0*pi; + angle2 = counts2/(8400.0)*2.0*pi; + angle3 = counts3/(8400.0)*2.0*pi; - derivative1 = error1 - lasterror1; - derivative2 = error2 - lasterror2; - derivative3 = error3 - lasterror3; + error1 = theta1 - angle1; + error2 = theta2 - angle2; + error3 = theta3 - angle3; - Kp = 50; + Kp = 10; Kd = potmeter1 * 1000; - - // Proportional part - u_k1 = Kp * error1; + u_k1 = Kp * error1; u_k2 = Kp * error2; u_k3 = Kp * error3; - - u_d1 = Kd * derivative1; - u_d2 = Kd * derivative2; - u_d3 = Kd * derivative3; - newmotor1 = u_k1 + u_d1; - newmotor2 = u_k2 + u_d2; - newmotor3 = u_k3 + u_d3; + newmotor1 = u_k1; + newmotor2 = u_k2; + newmotor3 = u_k3; if (newmotor1>1.0f){ newmotor1 =1.0f; @@ -162,204 +236,19 @@ newmotor3 = -1.0f; } - if (timer==true) { motor1_pwm.write(fabs(newmotor1)); - motor1_dir.write(newmotor1>0); + motor1_dir.write(newmotor1<0); motor2_pwm.write(fabs(newmotor2)); - motor2_dir.write(newmotor2<0); + motor2_dir.write(newmotor2>0); motor3_pwm.write(fabs(newmotor3)); - motor3_dir.write(newmotor3>0); - } + motor3_dir.write(newmotor3<0); -// else { -// motor1_pwm.write(0); -// motor2_pwm.write(0); -// motor3_pwm.write(0); -// } + time_sin += 0.002; } - -float delta_calcangleyz(float x00, float y00, float z00) { - float y2 = y00 + le; - float y1 = f; - float z1 = 0.0; - float z2 = z00; - float rje2 = re*re - x00*x00; - float rje = sqrt(rje2); - float r2 = (y1-y2)*(y1-y2) + (z1-z00)*(z1-z00); - 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; - return theta1; - } -} - - - -float delta_calcinverse(float x00, float y00, float z00) { - theta1 = theta2 = theta3 = 0; - x00=x0; - y00=y0; - z00=z0; - theta1 = delta_calcangleyz(x00, y00, z00); - - if (check == 0) { - x00=x0*cospi+y0*sinpi; - y00=y0*cospi-x0*sinpi; - z00=z0; - theta2 = delta_calcangleyz(x00, y00, z00); - x00=x0*cospi-y0*sinpi; - y00=y0*cospi+y0*sinpi; - z00=z0; - theta3 = delta_calcangleyz(x00, y00, z00); - } - - return theta1, theta2, theta3; -} - -//double error; - -// Calculate the steps from angle -float anglestep(float angle) { - float steps; - steps = angle / 360 * 8400; - return steps; - } - -// Omschrijving -float movefunctioninit () { - - theta1 = delta_calcinverse(x00,y00,z00); - -// theta1 = theta1 - oldtheta1; -// theta2 = theta2 - oldtheta2; -// theta3 = theta3 - oldtheta3; -// -// steps1 = steps1 + anglestep(theta1); -// steps2 = steps2 + anglestep(theta2); -// steps3 = steps3 + anglestep(theta3); - - steps1 = anglestep(theta1); - steps2 = anglestep(theta2); - steps3 = anglestep(theta3); - - pc.printf("\n\r de hoeken zijn(%f, %f, %f)", theta1, theta2, theta3); - pc.printf("\n\r coordinaten(%f, %f, %f)", x0, y0, z0); - pc.printf("\n\rsteps1 %f, steps2 %f, steps3 %f", steps1, steps2, steps3); - pc.printf("\n\rcounts1 %f, counts2 %f, counts3 %f", counts1, counts2, counts3); - - - -// pc.printf("\n\r error1: %f", error1); - - - // Set the direction of the motors. - // if (theta1 < 0) { -// motor1_dir.write(1); -// } -// else { -// motor1_dir.write(0); -// } -// -// if (theta2 < 0) { -// motor2_dir.write(0); -// } -// else { -// motor2_dir.write(1); -// } -// -// if (theta3 < 0) { -// motor3_dir.write(0); -// } -// else { -// motor3_dir.write(1); -// } -// -// int frequency_pwm = 10000; //10 kHz PWM -// motor1_pwm.period(1.0/(double)frequency_pwm); // T=1/f -// motor1_pwm.write(0.57); // write Duty Cycle -// -// motor2_pwm.period(1.0/(double)frequency_pwm); // T=1/f -// motor2_pwm.write(0.57); // write Duty Cycle -// -// motor3_pwm.period(1.0/(double)frequency_pwm); // T=1/f -// motor3_pwm.write(0.57); // write Duty Cycle -// -// check1 = true; -// check2 = true; -// check3 = true; -// -// Encoder1.reset(); -// Encoder2.reset(); -// Encoder3.reset(); -// -} - -// Omschrijving -//float movestop() { -//wait(0.6); -//timer = false; -//} -// -// while (check1 || check2 || check3) { -// -// int frequency_pwm = 10000; //10 kHz PWM -// motor1_pwm.period(1.0/(double)frequency_pwm); // T=1/f -// motor1_pwm.write(newmotor1); // write Duty Cycle -// -// motor2_pwm.period(1.0/(double)frequency_pwm); // T=1/f -// motor2_pwm.write(newmotor2); // write Duty Cycle -// -// motor3_pwm.period(1.0/(double)frequency_pwm); // T=1/f -// motor3_pwm.write(newmotor3); // write Duty Cycle -// -// if (error1==0) { -// check1=false; -// } -// if (error2==0) { -// check2=false; -// } -// if (error3==0) { -// check3=false; -// } -// -//// if(abs(counts1)>=abs(steps1)) { -//// pc.printf("\n\r 1 is false"); -//// motor1_pwm.write(0); -//// check1=false; -//// pc.printf("\n\rcounts1 %i", counts1); -//// } -//// -//// if (abs(counts2)>=abs(steps2)) { -//// pc.printf("\n\r 2 is false"); -//// motor2_pwm.write(0); -//// check2=false; -//// pc.printf("\n\rcounts2 %i", counts2); -//// } -//// -//// if (abs(counts3)>=abs(steps3)) { -//// pc.printf("\n\r 3 is false"); -//// motor3_pwm.write(0); -//// check3=false; -//// pc.printf("\n\rcounts3 %i", counts3); -//// } -// } -// wait(1.0); -// pc.printf("\n\rdone moving"); -//} - + /////////////////// // MAIN FUNCTION // @@ -372,116 +261,8 @@ char cc = pc.getc(); int frequency_pwm = 10000; //10 kHz PWM -// motor1_pwm.period(1.0/(double)frequency_pwm); // T=1/f -// motor1_pwm.write(newmotor1); // write Duty Cycle -// -// motor2_pwm.period(1.0/(double)frequency_pwm); // T=1/f -// motor2_pwm.write(newmotor2); // write Duty Cycle -// -// motor3_pwm.period(1.0/(double)frequency_pwm); // T=1/f -// motor3_pwm.write(newmotor3); // write Duty Cycle while(true){ - pc.printf("\n\r----------------------------------------------------------"); - pc.printf("\n\r----------------------------------------------------------"); - - - delta_calcinverse(x0,y0,z0); - - oldtheta1 = theta1; - oldtheta2 = theta2; - oldtheta3 = theta3; - - char cc = pc.getc(); - - // timer = true; - - - if (cc=='d') { - x0=x0+5.0f; - if (x0>=-75 && x0<=75){ - movefunctioninit (); -// movestop(); - } - else { - // timer = false; - x0=x0-5.0f; - - } } - - if (cc=='a') { - x0=x0-5.0f; - if (x0>=-75 && x0<=75){ - movefunctioninit (); -// movestop(); - } - else { - // timer = false; - x0=x0+5.0f; - } - } - - if (cc=='w'){ - y0=y0+5.0f; - if (y0>=-75 && y0<=75){ - movefunctioninit (); - // movestop(); - } - else { -// timer = false; - y0=y0-5.0f; - } - } - - if (cc=='s'){ - y0=y0-5.0f; - if (y0>=-75 && y0<=75) { - movefunctioninit (); -// movestop(); - } - else { -// timer = false; - y0=y0+5.0f; - } - } - - if (cc=='u') - { - z0=z0+1.0f; - if (z0>=-210 && z0<=-130) { - movefunctioninit (); - pc.printf("\n\r de hoeken zijn(%f, %f, %f)", theta1, theta2, theta3); - pc.printf("\n\r coordinaten(%f, %f, %f)", x0, y0, z0); - pc.printf("\n\rsteps1 %f, steps2 %f, steps3 %f", steps1, steps2, steps3); - pc.printf("\n\rcounts1 %f, counts2 %f, counts3 %f", counts1, counts2, counts3); -// movestop(); -// for (float i=0.0; i<5; i++) { -// pc.printf("\n\r dit is error1 %f, error2 %f, error3 %f", error1, error2, error3); -// wait(1.0/10.0); -// } - } - else{ - pc.printf("else statement"); -// timer = false; - z0=z0-5.0f; - } - } - - if (cc=='j') - { - z0=z0-1.0f; - if (z0>=-210 && z0<=-130){ - movefunctioninit (); - // movestop(); - } - else { -// timer = false; - z0=z0+5.0f; - } - } - // end while - } - -// end main +//END MAIN } \ No newline at end of file