P-controller geordend
Dependencies: Encoder HIDScope MODSERIAL mbed
main.cpp@11:126ae33919b3, 2017-11-02 (annotated)
- Committer:
- Gerber
- Date:
- Thu Nov 02 12:46:35 2017 +0000
- Revision:
- 11:126ae33919b3
- Parent:
- 10:d5369a546201
tot voor de pauze.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Miriam | 3:f755b4d41aa8 | 1 | //libaries |
Miriam | 0:2a99f692f683 | 2 | #include "mbed.h" |
Miriam | 0:2a99f692f683 | 3 | #include "HIDScope.h" |
Miriam | 0:2a99f692f683 | 4 | #include "encoder.h" |
Miriam | 0:2a99f692f683 | 5 | #include "MODSERIAL.h" |
Miriam | 0:2a99f692f683 | 6 | |
Miriam | 0:2a99f692f683 | 7 | |
Miriam | 3:f755b4d41aa8 | 8 | // globale variables |
Miriam | 3:f755b4d41aa8 | 9 | Ticker AInTicker; //We make a ticker named AIn (use for HIDScope) |
Miriam | 0:2a99f692f683 | 10 | |
Miriam | 3:f755b4d41aa8 | 11 | Ticker Treecko; //We make a awesome ticker for our control system |
Miriam | 3:f755b4d41aa8 | 12 | AnalogIn potMeter2(A1); //Analoge input of potmeter 2 (will be use for te reference position) |
Miriam | 3:f755b4d41aa8 | 13 | PwmOut M1E(D6); //Biorobotics Motor 1 PWM control of the speed |
Miriam | 3:f755b4d41aa8 | 14 | DigitalOut M1D(D7); //Biorobotics Motor 1 diraction control |
Gerber | 8:42d1d02673ae | 15 | Encoder motor1(D13,D12,true); |
Miriam | 0:2a99f692f683 | 16 | |
Miriam | 0:2a99f692f683 | 17 | MODSERIAL pc(USBTX,USBRX); |
Miriam | 0:2a99f692f683 | 18 | |
Miriam | 3:f755b4d41aa8 | 19 | float PwmPeriod = 1.0/5000.0; //set up of PWM periode (5000 Hz, want 5000 periodes in 1 seconde) |
Miriam | 2:b504e35af662 | 20 | const float Ts = 0.1; // tickettijd/ sample time |
Miriam | 2:b504e35af662 | 21 | float e_prev = 0; |
Miriam | 2:b504e35af662 | 22 | float e_int = 0; |
Miriam | 0:2a99f692f683 | 23 | |
paulineoonk | 7:05495acc08b0 | 24 | //tweede motor |
Gerber | 11:126ae33919b3 | 25 | AnalogIn potMeter1(A2); |
paulineoonk | 7:05495acc08b0 | 26 | PwmOut M2E(D5); |
paulineoonk | 7:05495acc08b0 | 27 | DigitalOut M2D(D4); |
paulineoonk | 7:05495acc08b0 | 28 | Encoder motor2(D9,D8,true); |
paulineoonk | 7:05495acc08b0 | 29 | Ticker DubbelTreecko; |
paulineoonk | 7:05495acc08b0 | 30 | |
paulineoonk | 7:05495acc08b0 | 31 | float PwmPeriod2 = 1.0/5000.0; //set up of PWM periode (5000 Hz, want 5000 periodes in 1 seconde) |
paulineoonk | 7:05495acc08b0 | 32 | float e_prev2 = 0; |
paulineoonk | 7:05495acc08b0 | 33 | float e_int2 = 0; |
paulineoonk | 7:05495acc08b0 | 34 | |
Gerber | 11:126ae33919b3 | 35 | double Potmeterwaarde1; |
Gerber | 11:126ae33919b3 | 36 | double Potmeterwaarde2; |
Gerber | 11:126ae33919b3 | 37 | double Potmeterwaarde; |
Gerber | 11:126ae33919b3 | 38 | double potmeterwaarde2; |
Gerber | 9:2435c48d2032 | 39 | int maxwaarde, maxwaarde2; |
Gerber | 9:2435c48d2032 | 40 | float refP, refP2; |
Gerber | 10:d5369a546201 | 41 | float kp, Proportional, kd, VelocityError, Derivative, ki, Integrator, motorValue; |
Gerber | 9:2435c48d2032 | 42 | float kp2, Proportional2, kd2, VelocityError2, Derivative2, ki2, Integrator2, motorValue2; |
Gerber | 10:d5369a546201 | 43 | float Huidigepositie; |
Gerber | 10:d5369a546201 | 44 | float Aerror; |
Gerber | 10:d5369a546201 | 45 | float Huidigepositie2; |
Gerber | 10:d5369a546201 | 46 | float Aerror2; |
Gerber | 10:d5369a546201 | 47 | float motorVal, motorVal2; |
Miriam | 3:f755b4d41aa8 | 48 | |
Gerber | 11:126ae33919b3 | 49 | double pi = 3.14159265359; |
Gerber | 11:126ae33919b3 | 50 | double SetPx = 38; //Setpoint position x-coordinate from changePosition (EMG dependent) |
Gerber | 11:126ae33919b3 | 51 | double SetPy = 30; //Setpoint position y-coordinate from changePosition (EMG dependent) |
Gerber | 11:126ae33919b3 | 52 | volatile double q1 = 0; //Reference position q1 from calibration (only the first time) |
Gerber | 11:126ae33919b3 | 53 | volatile double q2 = (pi/2); //Reference position q2 from calibration (only the first time) |
Gerber | 11:126ae33919b3 | 54 | const double L1 = 30; //Length arm 1 |
Gerber | 11:126ae33919b3 | 55 | const double L2 = 38; //Length arm 2 |
Gerber | 11:126ae33919b3 | 56 | double K = 1; //Spring constant for movement end-joint to setpoint |
Gerber | 11:126ae33919b3 | 57 | double B1 = 1; //Friction coefficient for motor 1 |
Gerber | 11:126ae33919b3 | 58 | double B2 = 1; //Friction coefficient for motot 2 |
Gerber | 11:126ae33919b3 | 59 | double T = 0.02; //Desired time step |
Gerber | 11:126ae33919b3 | 60 | double Motor1Set; //Motor1 angle |
Gerber | 11:126ae33919b3 | 61 | double Motor2Set; //Motor2 angle |
Gerber | 11:126ae33919b3 | 62 | double p; |
Gerber | 11:126ae33919b3 | 63 | double pp; |
Gerber | 11:126ae33919b3 | 64 | double bb; |
Gerber | 11:126ae33919b3 | 65 | double cc; |
Gerber | 11:126ae33919b3 | 66 | double a; |
Gerber | 11:126ae33919b3 | 67 | double aa; |
Gerber | 11:126ae33919b3 | 68 | |
Gerber | 11:126ae33919b3 | 69 | void RKI() |
Gerber | 11:126ae33919b3 | 70 | { |
Gerber | 11:126ae33919b3 | 71 | p=sin(q1)*L1; |
Gerber | 11:126ae33919b3 | 72 | pp=sin(q2)*L2; |
Gerber | 11:126ae33919b3 | 73 | a=cos(q1)*L1; |
Gerber | 11:126ae33919b3 | 74 | aa=cos(q2)*L2; |
Gerber | 11:126ae33919b3 | 75 | bb=SetPy; |
Gerber | 11:126ae33919b3 | 76 | cc=SetPx; |
Gerber | 11:126ae33919b3 | 77 | q1 = q1 + ((p + pp)*bb - (a + aa)*cc)*(K*T)/B1; //Calculate desired joint 1 position |
Gerber | 11:126ae33919b3 | 78 | q2 = q2 + ((bb - a)*pp + (p - cc)*aa)*(K*T)/B2; //Calculate desired joint 2 position |
Gerber | 11:126ae33919b3 | 79 | |
Gerber | 11:126ae33919b3 | 80 | int maxwaarde = 4096; // = 64x64 |
Gerber | 11:126ae33919b3 | 81 | |
Gerber | 11:126ae33919b3 | 82 | |
Gerber | 11:126ae33919b3 | 83 | Motor1Set = (q1/(2*pi))*maxwaarde; //Calculate the desired motor1 angle from the desired joint positions |
Gerber | 11:126ae33919b3 | 84 | Motor2Set = ((pi-q2-q1)/(2*pi))*maxwaarde; //Calculate the desired motor2 angle from the desired joint positions |
Gerber | 11:126ae33919b3 | 85 | |
Gerber | 11:126ae33919b3 | 86 | //pc.printf("waarde p = %f, waarde pp = %f, a= %f, aa = %f, bb = %f, cc = %f \r\n",p,pp,a,aa,bb,cc); |
Gerber | 11:126ae33919b3 | 87 | //pc.printf("q1 = %f, q2 = %f, Motor1Set = %f, Motor2Set = %f \r\n", q1, q2, Motor1Set, Motor2Set); |
Gerber | 11:126ae33919b3 | 88 | //pc.printf("Setpointx = %f, Setpointy = %f \r\n", SetPx, SetPy); |
Gerber | 11:126ae33919b3 | 89 | } |
Gerber | 11:126ae33919b3 | 90 | |
Gerber | 11:126ae33919b3 | 91 | void SetpointRobot() |
Gerber | 11:126ae33919b3 | 92 | { |
Gerber | 11:126ae33919b3 | 93 | Potmeterwaarde2 = potMeter2.read(); |
Gerber | 11:126ae33919b3 | 94 | Potmeterwaarde1 = potMeter1.read(); |
Gerber | 11:126ae33919b3 | 95 | |
Gerber | 11:126ae33919b3 | 96 | if (Potmeterwaarde2>0.6) { |
Gerber | 11:126ae33919b3 | 97 | SetPx++; // hoe veel verder gaat hij? 1 cm? 10 cm? |
Gerber | 11:126ae33919b3 | 98 | } |
Gerber | 11:126ae33919b3 | 99 | if (Potmeterwaarde2<0.4) { |
Gerber | 11:126ae33919b3 | 100 | SetPx--; |
Gerber | 11:126ae33919b3 | 101 | } |
Gerber | 11:126ae33919b3 | 102 | if (Potmeterwaarde1>0.6) { |
Gerber | 11:126ae33919b3 | 103 | SetPy++; |
Gerber | 11:126ae33919b3 | 104 | } |
Gerber | 11:126ae33919b3 | 105 | if (Potmeterwaarde1<0.4) { |
Gerber | 11:126ae33919b3 | 106 | SetPy--; |
Gerber | 11:126ae33919b3 | 107 | } |
Gerber | 11:126ae33919b3 | 108 | //pc.printf("Setpointx = %f, Setpointy = %f \r\n", SetPx, SetPy); |
Gerber | 11:126ae33919b3 | 109 | } |
Gerber | 11:126ae33919b3 | 110 | |
Miriam | 0:2a99f692f683 | 111 | float GetReferencePosition() |
Miriam | 0:2a99f692f683 | 112 | { |
Gerber | 8:42d1d02673ae | 113 | Potmeterwaarde = potMeter2.read(); |
Gerber | 9:2435c48d2032 | 114 | maxwaarde = 4096; // = 64x64 |
Gerber | 9:2435c48d2032 | 115 | refP = Potmeterwaarde*maxwaarde; |
Miriam | 3:f755b4d41aa8 | 116 | return refP; // value between 0 and 4096 |
Miriam | 0:2a99f692f683 | 117 | } |
paulineoonk | 7:05495acc08b0 | 118 | |
paulineoonk | 7:05495acc08b0 | 119 | float GetReferencePosition2() |
paulineoonk | 7:05495acc08b0 | 120 | { |
Gerber | 11:126ae33919b3 | 121 | potmeterwaarde2 = potMeter1.read(); |
Gerber | 9:2435c48d2032 | 122 | maxwaarde2 = 4096; // = 64x64 |
Gerber | 9:2435c48d2032 | 123 | refP2 = potmeterwaarde2*maxwaarde2; |
paulineoonk | 7:05495acc08b0 | 124 | return refP2; // value between 0 and 4096 |
paulineoonk | 7:05495acc08b0 | 125 | } |
Miriam | 0:2a99f692f683 | 126 | |
Miriam | 2:b504e35af662 | 127 | float FeedBackControl(float error, float &e_prev, float &e_int) // schaalt de snelheid naar de snelheid zodat onze chip het begrijpt (is nog niet in werking) |
Miriam | 0:2a99f692f683 | 128 | { |
Gerber | 9:2435c48d2032 | 129 | kp = 0.001; // kind of scaled. |
Gerber | 9:2435c48d2032 | 130 | Proportional= kp*error; |
Miriam | 2:b504e35af662 | 131 | |
Gerber | 9:2435c48d2032 | 132 | kd = 0.0004; // kind of scaled. |
Gerber | 9:2435c48d2032 | 133 | VelocityError = (error - e_prev)/Ts; |
Gerber | 9:2435c48d2032 | 134 | Derivative = kd*VelocityError; |
Miriam | 2:b504e35af662 | 135 | e_prev = error; |
Miriam | 2:b504e35af662 | 136 | |
Gerber | 10:d5369a546201 | 137 | ki = 0.0005; // kind of scaled. |
Miriam | 2:b504e35af662 | 138 | e_int = e_int+Ts*error; |
Gerber | 9:2435c48d2032 | 139 | Integrator = ki*e_int; |
Miriam | 2:b504e35af662 | 140 | |
Miriam | 2:b504e35af662 | 141 | |
Gerber | 10:d5369a546201 | 142 | motorVal = Proportional + Integrator + Derivative; |
Gerber | 10:d5369a546201 | 143 | return motorVal; |
Miriam | 0:2a99f692f683 | 144 | } |
Miriam | 0:2a99f692f683 | 145 | |
paulineoonk | 7:05495acc08b0 | 146 | float FeedBackControl2(float error2, float &e_prev2, float &e_int2) // schaalt de snelheid naar de snelheid zodat onze chip het begrijpt (is nog niet in werking) |
paulineoonk | 7:05495acc08b0 | 147 | { |
Gerber | 9:2435c48d2032 | 148 | kp2 = 0.001; // kind of scaled. |
Gerber | 9:2435c48d2032 | 149 | Proportional2= kp2*error2; |
paulineoonk | 7:05495acc08b0 | 150 | |
Gerber | 9:2435c48d2032 | 151 | kd2 = 0.0004; // kind of scaled. |
Gerber | 9:2435c48d2032 | 152 | VelocityError2 = (error2 - e_prev2)/Ts; |
Gerber | 9:2435c48d2032 | 153 | Derivative2 = kd2*VelocityError2; |
paulineoonk | 7:05495acc08b0 | 154 | e_prev2 = error2; |
paulineoonk | 7:05495acc08b0 | 155 | |
Gerber | 10:d5369a546201 | 156 | ki2 = 0.0005; // kind of scaled. |
paulineoonk | 7:05495acc08b0 | 157 | e_int2 = e_int2+Ts*error2; |
Gerber | 9:2435c48d2032 | 158 | Integrator2 = ki2*e_int2; |
paulineoonk | 7:05495acc08b0 | 159 | |
paulineoonk | 7:05495acc08b0 | 160 | |
Gerber | 10:d5369a546201 | 161 | motorVal2 = Proportional2 + Integrator2 + Derivative2; |
Gerber | 10:d5369a546201 | 162 | return motorVal2; |
paulineoonk | 7:05495acc08b0 | 163 | } |
paulineoonk | 7:05495acc08b0 | 164 | |
paulineoonk | 7:05495acc08b0 | 165 | |
Miriam | 0:2a99f692f683 | 166 | void SetMotor1(float motorValue) |
Miriam | 0:2a99f692f683 | 167 | { |
Miriam | 1:609671b1c96c | 168 | if (motorValue >= 0) |
Miriam | 0:2a99f692f683 | 169 | { |
Miriam | 0:2a99f692f683 | 170 | M1D = 0; |
Miriam | 0:2a99f692f683 | 171 | } |
Miriam | 0:2a99f692f683 | 172 | else |
Miriam | 0:2a99f692f683 | 173 | { |
Miriam | 0:2a99f692f683 | 174 | M1D = 1; |
Miriam | 0:2a99f692f683 | 175 | } |
Miriam | 0:2a99f692f683 | 176 | |
Miriam | 0:2a99f692f683 | 177 | if (fabs(motorValue) > 1) |
Miriam | 0:2a99f692f683 | 178 | { |
Miriam | 3:f755b4d41aa8 | 179 | M1E = 1; //de snelheid wordt teruggeschaald naar 8.4 rad/s (maximale snelheid, dus waarde 1) |
Miriam | 0:2a99f692f683 | 180 | } |
Miriam | 0:2a99f692f683 | 181 | else |
Miriam | 0:2a99f692f683 | 182 | { |
Miriam | 0:2a99f692f683 | 183 | M1E = fabs(motorValue); //de absolute snelheid wordt bepaald, de motor staat uit bij een waarde 0 |
Miriam | 0:2a99f692f683 | 184 | } |
Miriam | 0:2a99f692f683 | 185 | } |
Miriam | 0:2a99f692f683 | 186 | |
paulineoonk | 7:05495acc08b0 | 187 | void SetMotor2(float motorValue2) |
paulineoonk | 7:05495acc08b0 | 188 | { |
paulineoonk | 7:05495acc08b0 | 189 | if (motorValue2 >= 0) |
paulineoonk | 7:05495acc08b0 | 190 | { |
Gerber | 10:d5369a546201 | 191 | M2D = 1; |
paulineoonk | 7:05495acc08b0 | 192 | } |
paulineoonk | 7:05495acc08b0 | 193 | else |
paulineoonk | 7:05495acc08b0 | 194 | { |
Gerber | 10:d5369a546201 | 195 | M2D = 0; |
paulineoonk | 7:05495acc08b0 | 196 | } |
paulineoonk | 7:05495acc08b0 | 197 | |
paulineoonk | 7:05495acc08b0 | 198 | if (fabs(motorValue2) > 1) |
paulineoonk | 7:05495acc08b0 | 199 | { |
paulineoonk | 7:05495acc08b0 | 200 | M2E = 1; //de snelheid wordt teruggeschaald naar 8.4 rad/s (maximale snelheid, dus waarde 1) |
paulineoonk | 7:05495acc08b0 | 201 | } |
paulineoonk | 7:05495acc08b0 | 202 | else |
paulineoonk | 7:05495acc08b0 | 203 | { |
paulineoonk | 7:05495acc08b0 | 204 | M2E = fabs(motorValue2); //de absolute snelheid wordt bepaald, de motor staat uit bij een waarde 0 |
paulineoonk | 7:05495acc08b0 | 205 | } |
paulineoonk | 7:05495acc08b0 | 206 | } |
paulineoonk | 7:05495acc08b0 | 207 | |
Miriam | 0:2a99f692f683 | 208 | float Encoder () |
Miriam | 0:2a99f692f683 | 209 | { |
Gerber | 10:d5369a546201 | 210 | Huidigepositie = motor1.getPosition (); |
Miriam | 3:f755b4d41aa8 | 211 | return Huidigepositie; // huidige positie = current position |
Miriam | 0:2a99f692f683 | 212 | } |
Miriam | 0:2a99f692f683 | 213 | |
paulineoonk | 7:05495acc08b0 | 214 | float Encoder2 () |
paulineoonk | 7:05495acc08b0 | 215 | { |
Gerber | 10:d5369a546201 | 216 | Huidigepositie2 = motor2.getPosition (); |
paulineoonk | 7:05495acc08b0 | 217 | return Huidigepositie2; // huidige positie = current position |
paulineoonk | 7:05495acc08b0 | 218 | } |
paulineoonk | 7:05495acc08b0 | 219 | |
Miriam | 0:2a99f692f683 | 220 | void MeasureAndControl(void) |
Miriam | 0:2a99f692f683 | 221 | { |
Gerber | 11:126ae33919b3 | 222 | // RKI aanroepen |
Gerber | 11:126ae33919b3 | 223 | SetpointRobot(); |
Gerber | 11:126ae33919b3 | 224 | RKI(); |
Miriam | 3:f755b4d41aa8 | 225 | // hier the control of the control system |
Gerber | 11:126ae33919b3 | 226 | //refP = GetReferencePosition(); |
Gerber | 10:d5369a546201 | 227 | Huidigepositie = Encoder(); |
Gerber | 11:126ae33919b3 | 228 | Aerror = (Motor1Set - Huidigepositie);// make an error |
Gerber | 10:d5369a546201 | 229 | motorValue = FeedBackControl(Aerror, e_prev, e_int); |
Miriam | 0:2a99f692f683 | 230 | SetMotor1(motorValue); |
Miriam | 0:2a99f692f683 | 231 | |
paulineoonk | 7:05495acc08b0 | 232 | // hier the control of the control system |
Gerber | 11:126ae33919b3 | 233 | //refP2 = GetReferencePosition2(); |
Gerber | 10:d5369a546201 | 234 | Huidigepositie2 = Encoder2(); |
Gerber | 11:126ae33919b3 | 235 | Aerror2 = (Motor2Set - Huidigepositie2);// make an error |
Gerber | 10:d5369a546201 | 236 | motorValue2 = FeedBackControl2(Aerror2, e_prev2, e_int2); |
paulineoonk | 7:05495acc08b0 | 237 | SetMotor2(motorValue2); |
Gerber | 8:42d1d02673ae | 238 | |
Gerber | 8:42d1d02673ae | 239 | pc.baud(115200); |
Gerber | 11:126ae33919b3 | 240 | 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); |
paulineoonk | 7:05495acc08b0 | 241 | } |
paulineoonk | 7:05495acc08b0 | 242 | |
Miriam | 0:2a99f692f683 | 243 | |
Miriam | 0:2a99f692f683 | 244 | int main() |
Miriam | 0:2a99f692f683 | 245 | { |
Miriam | 5:987cc578988e | 246 | M1E.period(PwmPeriod); |
Gerber | 8:42d1d02673ae | 247 | M2E.period(PwmPeriod2); |
Miriam | 4:c119259c1ba5 | 248 | Treecko.attach(MeasureAndControl, Ts); //Elke 1 seconde zorgt de ticker voor het runnen en uitlezen van de verschillende |
Miriam | 0:2a99f692f683 | 249 | //functies en analoge signalen. Veranderingen worden elke 1 seconde doorgevoerd. |
Gerber | 8:42d1d02673ae | 250 | //DubbelTreecko.attach(MeasureAndControl2, Ts); |
Miriam | 4:c119259c1ba5 | 251 | |
Miriam | 4:c119259c1ba5 | 252 | |
Miriam | 0:2a99f692f683 | 253 | while(1) |
Miriam | 0:2a99f692f683 | 254 | { |
Miriam | 0:2a99f692f683 | 255 | wait(0.2); |
Gerber | 8:42d1d02673ae | 256 | //pc.printf("Potmeter2 = %f, refP = %f, motorValue = %f, \r\nPotmeter1 = %f, refP2 = %f, motorValue2 = %f \r\n", Potmeterwaarde, refP, motorValue, potmeterwaarde2, refP2, motorValue2); |
Gerber | 8:42d1d02673ae | 257 | //pc.baud(115200); |
Miriam | 0:2a99f692f683 | 258 | float B = motor1.getPosition(); |
Miriam | 0:2a99f692f683 | 259 | float Potmeterwaarde = potMeter2.read(); |
Miriam | 0:2a99f692f683 | 260 | //float positie = B%4096; |
Miriam | 6:083bd713670b | 261 | //pc.printf("pos: %d, speed %f, potmeter = %f V, \r\n",motor1.getPosition(), motor1.getSpeed(),(potMeter2.read()*3.3)); //potmeter uitlezen. tussen 0-1. voltage, dus *3.3V |
Miriam | 0:2a99f692f683 | 262 | } |
Miriam | 0:2a99f692f683 | 263 | } |