Projectgroep 20 Biorobotics / Mbed 2 deprecated week6ordenenscript

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?

UserRevisionLine numberNew 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 }