Projectgroep 20 Biorobotics / Mbed 2 deprecated DEMO_en_autodemo

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   Encoder MODSERIAL mbed

Fork of DEMO by Annelotte Bex

main.cpp@8:83b2ab57cf0d, 2017-11-02 (annotated)

Committer:
Miriam
Date:
Thu Nov 02 15:22:38 2017 +0000
Revision:
8:83b2ab57cf0d
Parent:
7:1d3eefa00e20
Child:
9:31c8de7ac6fe
even uitproberen met knopjes enzo en het pogramma een beetje opruimen. het ziet er allemaal niet zo netjes uit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Annelotte 0:ec8fa8a84edd 1 //libaries
Annelotte 0:ec8fa8a84edd 2 #include "mbed.h"
Annelotte 0:ec8fa8a84edd 3 #include "encoder.h"
Annelotte 0:ec8fa8a84edd 4 #include "MODSERIAL.h"
Annelotte 0:ec8fa8a84edd 5
Miriam 8:83b2ab57cf0d 6 // globale variables
Miriam 8:83b2ab57cf0d 7 Ticker Treecko; //We make a awesome ticker for our control system
Miriam 8:83b2ab57cf0d 8 const float Ts = 0.1; // tickettijd/ sample time
Miriam 8:83b2ab57cf0d 9 MODSERIAL pc(USBTX,USBRX);
Miriam 8:83b2ab57cf0d 10 float PwmPeriod = 1.0/5000.0; //set up of PWM periode (5000 Hz, want 5000 periodes in 1 seconde)
Miriam 8:83b2ab57cf0d 11 AnalogIn potMeter2(A1); //Analoge input of potmeter 2 (will be use for te reference position)
Miriam 8:83b2ab57cf0d 12 AnalogIn potMeter1(A2);
Miriam 8:83b2ab57cf0d 13 DigitalIn Knopje1 (PTA4); //tijdelijk
Miriam 8:83b2ab57cf0d 14 DigitalIn Knopje2 (PTC6); //tijdelijk
Miriam 8:83b2ab57cf0d 15 bool autodemo_done = 1; //automatische demo stand =0
Annelotte 0:ec8fa8a84edd 16
Miriam 8:83b2ab57cf0d 17 //eerste motor
Annelotte 0:ec8fa8a84edd 18 PwmOut M1E(D6); //Biorobotics Motor 1 PWM control of the speed
Annelotte 0:ec8fa8a84edd 19 DigitalOut M1D(D7); //Biorobotics Motor 1 diraction control
Miriam 8:83b2ab57cf0d 20 Encoder motor1(D13,D12,true);
Annelotte 0:ec8fa8a84edd 21
Annelotte 0:ec8fa8a84edd 22 float e_prev = 0;
Annelotte 0:ec8fa8a84edd 23 float e_int = 0;
Miriam 7:1d3eefa00e20 24 float error1;
Miriam 8:83b2ab57cf0d 25
Miriam 8:83b2ab57cf0d 26 //tweede motor
Miriam 8:83b2ab57cf0d 27 PwmOut M2E(D5);
Miriam 8:83b2ab57cf0d 28 DigitalOut M2D(D4);
Miriam 8:83b2ab57cf0d 29 Encoder motor2(D9,D8,true);
Miriam 8:83b2ab57cf0d 30
Miriam 8:83b2ab57cf0d 31
Annelotte 0:ec8fa8a84edd 32 float e_prev2 = 0;
Annelotte 0:ec8fa8a84edd 33 float e_int2 = 0;
Miriam 7:1d3eefa00e20 34 float error2;
Annelotte 0:ec8fa8a84edd 35
Miriam 8:83b2ab57cf0d 36 //RKI
Annelotte 0:ec8fa8a84edd 37 double pi = 3.14159265359;
Annelotte 4:836d7f9ac0ca 38 double SetPx = 0.38; //Setpoint position x-coordinate from changePosition (EMG dependent)
Annelotte 4:836d7f9ac0ca 39 double SetPy = 0.30; //Setpoint position y-coordinate from changePosition (EMG dependent)
Miriam 8:83b2ab57cf0d 40 double q1 = 0; //Reference position q1 from calibration (only the first time)
Miriam 8:83b2ab57cf0d 41 double q2 = (pi/2); //Reference position q2 from calibration (only the first time): LET OP! DE MOTOR
Annelotte 4:836d7f9ac0ca 42 const double L1 = 0.30; //Length arm 1
Annelotte 4:836d7f9ac0ca 43 const double L2 = 0.38; //Length arm 2
Miriam 8:83b2ab57cf0d 44 double K = 1; //Spring constant for movement end-joint to setpoint
Miriam 8:83b2ab57cf0d 45 double B1 = 1; //Friction coefficient for motor 1
Miriam 8:83b2ab57cf0d 46 double B2 = 1; //Friction coefficient for motot 2
Miriam 8:83b2ab57cf0d 47 double T = 0.1; //Desired time step
Miriam 8:83b2ab57cf0d 48 double Motor1Set; //Motor1 angle
Miriam 8:83b2ab57cf0d 49 double Motor2Set; //Motor2 angle
Annelotte 0:ec8fa8a84edd 50 double p;
Annelotte 1:e3db171abbb2 51 double pp;
Annelotte 1:e3db171abbb2 52 double bb;
Annelotte 1:e3db171abbb2 53 double cc;
Annelotte 1:e3db171abbb2 54 double a;
Annelotte 1:e3db171abbb2 55 double aa;
Annelotte 0:ec8fa8a84edd 56
Annelotte 4:836d7f9ac0ca 57
Annelotte 0:ec8fa8a84edd 58 void RKI()
Annelotte 0:ec8fa8a84edd 59 {
Annelotte 1:e3db171abbb2 60 p=sin(q1)*L1;
Annelotte 1:e3db171abbb2 61 pp=sin(q2)*L2;
Annelotte 1:e3db171abbb2 62 a=cos(q1)*L1;
Annelotte 1:e3db171abbb2 63 aa=cos(q2)*L2;
Annelotte 1:e3db171abbb2 64 bb=SetPy;
Annelotte 1:e3db171abbb2 65 cc=SetPx;
Annelotte 2:9f343567723c 66 q1 = q1 + ((p + pp)*bb - (a + aa)*cc)*(K*T)/B1; //Calculate desired joint 1 position
Annelotte 2:9f343567723c 67 q2 = q2 + ((bb - a)*pp + (p - cc)*aa)*(K*T)/B2; //Calculate desired joint 2 position
Annelotte 0:ec8fa8a84edd 68
Annelotte 2:9f343567723c 69 int maxwaarde = 4096; // = 64x64
Annelotte 2:9f343567723c 70
Annelotte 2:9f343567723c 71
Miriam 7:1d3eefa00e20 72 Motor1Set = -(q1/(2*pi))*maxwaarde; //Calculate the desired motor1 angle from the desired joint positions
Miriam 7:1d3eefa00e20 73 Motor2Set = ((pi-q2-q1)/(2*pi))*maxwaarde; //Calculate the desired motor2 angle from the desired joint positions
Annelotte 0:ec8fa8a84edd 74
Miriam 7:1d3eefa00e20 75 //pc.printf("waarde p = %f, waarde pp = %f, a= %f, aa = %f, bb = %f, cc = %f \r\n",p,pp,a,aa,bb,cc);
Annelotte 0:ec8fa8a84edd 76 //pc.printf("q1 = %f, q2 = %f, Motor1Set = %f, Motor2Set = %f \r\n", q1, q2, Motor1Set, Motor2Set);
Annelotte 0:ec8fa8a84edd 77 }
Annelotte 0:ec8fa8a84edd 78
Annelotte 0:ec8fa8a84edd 79 void SetpointRobot()
Annelotte 0:ec8fa8a84edd 80 {
Miriam 8:83b2ab57cf0d 81 //double Potmeterwaarde2 = potMeter2.read();
Miriam 8:83b2ab57cf0d 82 //double Potmeterwaarde1 = potMeter1.read();
Miriam 8:83b2ab57cf0d 83 bool knop1 = Knopje1;
Miriam 8:83b2ab57cf0d 84 bool knop2 = Knopje2;
Annelotte 0:ec8fa8a84edd 85
Annelotte 2:9f343567723c 86 if (Potmeterwaarde2>0.6) {
Annelotte 4:836d7f9ac0ca 87 SetPx += 0.001; // hoe veel verder gaat hij? 1 cm? 10 cm?
Annelotte 0:ec8fa8a84edd 88 }
Annelotte 3:120fbef23c17 89 else if (Potmeterwaarde2<0.4) {
Annelotte 4:836d7f9ac0ca 90 SetPx -= 0.001;
Annelotte 0:ec8fa8a84edd 91 }
Annelotte 3:120fbef23c17 92 else
Annelotte 3:120fbef23c17 93 {}
Annelotte 2:9f343567723c 94 if (Potmeterwaarde1>0.6) {
Annelotte 4:836d7f9ac0ca 95 SetPy += 0.001;
Annelotte 0:ec8fa8a84edd 96 }
Annelotte 3:120fbef23c17 97 else if (Potmeterwaarde1<0.4) {
Annelotte 4:836d7f9ac0ca 98 SetPy -= 0.001;
Annelotte 0:ec8fa8a84edd 99 }
Annelotte 3:120fbef23c17 100 else
Annelotte 3:120fbef23c17 101 {}
Annelotte 0:ec8fa8a84edd 102 //pc.printf("Setpointx = %f, Setpointy = %f \r\n", SetPx, SetPy);
Annelotte 0:ec8fa8a84edd 103 }
Annelotte 0:ec8fa8a84edd 104
Annelotte 1:e3db171abbb2 105 /*float GetReferencePosition()
Annelotte 0:ec8fa8a84edd 106 {
Annelotte 0:ec8fa8a84edd 107 float Potmeterwaarde = potMeter2.read();
Annelotte 0:ec8fa8a84edd 108 int maxwaarde = 4096; // = 64x64
Annelotte 0:ec8fa8a84edd 109 float refP = Potmeterwaarde*maxwaarde;
Annelotte 0:ec8fa8a84edd 110 return refP; // value between 0 and 4096
Annelotte 0:ec8fa8a84edd 111 }
Annelotte 0:ec8fa8a84edd 112
Annelotte 0:ec8fa8a84edd 113 float GetReferencePosition2()
Annelotte 0:ec8fa8a84edd 114 {
Annelotte 1:e3db171abbb2 115 float Potmeterwaarde2 = potMeter1.read();
Annelotte 0:ec8fa8a84edd 116 int maxwaarde2 = 4096; // = 64x64
Annelotte 1:e3db171abbb2 117 float refP2 = Potmeterwaarde2*maxwaarde2;
Annelotte 0:ec8fa8a84edd 118 return refP2; // value between 0 and 4096
Annelotte 1:e3db171abbb2 119 }*/
Annelotte 0:ec8fa8a84edd 120
Annelotte 0:ec8fa8a84edd 121 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)
Annelotte 0:ec8fa8a84edd 122 {
Annelotte 2:9f343567723c 123 float kp = 0.0005; // kind of scaled.
Annelotte 0:ec8fa8a84edd 124 float Proportional= kp*error;
Annelotte 0:ec8fa8a84edd 125
Annelotte 0:ec8fa8a84edd 126 float kd = 0.0004; // kind of scaled.
Annelotte 0:ec8fa8a84edd 127 float VelocityError = (error - e_prev)/Ts;
Annelotte 0:ec8fa8a84edd 128 float Derivative = kd*VelocityError;
Annelotte 0:ec8fa8a84edd 129 e_prev = error;
Annelotte 0:ec8fa8a84edd 130
Annelotte 0:ec8fa8a84edd 131 float ki = 0.00005; // kind of scaled.
Annelotte 0:ec8fa8a84edd 132 e_int = e_int+Ts*error;
Annelotte 0:ec8fa8a84edd 133 float Integrator = ki*e_int;
Annelotte 0:ec8fa8a84edd 134
Annelotte 0:ec8fa8a84edd 135
Annelotte 0:ec8fa8a84edd 136 float motorValue = Proportional + Integrator + Derivative;
Annelotte 0:ec8fa8a84edd 137 return motorValue;
Annelotte 0:ec8fa8a84edd 138 }
Annelotte 0:ec8fa8a84edd 139
Annelotte 0:ec8fa8a84edd 140 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)
Annelotte 0:ec8fa8a84edd 141 {
Annelotte 2:9f343567723c 142 float kp2 = 0.0005; // kind of scaled.
Annelotte 0:ec8fa8a84edd 143 float Proportional2= kp2*error2;
Annelotte 0:ec8fa8a84edd 144
Annelotte 0:ec8fa8a84edd 145 float kd2 = 0.0004; // kind of scaled.
Annelotte 0:ec8fa8a84edd 146 float VelocityError2 = (error2 - e_prev2)/Ts;
Annelotte 0:ec8fa8a84edd 147 float Derivative2 = kd2*VelocityError2;
Annelotte 0:ec8fa8a84edd 148 e_prev2 = error2;
Annelotte 0:ec8fa8a84edd 149
Annelotte 0:ec8fa8a84edd 150 float ki2 = 0.00005; // kind of scaled.
Annelotte 0:ec8fa8a84edd 151 e_int2 = e_int2+Ts*error2;
Annelotte 0:ec8fa8a84edd 152 float Integrator2 = ki2*e_int2;
Annelotte 0:ec8fa8a84edd 153
Annelotte 0:ec8fa8a84edd 154
Annelotte 0:ec8fa8a84edd 155 float motorValue2 = Proportional2 + Integrator2 + Derivative2;
Annelotte 0:ec8fa8a84edd 156 return motorValue2;
Annelotte 0:ec8fa8a84edd 157 }
Annelotte 0:ec8fa8a84edd 158
Annelotte 0:ec8fa8a84edd 159
Annelotte 0:ec8fa8a84edd 160 void SetMotor1(float motorValue)
Annelotte 0:ec8fa8a84edd 161 {
Annelotte 0:ec8fa8a84edd 162 if (motorValue >= 0)
Annelotte 0:ec8fa8a84edd 163 {
Annelotte 1:e3db171abbb2 164 M1D = 0; //direction ...
Annelotte 0:ec8fa8a84edd 165 }
Annelotte 0:ec8fa8a84edd 166 else
Annelotte 0:ec8fa8a84edd 167 {
Annelotte 1:e3db171abbb2 168 M1D = 1; //direction ...
Annelotte 0:ec8fa8a84edd 169 }
Annelotte 0:ec8fa8a84edd 170
Annelotte 0:ec8fa8a84edd 171 if (fabs(motorValue) > 1)
Annelotte 0:ec8fa8a84edd 172 {
Annelotte 0:ec8fa8a84edd 173 M1E = 1; //de snelheid wordt teruggeschaald naar 8.4 rad/s (maximale snelheid, dus waarde 1)
Annelotte 0:ec8fa8a84edd 174 }
Annelotte 0:ec8fa8a84edd 175 else
Annelotte 0:ec8fa8a84edd 176 {
Annelotte 0:ec8fa8a84edd 177 M1E = fabs(motorValue); //de absolute snelheid wordt bepaald, de motor staat uit bij een waarde 0
Annelotte 0:ec8fa8a84edd 178 }
Annelotte 0:ec8fa8a84edd 179 }
Annelotte 0:ec8fa8a84edd 180
Annelotte 0:ec8fa8a84edd 181 void SetMotor2(float motorValue2)
Annelotte 0:ec8fa8a84edd 182 {
Annelotte 0:ec8fa8a84edd 183 if (motorValue2 >= 0)
Annelotte 0:ec8fa8a84edd 184 {
Miriam 8:83b2ab57cf0d 185 M2D = 1; //PAS OP, DEZE MOET ZO ZIJN!
Annelotte 0:ec8fa8a84edd 186 }
Annelotte 0:ec8fa8a84edd 187 else
Annelotte 0:ec8fa8a84edd 188 {
Miriam 6:9943f69ff668 189 M2D = 0;
Annelotte 0:ec8fa8a84edd 190 }
Annelotte 0:ec8fa8a84edd 191
Annelotte 0:ec8fa8a84edd 192 if (fabs(motorValue2) > 1)
Annelotte 0:ec8fa8a84edd 193 {
Annelotte 0:ec8fa8a84edd 194 M2E = 1; //de snelheid wordt teruggeschaald naar 8.4 rad/s (maximale snelheid, dus waarde 1)
Annelotte 0:ec8fa8a84edd 195 }
Annelotte 0:ec8fa8a84edd 196 else
Annelotte 0:ec8fa8a84edd 197 {
Annelotte 0:ec8fa8a84edd 198 M2E = fabs(motorValue2); //de absolute snelheid wordt bepaald, de motor staat uit bij een waarde 0
Annelotte 0:ec8fa8a84edd 199 }
Annelotte 0:ec8fa8a84edd 200 }
Annelotte 0:ec8fa8a84edd 201
Annelotte 0:ec8fa8a84edd 202 float Encoder ()
Annelotte 0:ec8fa8a84edd 203 {
Annelotte 0:ec8fa8a84edd 204 float Huidigepositie = motor1.getPosition ();
Annelotte 0:ec8fa8a84edd 205 return Huidigepositie; // huidige positie = current position
Annelotte 0:ec8fa8a84edd 206 }
Annelotte 0:ec8fa8a84edd 207
Annelotte 0:ec8fa8a84edd 208 float Encoder2 ()
Annelotte 0:ec8fa8a84edd 209 {
Annelotte 0:ec8fa8a84edd 210 float Huidigepositie2 = motor2.getPosition ();
Annelotte 0:ec8fa8a84edd 211 return Huidigepositie2; // huidige positie = current position
Annelotte 0:ec8fa8a84edd 212 }
Annelotte 0:ec8fa8a84edd 213
Miriam 7:1d3eefa00e20 214 void MeasureAndControl(void)
Miriam 7:1d3eefa00e20 215 {
Miriam 7:1d3eefa00e20 216 // RKI aanroepen
Miriam 7:1d3eefa00e20 217 RKI();
Miriam 7:1d3eefa00e20 218
Miriam 7:1d3eefa00e20 219 // hier the control of the control system
Miriam 7:1d3eefa00e20 220 //float refP = GetReferencePosition();
Miriam 7:1d3eefa00e20 221 float Huidigepositie = Encoder();
Miriam 7:1d3eefa00e20 222 error1 = (Motor1Set - Huidigepositie);// make an error
Miriam 7:1d3eefa00e20 223 float motorValue = FeedBackControl(error1, e_prev, e_int);
Miriam 7:1d3eefa00e20 224 SetMotor1(motorValue);
Miriam 7:1d3eefa00e20 225
Miriam 7:1d3eefa00e20 226 // hier the control of the control system
Miriam 7:1d3eefa00e20 227 //float refP2 = GetReferencePosition2();
Miriam 7:1d3eefa00e20 228 float Huidigepositie2 = Encoder2();
Miriam 7:1d3eefa00e20 229 error2 = (Motor2Set - Huidigepositie2);// make an error
Miriam 7:1d3eefa00e20 230 float motorValue2 = FeedBackControl2(error2, e_prev2, e_int2);
Miriam 7:1d3eefa00e20 231 SetMotor2(motorValue2);
Miriam 7:1d3eefa00e20 232 }
Miriam 7:1d3eefa00e20 233
Annelotte 4:836d7f9ac0ca 234 void Autodemo_or_demo()
Annelotte 4:836d7f9ac0ca 235 {
Annelotte 4:836d7f9ac0ca 236 if (autodemo_done == 0)
Annelotte 4:836d7f9ac0ca 237 {
Miriam 5:9651c3f7602b 238 SetPx = 0.38 + 0.15;
Miriam 5:9651c3f7602b 239 SetPy = 0.30;
Annelotte 4:836d7f9ac0ca 240 MeasureAndControl ();
Miriam 5:9651c3f7602b 241 SetPx = 0.38;
Miriam 5:9651c3f7602b 242 SetPy = 0.30;
Annelotte 4:836d7f9ac0ca 243 MeasureAndControl ();
Miriam 5:9651c3f7602b 244 SetPx = 0.38;
Miriam 5:9651c3f7602b 245 SetPy = 0.30 - 0.30;
Annelotte 4:836d7f9ac0ca 246 MeasureAndControl ();
Miriam 5:9651c3f7602b 247 SetPx = 0.38;
Miriam 5:9651c3f7602b 248 SetPy = 0.30;
Annelotte 4:836d7f9ac0ca 249 MeasureAndControl ();
Annelotte 4:836d7f9ac0ca 250 autodemo_done = true;
Annelotte 4:836d7f9ac0ca 251 }
Annelotte 4:836d7f9ac0ca 252
Annelotte 4:836d7f9ac0ca 253 else if (autodemo_done == 1)
Annelotte 4:836d7f9ac0ca 254 {
Annelotte 4:836d7f9ac0ca 255 SetpointRobot();
Annelotte 4:836d7f9ac0ca 256 MeasureAndControl ();
Annelotte 4:836d7f9ac0ca 257 }
Annelotte 4:836d7f9ac0ca 258
Annelotte 4:836d7f9ac0ca 259 }
Annelotte 0:ec8fa8a84edd 260
Annelotte 0:ec8fa8a84edd 261
Annelotte 0:ec8fa8a84edd 262 int main()
Annelotte 0:ec8fa8a84edd 263 {
Annelotte 0:ec8fa8a84edd 264 M1E.period(PwmPeriod);
Annelotte 4:836d7f9ac0ca 265 Treecko.attach(&Autodemo_or_demo, Ts); //Elke 1 seconde zorgt de ticker voor het runnen en uitlezen van de verschillende
Annelotte 0:ec8fa8a84edd 266 //functies en analoge signalen. Veranderingen worden elke 1 seconde doorgevoerd.
Annelotte 1:e3db171abbb2 267 pc.baud(115200);
Annelotte 0:ec8fa8a84edd 268
Annelotte 0:ec8fa8a84edd 269
Annelotte 0:ec8fa8a84edd 270 while(1)
Annelotte 0:ec8fa8a84edd 271 {
Annelotte 1:e3db171abbb2 272 //wait(0.2);
Annelotte 0:ec8fa8a84edd 273 float B = motor1.getPosition();
Miriam 7:1d3eefa00e20 274 pc.printf("Setpointx = %f, Setpointy = %f, Motor1Set = %f, Motor2Set = %f \r\n, error1 = %f, error2 = %f", SetPx, SetPy, Motor1Set, Motor2Set,error1, error2);
Annelotte 0:ec8fa8a84edd 275 //float positie = B%4096;
Annelotte 0:ec8fa8a84edd 276 //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
Annelotte 1:e3db171abbb2 277 //pc.printf("q1 = %f, q2 = %f, Motor1Set = %f, Motor2Set = %f \r\n", q1, q2, Motor1Set, Motor2Set);
Annelotte 0:ec8fa8a84edd 278 }
Annelotte 0:ec8fa8a84edd 279 }