Motor control
Dependencies: mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
Diff: main.cpp
- Revision:
- 9:08a7a8e59a6a
- Parent:
- 8:c6c94d55b088
- Child:
- 10:b871d1b05787
diff -r c6c94d55b088 -r 08a7a8e59a6a main.cpp --- a/main.cpp Mon Oct 14 07:14:19 2019 +0000 +++ b/main.cpp Mon Oct 14 09:06:08 2019 +0000 @@ -11,7 +11,7 @@ AnalogIn potmeter1(A0); AnalogIn potmeter2(A1); AnalogIn potmeter3(A2); -AnalogIn potmeter4(A4); +AnalogIn potmeter4(A3); // Encoder DigitalIn encA(D13); DigitalIn encB(D12); @@ -32,7 +32,7 @@ //Motorcontrol bool motordir; double motorpwm; -float u1; +float u1= 0; double u2; double potValue; double pi2= 6.283185; @@ -72,16 +72,18 @@ static BiQuad LowPassFilter(0.0640,0.1279,0.0640,-1.1683,0.4241); //Proportional part: - Kp=2*potmeter2.read(); + //Kp=2*potmeter1.read(); + Kp=101; u_k=Kp*e; //Integral part - Ki=2*potmeter3.read(); + Ki=10; error_integral=error_integral+e*Ts; u_i=Ki*error_integral; //Derivative part - Kd=2*potmeter4.read(); + //Kd=2*potmeter2.read(); + Kd=10; float error_derivative =(e-e_prev)/Ts; float filtered_error_derivative = LowPassFilter.step(error_derivative); u_d=Kd*filtered_error_derivative; @@ -99,12 +101,18 @@ countsPrev = counts; } + +void togglehoek(){ + u1= 0.25*pi2+u1; + } + void motorControl() { + button1.fall(&togglehoek); angle = counts * factorin / gearratio; // Angle of motor shaft in rad omega = deltaCounts / Ts * factorin / gearratio; // Angular velocity of motor shaft in rad/s potValue= potmeter1.read(); - u1= (potValue*2*pi2)-pi2; + //u1= (potValue*2*pi2)-pi2; e=u1-angle; u2=PID_controller(e); @@ -143,6 +151,7 @@ encoderTicker.attach(readEncoder, Ts); button2.fall(&toggleMotor); + while (true) { //pc.printf("Potmeter: %d \r\n", potValue,);