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@3:120fbef23c17, 2017-11-02 (annotated)

Committer:
Annelotte
Date:
Thu Nov 02 10:00:46 2017 +0000
Revision:
3:120fbef23c17
Parent:
2:9f343567723c
Child:
4:836d7f9ac0ca
else statements veranderd

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