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@9:31c8de7ac6fe, 2017-11-02 (annotated)

Committer:
Miriam
Date:
Thu Nov 02 15:31:35 2017 +0000
Revision:
9:31c8de7ac6fe
Parent:
8:83b2ab57cf0d
Child:
10:37698a6a1ea7
zodra de setpoint een beetje veranderd de motorset blijft groeien!

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
Miriam 9:31c8de7ac6fe 74
Annelotte 0:ec8fa8a84edd 75 }
Annelotte 0:ec8fa8a84edd 76
Annelotte 0:ec8fa8a84edd 77 void SetpointRobot()
Annelotte 0:ec8fa8a84edd 78 {
Miriam 8:83b2ab57cf0d 79 //double Potmeterwaarde2 = potMeter2.read();
Miriam 8:83b2ab57cf0d 80 //double Potmeterwaarde1 = potMeter1.read();
Miriam 8:83b2ab57cf0d 81 bool knop1 = Knopje1;
Miriam 8:83b2ab57cf0d 82 bool knop2 = Knopje2;
Annelotte 0:ec8fa8a84edd 83
Miriam 9:31c8de7ac6fe 84 if (knop1 == false){ //(Potmeterwaarde2>0.6) {
Miriam 9:31c8de7ac6fe 85 SetPx += 0.01; // hoe veel verder gaat hij? 1 cm? 10 cm?
Annelotte 0:ec8fa8a84edd 86 }
Miriam 9:31c8de7ac6fe 87 else if (knop1 == true){ //(Potmeterwaarde2<0.4) {
Miriam 9:31c8de7ac6fe 88 //SetPx -= 0.001;
Annelotte 0:ec8fa8a84edd 89 }
Annelotte 3:120fbef23c17 90 else
Annelotte 3:120fbef23c17 91 {}
Miriam 9:31c8de7ac6fe 92 if (knop2 == false){ //(Potmeterwaarde1>0.6) {
Miriam 9:31c8de7ac6fe 93 SetPy += 0.01;
Annelotte 0:ec8fa8a84edd 94 }
Miriam 9:31c8de7ac6fe 95 else if (knop2 ==true){ //(Potmeterwaarde1<0.4) {
Miriam 9:31c8de7ac6fe 96 //SetPy -= 0.001;
Annelotte 0:ec8fa8a84edd 97 }
Annelotte 3:120fbef23c17 98 else
Annelotte 3:120fbef23c17 99 {}
Annelotte 0:ec8fa8a84edd 100 //pc.printf("Setpointx = %f, Setpointy = %f \r\n", SetPx, SetPy);
Annelotte 0:ec8fa8a84edd 101 }
Annelotte 0:ec8fa8a84edd 102
Annelotte 1:e3db171abbb2 103 /*float GetReferencePosition()
Annelotte 0:ec8fa8a84edd 104 {
Annelotte 0:ec8fa8a84edd 105 float Potmeterwaarde = potMeter2.read();
Annelotte 0:ec8fa8a84edd 106 int maxwaarde = 4096; // = 64x64
Annelotte 0:ec8fa8a84edd 107 float refP = Potmeterwaarde*maxwaarde;
Annelotte 0:ec8fa8a84edd 108 return refP; // value between 0 and 4096
Annelotte 0:ec8fa8a84edd 109 }
Annelotte 0:ec8fa8a84edd 110
Annelotte 0:ec8fa8a84edd 111 float GetReferencePosition2()
Annelotte 0:ec8fa8a84edd 112 {
Annelotte 1:e3db171abbb2 113 float Potmeterwaarde2 = potMeter1.read();
Annelotte 0:ec8fa8a84edd 114 int maxwaarde2 = 4096; // = 64x64
Annelotte 1:e3db171abbb2 115 float refP2 = Potmeterwaarde2*maxwaarde2;
Annelotte 0:ec8fa8a84edd 116 return refP2; // value between 0 and 4096
Annelotte 1:e3db171abbb2 117 }*/
Annelotte 0:ec8fa8a84edd 118
Annelotte 0:ec8fa8a84edd 119 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 120 {
Annelotte 2:9f343567723c 121 float kp = 0.0005; // kind of scaled.
Annelotte 0:ec8fa8a84edd 122 float Proportional= kp*error;
Annelotte 0:ec8fa8a84edd 123
Annelotte 0:ec8fa8a84edd 124 float kd = 0.0004; // kind of scaled.
Annelotte 0:ec8fa8a84edd 125 float VelocityError = (error - e_prev)/Ts;
Annelotte 0:ec8fa8a84edd 126 float Derivative = kd*VelocityError;
Annelotte 0:ec8fa8a84edd 127 e_prev = error;
Annelotte 0:ec8fa8a84edd 128
Annelotte 0:ec8fa8a84edd 129 float ki = 0.00005; // kind of scaled.
Annelotte 0:ec8fa8a84edd 130 e_int = e_int+Ts*error;
Annelotte 0:ec8fa8a84edd 131 float Integrator = ki*e_int;
Annelotte 0:ec8fa8a84edd 132
Annelotte 0:ec8fa8a84edd 133
Annelotte 0:ec8fa8a84edd 134 float motorValue = Proportional + Integrator + Derivative;
Annelotte 0:ec8fa8a84edd 135 return motorValue;
Annelotte 0:ec8fa8a84edd 136 }
Annelotte 0:ec8fa8a84edd 137
Annelotte 0:ec8fa8a84edd 138 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 139 {
Annelotte 2:9f343567723c 140 float kp2 = 0.0005; // kind of scaled.
Annelotte 0:ec8fa8a84edd 141 float Proportional2= kp2*error2;
Annelotte 0:ec8fa8a84edd 142
Annelotte 0:ec8fa8a84edd 143 float kd2 = 0.0004; // kind of scaled.
Annelotte 0:ec8fa8a84edd 144 float VelocityError2 = (error2 - e_prev2)/Ts;
Annelotte 0:ec8fa8a84edd 145 float Derivative2 = kd2*VelocityError2;
Annelotte 0:ec8fa8a84edd 146 e_prev2 = error2;
Annelotte 0:ec8fa8a84edd 147
Annelotte 0:ec8fa8a84edd 148 float ki2 = 0.00005; // kind of scaled.
Annelotte 0:ec8fa8a84edd 149 e_int2 = e_int2+Ts*error2;
Annelotte 0:ec8fa8a84edd 150 float Integrator2 = ki2*e_int2;
Annelotte 0:ec8fa8a84edd 151
Annelotte 0:ec8fa8a84edd 152
Annelotte 0:ec8fa8a84edd 153 float motorValue2 = Proportional2 + Integrator2 + Derivative2;
Annelotte 0:ec8fa8a84edd 154 return motorValue2;
Annelotte 0:ec8fa8a84edd 155 }
Annelotte 0:ec8fa8a84edd 156
Annelotte 0:ec8fa8a84edd 157
Annelotte 0:ec8fa8a84edd 158 void SetMotor1(float motorValue)
Annelotte 0:ec8fa8a84edd 159 {
Annelotte 0:ec8fa8a84edd 160 if (motorValue >= 0)
Annelotte 0:ec8fa8a84edd 161 {
Annelotte 1:e3db171abbb2 162 M1D = 0; //direction ...
Annelotte 0:ec8fa8a84edd 163 }
Annelotte 0:ec8fa8a84edd 164 else
Annelotte 0:ec8fa8a84edd 165 {
Annelotte 1:e3db171abbb2 166 M1D = 1; //direction ...
Annelotte 0:ec8fa8a84edd 167 }
Annelotte 0:ec8fa8a84edd 168
Annelotte 0:ec8fa8a84edd 169 if (fabs(motorValue) > 1)
Annelotte 0:ec8fa8a84edd 170 {
Annelotte 0:ec8fa8a84edd 171 M1E = 1; //de snelheid wordt teruggeschaald naar 8.4 rad/s (maximale snelheid, dus waarde 1)
Annelotte 0:ec8fa8a84edd 172 }
Annelotte 0:ec8fa8a84edd 173 else
Annelotte 0:ec8fa8a84edd 174 {
Annelotte 0:ec8fa8a84edd 175 M1E = fabs(motorValue); //de absolute snelheid wordt bepaald, de motor staat uit bij een waarde 0
Annelotte 0:ec8fa8a84edd 176 }
Annelotte 0:ec8fa8a84edd 177 }
Annelotte 0:ec8fa8a84edd 178
Annelotte 0:ec8fa8a84edd 179 void SetMotor2(float motorValue2)
Annelotte 0:ec8fa8a84edd 180 {
Annelotte 0:ec8fa8a84edd 181 if (motorValue2 >= 0)
Annelotte 0:ec8fa8a84edd 182 {
Miriam 8:83b2ab57cf0d 183 M2D = 1; //PAS OP, DEZE MOET ZO ZIJN!
Annelotte 0:ec8fa8a84edd 184 }
Annelotte 0:ec8fa8a84edd 185 else
Annelotte 0:ec8fa8a84edd 186 {
Miriam 6:9943f69ff668 187 M2D = 0;
Annelotte 0:ec8fa8a84edd 188 }
Annelotte 0:ec8fa8a84edd 189
Annelotte 0:ec8fa8a84edd 190 if (fabs(motorValue2) > 1)
Annelotte 0:ec8fa8a84edd 191 {
Annelotte 0:ec8fa8a84edd 192 M2E = 1; //de snelheid wordt teruggeschaald naar 8.4 rad/s (maximale snelheid, dus waarde 1)
Annelotte 0:ec8fa8a84edd 193 }
Annelotte 0:ec8fa8a84edd 194 else
Annelotte 0:ec8fa8a84edd 195 {
Annelotte 0:ec8fa8a84edd 196 M2E = fabs(motorValue2); //de absolute snelheid wordt bepaald, de motor staat uit bij een waarde 0
Annelotte 0:ec8fa8a84edd 197 }
Annelotte 0:ec8fa8a84edd 198 }
Annelotte 0:ec8fa8a84edd 199
Annelotte 0:ec8fa8a84edd 200 float Encoder ()
Annelotte 0:ec8fa8a84edd 201 {
Annelotte 0:ec8fa8a84edd 202 float Huidigepositie = motor1.getPosition ();
Annelotte 0:ec8fa8a84edd 203 return Huidigepositie; // huidige positie = current position
Annelotte 0:ec8fa8a84edd 204 }
Annelotte 0:ec8fa8a84edd 205
Annelotte 0:ec8fa8a84edd 206 float Encoder2 ()
Annelotte 0:ec8fa8a84edd 207 {
Annelotte 0:ec8fa8a84edd 208 float Huidigepositie2 = motor2.getPosition ();
Annelotte 0:ec8fa8a84edd 209 return Huidigepositie2; // huidige positie = current position
Annelotte 0:ec8fa8a84edd 210 }
Annelotte 0:ec8fa8a84edd 211
Miriam 7:1d3eefa00e20 212 void MeasureAndControl(void)
Miriam 7:1d3eefa00e20 213 {
Miriam 7:1d3eefa00e20 214 // RKI aanroepen
Miriam 7:1d3eefa00e20 215 RKI();
Miriam 7:1d3eefa00e20 216
Miriam 7:1d3eefa00e20 217 // hier the control of the control system
Miriam 7:1d3eefa00e20 218 //float refP = GetReferencePosition();
Miriam 7:1d3eefa00e20 219 float Huidigepositie = Encoder();
Miriam 7:1d3eefa00e20 220 error1 = (Motor1Set - Huidigepositie);// make an error
Miriam 7:1d3eefa00e20 221 float motorValue = FeedBackControl(error1, e_prev, e_int);
Miriam 7:1d3eefa00e20 222 SetMotor1(motorValue);
Miriam 7:1d3eefa00e20 223
Miriam 7:1d3eefa00e20 224 // hier the control of the control system
Miriam 7:1d3eefa00e20 225 //float refP2 = GetReferencePosition2();
Miriam 7:1d3eefa00e20 226 float Huidigepositie2 = Encoder2();
Miriam 7:1d3eefa00e20 227 error2 = (Motor2Set - Huidigepositie2);// make an error
Miriam 7:1d3eefa00e20 228 float motorValue2 = FeedBackControl2(error2, e_prev2, e_int2);
Miriam 7:1d3eefa00e20 229 SetMotor2(motorValue2);
Miriam 7:1d3eefa00e20 230 }
Miriam 7:1d3eefa00e20 231
Annelotte 4:836d7f9ac0ca 232 void Autodemo_or_demo()
Annelotte 4:836d7f9ac0ca 233 {
Annelotte 4:836d7f9ac0ca 234 if (autodemo_done == 0)
Annelotte 4:836d7f9ac0ca 235 {
Miriam 5:9651c3f7602b 236 SetPx = 0.38 + 0.15;
Miriam 5:9651c3f7602b 237 SetPy = 0.30;
Annelotte 4:836d7f9ac0ca 238 MeasureAndControl ();
Miriam 5:9651c3f7602b 239 SetPx = 0.38;
Miriam 5:9651c3f7602b 240 SetPy = 0.30;
Annelotte 4:836d7f9ac0ca 241 MeasureAndControl ();
Miriam 5:9651c3f7602b 242 SetPx = 0.38;
Miriam 5:9651c3f7602b 243 SetPy = 0.30 - 0.30;
Annelotte 4:836d7f9ac0ca 244 MeasureAndControl ();
Miriam 5:9651c3f7602b 245 SetPx = 0.38;
Miriam 5:9651c3f7602b 246 SetPy = 0.30;
Annelotte 4:836d7f9ac0ca 247 MeasureAndControl ();
Annelotte 4:836d7f9ac0ca 248 autodemo_done = true;
Annelotte 4:836d7f9ac0ca 249 }
Annelotte 4:836d7f9ac0ca 250
Annelotte 4:836d7f9ac0ca 251 else if (autodemo_done == 1)
Annelotte 4:836d7f9ac0ca 252 {
Annelotte 4:836d7f9ac0ca 253 SetpointRobot();
Annelotte 4:836d7f9ac0ca 254 MeasureAndControl ();
Annelotte 4:836d7f9ac0ca 255 }
Annelotte 4:836d7f9ac0ca 256
Annelotte 4:836d7f9ac0ca 257 }
Annelotte 0:ec8fa8a84edd 258
Annelotte 0:ec8fa8a84edd 259
Annelotte 0:ec8fa8a84edd 260 int main()
Annelotte 0:ec8fa8a84edd 261 {
Annelotte 0:ec8fa8a84edd 262 M1E.period(PwmPeriod);
Annelotte 4:836d7f9ac0ca 263 Treecko.attach(&Autodemo_or_demo, Ts); //Elke 1 seconde zorgt de ticker voor het runnen en uitlezen van de verschillende
Annelotte 0:ec8fa8a84edd 264 //functies en analoge signalen. Veranderingen worden elke 1 seconde doorgevoerd.
Annelotte 1:e3db171abbb2 265 pc.baud(115200);
Annelotte 0:ec8fa8a84edd 266
Annelotte 0:ec8fa8a84edd 267
Annelotte 0:ec8fa8a84edd 268 while(1)
Annelotte 0:ec8fa8a84edd 269 {
Annelotte 1:e3db171abbb2 270 //wait(0.2);
Annelotte 0:ec8fa8a84edd 271 float B = motor1.getPosition();
Miriam 9:31c8de7ac6fe 272 pc.printf("Setpointx = %f, Setpointy = %f, Motor1Set = %f, Motor2Set = %f, error1 = %f, error2 = %f \r\n", SetPx, SetPy, Motor1Set, Motor2Set,error1, error2);
Annelotte 0:ec8fa8a84edd 273 //float positie = B%4096;
Annelotte 0:ec8fa8a84edd 274 //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 275 //pc.printf("q1 = %f, q2 = %f, Motor1Set = %f, Motor2Set = %f \r\n", q1, q2, Motor1Set, Motor2Set);
Annelotte 0:ec8fa8a84edd 276 }
Annelotte 0:ec8fa8a84edd 277 }