Moet dit er bij
Dependencies: mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 11:cd7be08f46b0
- Parent:
- 10:990287a722d2
- Child:
- 12:55b76c319364
diff -r 990287a722d2 -r cd7be08f46b0 main.cpp --- a/main.cpp Tue Oct 29 20:05:39 2019 +0000 +++ b/main.cpp Tue Oct 29 20:57:56 2019 +0000 @@ -23,15 +23,16 @@ QEI encoder1(D9,D8,NC,64,QEI::X4_ENCODING); //Encoding motor 1 QEI encoder2(D13,D12,NC,64,QEI::X4_ENCODING); //Encoding motor 2 float Ts = 0.01; //Sample time -float motor1angle=-135.0*0.0174532*5.5; //Measured angle motor 1 -float motor2angle; //Measured angle motor 2 +float deg2rad=0.0174532; //Conversion factor degree to rad +float rad2deg=57.29578; //Conversion factor rad to degree +float motor1angle=(-135.0-10.0)*deg2rad*5.5; //Measured angle motor 1 +float motor2angle=(-10.0)*deg2rad*2.75; //Measured angle motor 2 float motor1offset; //Offset bij calibratie float motor2offset; float potmeter; float omega1; //velocity rad/s motor 1 float omega2; //Velocity rad/s motor2 -float deg2rad=0.0174532; //Conversion factor degree to rad -float rad2deg=57.29578; //Conversion factor rad to degree + // Motor @@ -43,8 +44,8 @@ //Motorcontrol bool motordir1; bool motordir2; -float motor1ref=-135.0*0.0174532*5.5; -float motor2ref; +float motor1ref=(-135.0-10.0)*deg2rad*5.5; +float motor2ref=(-10.0)*deg2rad*2.75; double controlsignal1; double controlsignal2; double pi2= 6.283185; @@ -72,7 +73,7 @@ //RKI float Vx=0.0; //Desired linear velocity x direction float Vy=0.0; //Desired linear velocity y direction -float q1=0.0f*deg2rad; //Angle of first joint [rad] +float q1=-10.0f*deg2rad; //Angle of first joint [rad] float q2=-135.0f*deg2rad; //Angle of second joint [rad] float q1dot; //Velocity of first joint [rad/s] float q2dot; //Velocity of second joint [rad/s] @@ -212,9 +213,8 @@ //Vy=potmeter2.read()*10*motortoggle; static float t=0; Vx=6.0f*sin(1.0f*t); + Vy=0.0f; t+=Ts; - //Vx=-1.0*; - Vy=0.0f; q1dot=(l2*cos(q1+q2)*Vx+l2*sin(q1+q2)*Vy)/((-l1*sin(q1)-l2*sin(q1+q2))*l2*cos(q1+q2)+l2*sin(q1+q2)*(l1*cos(q1)+l2*cos(q1+q2))); q2dot=((-l1*cos(q1)-l2*cos(q1+q2))*Vx+(-l1*sin(q1)-l2*sin(q1+q2))*Vy)/((-l1*sin(q1)-l2*sin(q1+q2))*l2*cos(q1+q2)+l2*sin(q1+q2)*(l1*cos(q1)+l2*cos(q1+q2))); q1=q1+q1dot*Ts; @@ -247,25 +247,27 @@ void motorControl(){ counts1= counts1af-counts1offset; - motor1angle = (counts1 * factorin / gearratio)-(135.0*5.5*deg2rad); // Angle of motor shaft in rad + motor1angle = (counts1 * factorin / gearratio)-((135.0*5.5*deg2rad)+(10.0*5.5*deg2rad)); // Angle of motor shaft in rad + correctie voor q1 en q2 omega1 = deltaCounts1 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s motor1error=motor1ref-motor1angle; if (controlsignal1<0){ motordir1= 0;} else { motordir1= 1;} + motor1Power.write(abs(controlsignal1)); motor1Direction= motordir1; counts2= counts2af - counts2offset; - motor2angle = (counts2 * factorin / gearratio); // Angle of motor shaft in rad + motor2angle = (counts2 * factorin / gearratio)-(10.0*2.75*deg2rad); // Angle of motor shaft in rad + correctie voor q1 omega2 = deltaCounts2 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s motor2error=motor2ref-motor2angle; if (controlsignal2<0){ motordir2= 0;} else { motordir2= 1;} - motor2Power.write(abs(controlsignal2)); + if (motor_calibration_done == true){ + motor2Power.write(abs(controlsignal2));} motor2Direction= motordir2; } @@ -398,6 +400,8 @@ pc.printf("counts1offset %i counts1af: %i counts1: %i\r\n",counts1offset,counts1af,counts1); pc.printf("counts2offset %i counts2af: %i counts2: %i\r\n",counts2offset,counts2af,counts2); pc.printf("state: %i motor1ref: %f motor1angle: %f\r\n",motor_curr_state,motor1ref,motor1angle); + pc.printf("q1: %f q2: %f motor1error: %f \r\n",q1*rad2deg,q2*rad2deg, motor1error); + wait(0.5); } }