Motor control
Dependencies: mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 17:8a0c720733b8
- Parent:
- 16:e51ddfaf2e7a
- Child:
- 18:e0086bd1d87e
diff -r e51ddfaf2e7a -r 8a0c720733b8 main.cpp --- a/main.cpp Mon Oct 21 12:43:43 2019 +0000 +++ b/main.cpp Mon Oct 21 14:27:25 2019 +0000 @@ -34,13 +34,13 @@ DigitalOut motor1Direction(D7); FastPWM motor1Power(D6); -volatile int motor1Toggle = 1; +volatile int motortoggle = 1; //Motorcontrol bool motordir1; bool motordir2; -float motor1ref= 0; -float motor2ref=0; +float motor1ref=0.1745; +float motor2ref=0.0873; double controlsignal1; double controlsignal2; double pi2= 6.283185; @@ -61,9 +61,17 @@ float up2; float ek1; float ek2; -float ei1= 0; -float ei2=0; -float Ka= 1; +float ei1= 0.0; +float ei2=0.0; +float Ka= 1.0; + +//RKI +float Vx; +float Vy; +float q1dot; +float q2dot; +float l1=26.0; //Afstand base-link [cm] +float l2=62.0; //Afstand link-endpoint [cm] //Hidscope HIDScope scope(3); //Going to send 3 channels of data. To access data go to 'http:/localhost:18082/' after starting HIDScope application. @@ -174,22 +182,27 @@ motor1ref= 0.5*pi2+motor1ref; motor2ref= 0.5*pi2+motor2ref; // static float t = 0; - // refangle= pi2/3.0f*sin(5.0f*t)*motor1Toggle; + // refangle= pi2/3.0f*sin(5.0f*t)*motortoggle; //t+=0.01; } -//void RKI(){ - -// RKI magic - -// motor1ref=??; -// motor2ref=??; -// } +void RKI(){ + + Vy=potmeter1.read()*-10.0*motortoggle; + //Vy=potmeter2.read()*10*motortoggle; + //Vx=-1.0*; + Vx=0.0; + q1dot=(l2*cos(motor1ref+motor2ref)*Vx+l2*sin(motor1ref+motor2ref)*Vy)/((-l1*sin(motor1ref)-l2*sin(motor1ref+motor2ref))*l2*cos(motor1ref+motor2ref)+l2*sin(motor1ref+motor2ref)*(l1*cos(motor1ref)+l2*cos(motor1ref+motor2ref))); + q2dot=((-l1*cos(motor1ref)-l2*cos(motor1ref+motor2ref))*Vx+(-l1*sin(motor1ref)-l2*sin(motor1ref+motor2ref))*Vy)/((-l1*sin(motor1ref)-l2*sin(motor1ref+motor2ref))*l2*cos(motor1ref+motor2ref)+l2*sin(motor1ref+motor2ref)*(l1*cos(motor1ref)+l2*cos(motor1ref+motor2ref))); + + motor1ref=motor1ref+q1dot*Ts; + motor2ref=motor2ref+q2dot*Ts; + } void motorControl() { button1.fall(&togglehoek); -// RKI() + RKI(); motor1angle = counts1 * factorin / gearratio; // Angle of motor shaft in rad omega1 = deltaCounts1 / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s motor1error=motor1ref-motor1angle; @@ -215,15 +228,16 @@ void Plotje(){ scope.set(0,motor1ref); //gewenste hoek - scope.set(1,motor1angle); //Gemeten hoek - scope.set(2,motor1error); //verschil in gewenste en gemeten hoek + scope.set(1,motor2ref); + scope.set(2,motor1angle); //Gemeten hoek + scope.set(3,motor1error); //verschil in gewenste en gemeten hoek scope.send(); //send what's in scope memory to PC } void toggleMotor() { - motor1Toggle = !motor1Toggle; + motortoggle = !motortoggle; } int main() @@ -240,11 +254,13 @@ button2.fall(&toggleMotor); while (true) { - pc.printf("Angle1: %f Omega1: %f\r\n", motor1angle, omega1); +// pc.printf("Angle1: %f Omega1: %f\r\n", motor1angle, omega1); pc.printf("refangle1: %f Error1: %f \r\n",motor1ref, motor1error); - pc.printf("Angle2: %f Omega2: %f\r\n", motor2angle, omega2); +// pc.printf("Angle2: %f Omega2: %f\r\n", motor2angle, omega2); pc.printf("refangle2: %f Error2: %f \r\n",motor2ref, motor2error); - pc.printf("controlsignal2: %d \r\n", controlsignal2); +// pc.printf("controlsignal2: %d \r\n", controlsignal2); + pc.printf("Vx: %f Vy: %f \r\n",Vx,Vy); + pc.printf("q1dot: %f q2dot: %f \r\n",q1dot,q2dot); wait(0.5); }