![](/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:
- 22:9f911405e096
- Parent:
- 21:c826abca79c3
- Child:
- 23:18b0be02187f
diff -r c826abca79c3 -r 9f911405e096 Motor_tryout.cpp --- a/Motor_tryout.cpp Fri Oct 25 11:23:15 2019 +0000 +++ b/Motor_tryout.cpp Fri Oct 25 11:44:33 2019 +0000 @@ -37,6 +37,8 @@ const float re = 174.0; const float rf = 50.0; const float pi = 3.14159265358979323846; +const float cospi = -0.5; +const float sinpi = 0.8660254; float y2; float y1; float z1; @@ -67,7 +69,7 @@ float z2 = z00; float rje2 = re*re - x00*x00; float rje = sqrt(rje2); - float r2 = (y1-y2)(y1-y2) + (z1-z00)(z1-z00); + float r2 = (y1-y2)*(y1-y2) + (z1-z00)*(z1-z00); float r = sqrt(r2); if ((r+rje<rf) || (r + rf <rje) || (rf+rje<r)) { @@ -82,22 +84,25 @@ } 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(x0*cos(2/3*pi) + y0*sin(2/3*pi), y0*cos(2/3*pi)-x0*sin(2/3*pi), z0); + theta2 = delta_calcangleyz(x00, y00, z00); x00=x0*cospi-y0*sinpi; y00=y0*cospi+y0*sinpi; z00=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); + theta3 = delta_calcangleyz(x00, y00, z00); } return theta1, theta2, theta3; @@ -117,9 +122,9 @@ } -float move_steps() { +float movefunction() { - theta1 = delta_calcinverse(x0,y0,z0); + theta1 = delta_calcinverse(x00,y00,z00); pc.printf("\n\r de hoeken zijn(%f, %f, %f)", theta1, theta2, theta3); pc.printf("\n\r coordinaten(%f, %f, %f)", x0, y0, z0); @@ -161,7 +166,11 @@ 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 + motor3_pwm.write(0.7); // write Duty Cycle + + check1 = true; + check2 = true; + check3 = true; while (check1 || check2 || check3) { counts1 = Encoder1.getPulses(); @@ -190,8 +199,9 @@ wait(1.0/100); } } -int main(void) -{ +} + +int main(void) { pc.baud(115200); char cc = pc.getc(); @@ -209,13 +219,13 @@ char cc = pc.getc(); if (cc=='d') { -x0=x0+2.0f; -if (x0>=-75 && x0<=75) { - movefunction (); - } - else { - x0=x0-2.0f; - } + x0=x0+2.0f; + if (x0>=-75 && x0<=75) { + movefunction (); + } + else { + x0=x0-2.0f; + } } if (cc=='a') { @@ -267,6 +277,5 @@ z0=z0+2.0f; } } - - - \ No newline at end of file +} +} \ No newline at end of file