P-controller geordend
Dependencies: Encoder HIDScope MODSERIAL mbed
Diff: main.cpp
- Revision:
- 11:126ae33919b3
- Parent:
- 10:d5369a546201
diff -r d5369a546201 -r 126ae33919b3 main.cpp --- a/main.cpp Thu Nov 02 11:22:08 2017 +0000 +++ b/main.cpp Thu Nov 02 12:46:35 2017 +0000 @@ -22,7 +22,7 @@ float e_int = 0; //tweede motor -AnalogIn potmeter1(A2); +AnalogIn potMeter1(A2); PwmOut M2E(D5); DigitalOut M2D(D4); Encoder motor2(D9,D8,true); @@ -32,8 +32,10 @@ float e_prev2 = 0; float e_int2 = 0; -float Potmeterwaarde; -float potmeterwaarde2; +double Potmeterwaarde1; +double Potmeterwaarde2; +double Potmeterwaarde; +double potmeterwaarde2; int maxwaarde, maxwaarde2; float refP, refP2; float kp, Proportional, kd, VelocityError, Derivative, ki, Integrator, motorValue; @@ -44,6 +46,68 @@ float Aerror2; float motorVal, motorVal2; +double pi = 3.14159265359; +double SetPx = 38; //Setpoint position x-coordinate from changePosition (EMG dependent) +double SetPy = 30; //Setpoint position y-coordinate from changePosition (EMG dependent) +volatile double q1 = 0; //Reference position q1 from calibration (only the first time) +volatile double q2 = (pi/2); //Reference position q2 from calibration (only the first time) +const double L1 = 30; //Length arm 1 +const double L2 = 38; //Length arm 2 +double K = 1; //Spring constant for movement end-joint to setpoint +double B1 = 1; //Friction coefficient for motor 1 +double B2 = 1; //Friction coefficient for motot 2 +double T = 0.02; //Desired time step +double Motor1Set; //Motor1 angle +double Motor2Set; //Motor2 angle +double p; +double pp; +double bb; +double cc; +double a; +double aa; + +void RKI() +{ + p=sin(q1)*L1; + pp=sin(q2)*L2; + a=cos(q1)*L1; + aa=cos(q2)*L2; + bb=SetPy; + cc=SetPx; + q1 = q1 + ((p + pp)*bb - (a + aa)*cc)*(K*T)/B1; //Calculate desired joint 1 position + q2 = q2 + ((bb - a)*pp + (p - cc)*aa)*(K*T)/B2; //Calculate desired joint 2 position + + int maxwaarde = 4096; // = 64x64 + + + Motor1Set = (q1/(2*pi))*maxwaarde; //Calculate the desired motor1 angle from the desired joint positions + Motor2Set = ((pi-q2-q1)/(2*pi))*maxwaarde; //Calculate the desired motor2 angle from the desired joint positions + + //pc.printf("waarde p = %f, waarde pp = %f, a= %f, aa = %f, bb = %f, cc = %f \r\n",p,pp,a,aa,bb,cc); + //pc.printf("q1 = %f, q2 = %f, Motor1Set = %f, Motor2Set = %f \r\n", q1, q2, Motor1Set, Motor2Set); + //pc.printf("Setpointx = %f, Setpointy = %f \r\n", SetPx, SetPy); +} + +void SetpointRobot() +{ + Potmeterwaarde2 = potMeter2.read(); + Potmeterwaarde1 = potMeter1.read(); + + if (Potmeterwaarde2>0.6) { + SetPx++; // hoe veel verder gaat hij? 1 cm? 10 cm? + } + if (Potmeterwaarde2<0.4) { + SetPx--; + } + if (Potmeterwaarde1>0.6) { + SetPy++; + } + if (Potmeterwaarde1<0.4) { + SetPy--; + } + //pc.printf("Setpointx = %f, Setpointy = %f \r\n", SetPx, SetPy); +} + float GetReferencePosition() { Potmeterwaarde = potMeter2.read(); @@ -54,7 +118,7 @@ float GetReferencePosition2() { - potmeterwaarde2 = potmeter1.read(); + potmeterwaarde2 = potMeter1.read(); maxwaarde2 = 4096; // = 64x64 refP2 = potmeterwaarde2*maxwaarde2; return refP2; // value between 0 and 4096 @@ -155,22 +219,25 @@ void MeasureAndControl(void) { + // RKI aanroepen + SetpointRobot(); + RKI(); // hier the control of the control system - refP = GetReferencePosition(); + //refP = GetReferencePosition(); Huidigepositie = Encoder(); - Aerror = (refP - Huidigepositie);// make an error + Aerror = (Motor1Set - Huidigepositie);// make an error motorValue = FeedBackControl(Aerror, e_prev, e_int); SetMotor1(motorValue); // hier the control of the control system - refP2 = GetReferencePosition2(); + //refP2 = GetReferencePosition2(); Huidigepositie2 = Encoder2(); - Aerror2 = (refP2 - Huidigepositie2);// make an error + Aerror2 = (Motor2Set - Huidigepositie2);// make an error motorValue2 = FeedBackControl2(Aerror2, e_prev2, e_int2); SetMotor2(motorValue2); pc.baud(115200); - pc.printf("potmeter = %f, refP = %f, huidigepositie = %f, error = %f, motorvalue = %f \r\n, potmeter2 = %f, refP2 = %f, huidigepositie2 = %f, error2 = %f, motorvalue2 = %f \r\n", Potmeterwaarde, refP, Huidigepositie, Aerror, motorValue, potmeterwaarde2, refP2, Huidigepositie2, Aerror2, motorValue2); + pc.printf("SetPx = %f, SetPy = %f\r\n potmeter = %f, refP = %f, huidigepositie = %f, error = %f, motorvalue = %f \r\npotmeter2 = %f, refP2 = %f, huidigepositie2 = %f, error2 = %f, motorvalue2 = %f \r\n", SetPx, SetPy, Potmeterwaarde1, Motor1Set, Huidigepositie, Aerror, motorValue, Potmeterwaarde2, Motor2Set, Huidigepositie2, Aerror2, motorValue2); }