Projectgroep 20 Biorobotics / Mbed 2 deprecated week6ordenenscript

P-controller geordend

Dependencies:   Encoder HIDScope MODSERIAL mbed

main.cpp@18:b42a884bca02, 2017-11-03 (annotated)

Committer:
Annelotte
Date:
Fri Nov 03 03:13:41 2017 +0000
Revision:
18:b42a884bca02
Parent:
17:1246d6b0c5d0 
Met demo modus

; hoop ik

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
Miriam 0:2a99f692f683 15
Annelotte 15:a5849f3a60fc 16 double pi = 3.14159265359;
Annelotte 15:a5849f3a60fc 17 double q1 = (pi/2); //Reference position hoek 1 in radiance
Annelotte 15:a5849f3a60fc 18 double q2 = -(pi/2); //Reference position hoek 2 in radiance
Annelotte 15:a5849f3a60fc 19 const double L1 = 0.30; //Length arm 1 in mm
Annelotte 15:a5849f3a60fc 20 const double L2 = 0.38; //Length arm 2 in mm
Annelotte 15:a5849f3a60fc 21 double B1 = 1; //Friction constant motor 1
Annelotte 15:a5849f3a60fc 22 double B2 = 1; //Friction constant motor 2
Annelotte 15:a5849f3a60fc 23 double K = 1; //Spring constant movement from end-effector position to setpoint position
Annelotte 15:a5849f3a60fc 24 double Tijd = 1; //Timestep value
Annelotte 15:a5849f3a60fc 25 double Rsx = 0.38; //Reference x-component of the setpoint radius
Annelotte 15:a5849f3a60fc 26 double Rsy = 0.30; //Reference y-component of the setpoint radius
Annelotte 17:1246d6b0c5d0 27 double refP = 0; //Reference position motor 1
Annelotte 17:1246d6b0c5d0 28 double refP2 = 0.5*pi; //Reference position motor 2
Annelotte 15:a5849f3a60fc 29 double Rex = cos(q1)*L1 - sin(q2)*L2; //The x-component of the end-effector radius
Annelotte 15:a5849f3a60fc 30 double Rey = sin(q1)*L1 + cos(q2)*L2; //The y-component of the end-effector radius
Annelotte 15:a5849f3a60fc 31 double R1x = 0; //The x-component of the joint 1 radius
Annelotte 15:a5849f3a60fc 32 double R1y = 0; //The y-component of the joint 1 radius
Annelotte 15:a5849f3a60fc 33 double R2x = cos(q1)*L1; //The x-component of the joint 2 radius
Annelotte 15:a5849f3a60fc 34 double R2y = sin(q1)*L1; //The y-component of the joint 1 radius
Annelotte 15:a5849f3a60fc 35 double Fx = 0;
Annelotte 15:a5849f3a60fc 36 double Fy = 0;
Annelotte 15:a5849f3a60fc 37 double Tor1 = 0;
Annelotte 15:a5849f3a60fc 38 double Tor2 = 0;
Annelotte 16:0a0b1c3be4d0 39 double w1= 0;
Annelotte 16:0a0b1c3be4d0 40 double w2= 0;
Annelotte 18:b42a884bca02 41 bool automode = false;
Annelotte 15:a5849f3a60fc 42
Miriam 0:2a99f692f683 43 Encoder motor1(D13,D12,true);
Miriam 0:2a99f692f683 44 MODSERIAL pc(USBTX,USBRX);
Miriam 0:2a99f692f683 45
Miriam 3:f755b4d41aa8 46 float PwmPeriod = 1.0/5000.0; //set up of PWM periode (5000 Hz, want 5000 periodes in 1 seconde)
Gerber 14:5534b8282a06 47 const float Ts = 0.1; // tickettijd/ sample time
Miriam 2:b504e35af662 48 float e_prev = 0;
Miriam 2:b504e35af662 49 float e_int = 0;
Miriam 0:2a99f692f683 50
paulineoonk 7:05495acc08b0 51 //tweede motor
Annelotte 17:1246d6b0c5d0 52 AnalogIn potMeter1(A2);
paulineoonk 7:05495acc08b0 53 PwmOut M2E(D5);
paulineoonk 7:05495acc08b0 54 DigitalOut M2D(D4);
paulineoonk 7:05495acc08b0 55 Encoder motor2(D9,D8,true);
paulineoonk 7:05495acc08b0 56 Ticker DubbelTreecko;
paulineoonk 7:05495acc08b0 57
paulineoonk 12:e125b9fa77b9 58 //motors
paulineoonk 12:e125b9fa77b9 59 //float Huidigepositie2;
paulineoonk 12:e125b9fa77b9 60 //float Huidigepositie;
paulineoonk 12:e125b9fa77b9 61
paulineoonk 7:05495acc08b0 62 float PwmPeriod2 = 1.0/5000.0; //set up of PWM periode (5000 Hz, want 5000 periodes in 1 seconde)
paulineoonk 7:05495acc08b0 63 float e_prev2 = 0;
paulineoonk 7:05495acc08b0 64 float e_int2 = 0;
paulineoonk 7:05495acc08b0 65
Annelotte 15:a5849f3a60fc 66 void RKI()
Miriam 0:2a99f692f683 67 {
Annelotte 15:a5849f3a60fc 68 Rex = cos(q1)*L1 - sin(q2)*L2;
Annelotte 15:a5849f3a60fc 69 Rey = sin(q1)*L1 + cos(q2)*L2;
Annelotte 15:a5849f3a60fc 70 R2x = cos(q1)*L1;
Annelotte 15:a5849f3a60fc 71 R2y = sin(q1)*L1;
Annelotte 15:a5849f3a60fc 72 Fx = (Rsx-Rex)*K;
Annelotte 15:a5849f3a60fc 73 Fy = (Rsy-Rey)*K;
Annelotte 15:a5849f3a60fc 74 Tor1 = (Rex-R1x)*Fy + (R1y-Rey)*Fx;
Annelotte 15:a5849f3a60fc 75 Tor2 = (Rex-R2x)*Fy + (R2y-Rey)*Fx;
Annelotte 15:a5849f3a60fc 76 w1 = Tor1/B1;
Annelotte 15:a5849f3a60fc 77 w2 = Tor2/B2;
Annelotte 15:a5849f3a60fc 78 q1 = q1 + w1*Tijd;
Annelotte 15:a5849f3a60fc 79 q2 = q2 + w2*Tijd;
Annelotte 15:a5849f3a60fc 80
Miriam 0:2a99f692f683 81 int maxwaarde = 4096; // = 64x64
Annelotte 15:a5849f3a60fc 82 refP = (((0.5*pi) - q1)/(2*pi))*maxwaarde;
Annelotte 15:a5849f3a60fc 83 refP2 = (((0.5*pi) + q1 - q2)/(2*pi))*maxwaarde; //Get reference positions
Miriam 0:2a99f692f683 84 }
paulineoonk 7:05495acc08b0 85
Annelotte 15:a5849f3a60fc 86 void SetpointRobot()
Annelotte 18:b42a884bca02 87 {
Annelotte 18:b42a884bca02 88 if automode == 0
Annelotte 18:b42a884bca02 89 {
Annelotte 18:b42a884bca02 90 for (int n = 0; n < 100; n++)
Annelotte 18:b42a884bca02 91 {
Annelotte 18:b42a884bca02 92 Rsy -= 0.002;
Annelotte 18:b42a884bca02 93 RKI();
Annelotte 18:b42a884bca02 94 // hier the control of the control system
Annelotte 18:b42a884bca02 95 float Huidigepositie = Encoder();
Annelotte 18:b42a884bca02 96 float error = (refP - Huidigepositie);// make an error
Annelotte 18:b42a884bca02 97 float motorValue = FeedBackControl(error, e_prev, e_int);
Annelotte 18:b42a884bca02 98 SetMotor1(motorValue);
Annelotte 18:b42a884bca02 99
Annelotte 18:b42a884bca02 100 float Huidigepositie2 = Encoder2();
Annelotte 18:b42a884bca02 101 float error2 = (refP2 - Huidigepositie2);// make an error
Annelotte 18:b42a884bca02 102 float motorValue2 = FeedBackControl2(error2, e_prev2, e_int2);
Annelotte 18:b42a884bca02 103 SetMotor2(motorValue2);
Annelotte 18:b42a884bca02 104 //pc.printf("encoder 2 = %f\r\n",Huidigepositie2);
Annelotte 18:b42a884bca02 105 pc.printf("refP2 = %f, Huidigepositie2 = %f, error = %f, motorValue2 = %f \r\n", refP2, Huidigepositie2, error2, motorValue2);
Annelotte 18:b42a884bca02 106 }
Annelotte 18:b42a884bca02 107
Annelotte 18:b42a884bca02 108 for (int n = 0; n < 100; n++)
Annelotte 18:b42a884bca02 109 {
Annelotte 18:b42a884bca02 110 Rsy += 0.002;
Annelotte 18:b42a884bca02 111 RKI();
Annelotte 18:b42a884bca02 112 // hier the control of the control system
Annelotte 18:b42a884bca02 113 float Huidigepositie = Encoder();
Annelotte 18:b42a884bca02 114 float error = (refP - Huidigepositie);// make an error
Annelotte 18:b42a884bca02 115 float motorValue = FeedBackControl(error, e_prev, e_int);
Annelotte 18:b42a884bca02 116 SetMotor1(motorValue);
Annelotte 18:b42a884bca02 117
Annelotte 18:b42a884bca02 118 float Huidigepositie2 = Encoder2();
Annelotte 18:b42a884bca02 119 float error2 = (refP2 - Huidigepositie2);// make an error
Annelotte 18:b42a884bca02 120 float motorValue2 = FeedBackControl2(error2, e_prev2, e_int2);
Annelotte 18:b42a884bca02 121 SetMotor2(motorValue2);
Annelotte 18:b42a884bca02 122 //pc.printf("encoder 2 = %f\r\n",Huidigepositie2);
Annelotte 18:b42a884bca02 123 pc.printf("refP2 = %f, Huidigepositie2 = %f, error = %f, motorValue2 = %f \r\n", refP2, Huidigepositie2, error2, motorValue2);
Annelotte 18:b42a884bca02 124 }
Annelotte 18:b42a884bca02 125
Annelotte 18:b42a884bca02 126 for (int n = 0; n < 100; n++)
Annelotte 18:b42a884bca02 127 {
Annelotte 18:b42a884bca02 128 Rsx += 0.002;
Annelotte 18:b42a884bca02 129 RKI();
Annelotte 18:b42a884bca02 130 // hier the control of the control system
Annelotte 18:b42a884bca02 131 float Huidigepositie = Encoder();
Annelotte 18:b42a884bca02 132 float error = (refP - Huidigepositie);// make an error
Annelotte 18:b42a884bca02 133 float motorValue = FeedBackControl(error, e_prev, e_int);
Annelotte 18:b42a884bca02 134 SetMotor1(motorValue);
Annelotte 18:b42a884bca02 135
Annelotte 18:b42a884bca02 136 float Huidigepositie2 = Encoder2();
Annelotte 18:b42a884bca02 137 float error2 = (refP2 - Huidigepositie2);// make an error
Annelotte 18:b42a884bca02 138 float motorValue2 = FeedBackControl2(error2, e_prev2, e_int2);
Annelotte 18:b42a884bca02 139 SetMotor2(motorValue2);
Annelotte 18:b42a884bca02 140 //pc.printf("encoder 2 = %f\r\n",Huidigepositie2);
Annelotte 18:b42a884bca02 141 pc.printf("refP2 = %f, Huidigepositie2 = %f, error = %f, motorValue2 = %f \r\n", refP2, Huidigepositie2, error2, motorValue2);
Annelotte 18:b42a884bca02 142 }
Annelotte 17:1246d6b0c5d0 143
Annelotte 18:b42a884bca02 144 for (int n = 0; n < 100; n++)
Annelotte 18:b42a884bca02 145 {
Annelotte 18:b42a884bca02 146 Rsx -= 0.002;
Annelotte 18:b42a884bca02 147 RKI()
Annelotte 18:b42a884bca02 148 // hier the control of the control system
Annelotte 18:b42a884bca02 149 float Huidigepositie = Encoder();
Annelotte 18:b42a884bca02 150 float error = (refP - Huidigepositie);// make an error
Annelotte 18:b42a884bca02 151 float motorValue = FeedBackControl(error, e_prev, e_int);
Annelotte 18:b42a884bca02 152 SetMotor1(motorValue);
Annelotte 18:b42a884bca02 153
Annelotte 18:b42a884bca02 154 float Huidigepositie2 = Encoder2();
Annelotte 18:b42a884bca02 155 float error2 = (refP2 - Huidigepositie2);// make an error
Annelotte 18:b42a884bca02 156 float motorValue2 = FeedBackControl2(error2, e_prev2, e_int2);
Annelotte 18:b42a884bca02 157 SetMotor2(motorValue2);
Annelotte 18:b42a884bca02 158 //pc.printf("encoder 2 = %f\r\n",Huidigepositie2);
Annelotte 18:b42a884bca02 159 pc.printf("refP2 = %f, Huidigepositie2 = %f, error = %f, motorValue2 = %f \r\n", refP2, Huidigepositie2, error2, motorValue2);
Annelotte 18:b42a884bca02 160 }
Annelotte 18:b42a884bca02 161
Annelotte 18:b42a884bca02 162 automode = 1;
Annelotte 15:a5849f3a60fc 163 }
Annelotte 18:b42a884bca02 164
Annelotte 18:b42a884bca02 165 else automode = 1
Annelotte 18:b42a884bca02 166 {
Annelotte 18:b42a884bca02 167 double Potmeterwaarde2 = potMeter2.read();
Annelotte 18:b42a884bca02 168 double Potmeterwaarde1 = potMeter1.read();
Annelotte 18:b42a884bca02 169
Annelotte 18:b42a884bca02 170 if (Potmeterwaarde2>0.6) {
Annelotte 18:b42a884bca02 171 Rsx += 0.001; //het gaat telkens 1 mm verder wanneer de potmeter boven de 0.6 staat
Annelotte 18:b42a884bca02 172 }
Annelotte 18:b42a884bca02 173 else if (Potmeterwaarde2<0.4) {
Annelotte 18:b42a884bca02 174 Rsx -= 0.001; //het gaat telkens 1 mm terug wanneer de potmeter onder de 0.4 staat
Annelotte 18:b42a884bca02 175 }
Annelotte 18:b42a884bca02 176 else { //de x-waarde van de setpoint verandert niet
Annelotte 15:a5849f3a60fc 177 }
Annelotte 15:a5849f3a60fc 178
Annelotte 18:b42a884bca02 179 if (Potmeterwaarde1>0.6) { //het gaat telkens 1 mm verder wanneer de potmeter boven de 0.6 staat
Annelotte 18:b42a884bca02 180 Rsy += 0.001;
Annelotte 18:b42a884bca02 181 }
Annelotte 18:b42a884bca02 182 else if (Potmeterwaarde1<0.4) { //het gaat telkens 1 mm terug wanneer de potmeter onder de 0.4
Annelotte 18:b42a884bca02 183 Rsy -= 0.001;
Annelotte 18:b42a884bca02 184 }
Annelotte 18:b42a884bca02 185 else { //de y-waarde van de setpoint verandert niet
Annelotte 18:b42a884bca02 186 }
Annelotte 15:a5849f3a60fc 187 }
paulineoonk 7:05495acc08b0 188 }
Miriam 0:2a99f692f683 189
Miriam 2:b504e35af662 190 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 191 {
paulineoonk 12:e125b9fa77b9 192 float kp = 0.001; // kind of scaled.
Miriam 2:b504e35af662 193 float Proportional= kp*error;
Miriam 2:b504e35af662 194
Miriam 6:083bd713670b 195 float kd = 0.0004; // kind of scaled.
Miriam 2:b504e35af662 196 float VelocityError = (error - e_prev)/Ts;
Miriam 2:b504e35af662 197 float Derivative = kd*VelocityError;
Miriam 2:b504e35af662 198 e_prev = error;
Miriam 2:b504e35af662 199
Gerber 14:5534b8282a06 200 float ki = 0.0005; // kind of scaled.
Miriam 2:b504e35af662 201 e_int = e_int+Ts*error;
Miriam 2:b504e35af662 202 float Integrator = ki*e_int;
Miriam 2:b504e35af662 203
Miriam 2:b504e35af662 204
Miriam 2:b504e35af662 205 float motorValue = Proportional + Integrator + Derivative;
Miriam 0:2a99f692f683 206 return motorValue;
Miriam 0:2a99f692f683 207 }
Miriam 0:2a99f692f683 208
paulineoonk 7:05495acc08b0 209 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 210 {
Gerber 13:eaaeb41e22d2 211 float kp2 = 0.001; // kind of scaled.
paulineoonk 7:05495acc08b0 212 float Proportional2= kp2*error2;
paulineoonk 7:05495acc08b0 213
Gerber 13:eaaeb41e22d2 214 float kd2 = 0.001; // kind of scaled.
paulineoonk 7:05495acc08b0 215 float VelocityError2 = (error2 - e_prev2)/Ts;
paulineoonk 7:05495acc08b0 216 float Derivative2 = kd2*VelocityError2;
paulineoonk 7:05495acc08b0 217 e_prev2 = error2;
paulineoonk 7:05495acc08b0 218
Gerber 14:5534b8282a06 219 float ki2 = 0.005; // kind of scaled.
paulineoonk 7:05495acc08b0 220 e_int2 = e_int2+Ts*error2;
paulineoonk 7:05495acc08b0 221 float Integrator2 = ki2*e_int2;
paulineoonk 7:05495acc08b0 222
paulineoonk 7:05495acc08b0 223
paulineoonk 7:05495acc08b0 224 float motorValue2 = Proportional2 + Integrator2 + Derivative2;
paulineoonk 7:05495acc08b0 225 return motorValue2;
paulineoonk 7:05495acc08b0 226 }
paulineoonk 7:05495acc08b0 227
paulineoonk 7:05495acc08b0 228
Miriam 0:2a99f692f683 229 void SetMotor1(float motorValue)
Miriam 0:2a99f692f683 230 {
Miriam 1:609671b1c96c 231 if (motorValue >= 0)
Miriam 0:2a99f692f683 232 {
Miriam 0:2a99f692f683 233 M1D = 0;
Miriam 0:2a99f692f683 234 }
Miriam 0:2a99f692f683 235 else
Miriam 0:2a99f692f683 236 {
Miriam 0:2a99f692f683 237 M1D = 1;
Miriam 0:2a99f692f683 238 }
Miriam 0:2a99f692f683 239
Miriam 0:2a99f692f683 240 if (fabs(motorValue) > 1)
Miriam 0:2a99f692f683 241 {
Miriam 3:f755b4d41aa8 242 M1E = 1; //de snelheid wordt teruggeschaald naar 8.4 rad/s (maximale snelheid, dus waarde 1)
Miriam 0:2a99f692f683 243 }
Miriam 0:2a99f692f683 244 else
Miriam 0:2a99f692f683 245 {
Miriam 0:2a99f692f683 246 M1E = fabs(motorValue); //de absolute snelheid wordt bepaald, de motor staat uit bij een waarde 0
Miriam 0:2a99f692f683 247 }
Miriam 0:2a99f692f683 248 }
Miriam 0:2a99f692f683 249
paulineoonk 7:05495acc08b0 250 void SetMotor2(float motorValue2)
paulineoonk 7:05495acc08b0 251 {
paulineoonk 7:05495acc08b0 252 if (motorValue2 >= 0)
paulineoonk 7:05495acc08b0 253 {
paulineoonk 12:e125b9fa77b9 254 M2D = 1;
paulineoonk 7:05495acc08b0 255 }
paulineoonk 7:05495acc08b0 256 else
paulineoonk 7:05495acc08b0 257 {
paulineoonk 12:e125b9fa77b9 258 M2D =0;
paulineoonk 7:05495acc08b0 259 }
paulineoonk 7:05495acc08b0 260
paulineoonk 7:05495acc08b0 261 if (fabs(motorValue2) > 1)
paulineoonk 7:05495acc08b0 262 {
paulineoonk 7:05495acc08b0 263 M2E = 1; //de snelheid wordt teruggeschaald naar 8.4 rad/s (maximale snelheid, dus waarde 1)
paulineoonk 7:05495acc08b0 264 }
paulineoonk 7:05495acc08b0 265 else
paulineoonk 7:05495acc08b0 266 {
paulineoonk 7:05495acc08b0 267 M2E = fabs(motorValue2); //de absolute snelheid wordt bepaald, de motor staat uit bij een waarde 0
paulineoonk 7:05495acc08b0 268 }
paulineoonk 7:05495acc08b0 269 }
paulineoonk 7:05495acc08b0 270
Miriam 0:2a99f692f683 271 float Encoder ()
Miriam 0:2a99f692f683 272 {
Miriam 0:2a99f692f683 273 float Huidigepositie = motor1.getPosition ();
Miriam 3:f755b4d41aa8 274 return Huidigepositie; // huidige positie = current position
Miriam 0:2a99f692f683 275 }
Miriam 0:2a99f692f683 276
paulineoonk 7:05495acc08b0 277 float Encoder2 ()
paulineoonk 7:05495acc08b0 278 {
paulineoonk 7:05495acc08b0 279 float Huidigepositie2 = motor2.getPosition ();
paulineoonk 7:05495acc08b0 280 return Huidigepositie2; // huidige positie = current position
paulineoonk 7:05495acc08b0 281 }
paulineoonk 7:05495acc08b0 282
Miriam 0:2a99f692f683 283 void MeasureAndControl(void)
Miriam 0:2a99f692f683 284 {
Annelotte 15:a5849f3a60fc 285 SetpointRobot();
Annelotte 15:a5849f3a60fc 286 // RKI aanroepen
Annelotte 15:a5849f3a60fc 287 RKI();
Annelotte 15:a5849f3a60fc 288
Miriam 3:f755b4d41aa8 289 // hier the control of the control system
Miriam 0:2a99f692f683 290 float Huidigepositie = Encoder();
Miriam 3:f755b4d41aa8 291 float error = (refP - Huidigepositie);// make an error
Miriam 2:b504e35af662 292 float motorValue = FeedBackControl(error, e_prev, e_int);
Miriam 0:2a99f692f683 293 SetMotor1(motorValue);
Miriam 0:2a99f692f683 294
paulineoonk 7:05495acc08b0 295 float Huidigepositie2 = Encoder2();
paulineoonk 7:05495acc08b0 296 float error2 = (refP2 - Huidigepositie2);// make an error
paulineoonk 7:05495acc08b0 297 float motorValue2 = FeedBackControl2(error2, e_prev2, e_int2);
paulineoonk 7:05495acc08b0 298 SetMotor2(motorValue2);
paulineoonk 12:e125b9fa77b9 299 //pc.printf("encoder 2 = %f\r\n",Huidigepositie2);
Gerber 13:eaaeb41e22d2 300 pc.printf("refP2 = %f, Huidigepositie2 = %f, error = %f, motorValue2 = %f \r\n", refP2, Huidigepositie2, error2, motorValue2);
paulineoonk 7:05495acc08b0 301 }
paulineoonk 7:05495acc08b0 302
Miriam 0:2a99f692f683 303 int main()
Miriam 0:2a99f692f683 304 {
Miriam 5:987cc578988e 305 M1E.period(PwmPeriod);
Miriam 4:c119259c1ba5 306 Treecko.attach(MeasureAndControl, Ts); //Elke 1 seconde zorgt de ticker voor het runnen en uitlezen van de verschillende
Miriam 0:2a99f692f683 307 //functies en analoge signalen. Veranderingen worden elke 1 seconde doorgevoerd.
Annelotte 15:a5849f3a60fc 308 pc.baud(115200);
Miriam 4:c119259c1ba5 309
Miriam 0:2a99f692f683 310 while(1)
Miriam 0:2a99f692f683 311 {
Miriam 0:2a99f692f683 312 wait(0.2);
paulineoonk 12:e125b9fa77b9 313 // pc.printf(" encoder 1 %f, encoder 2 %f\r\n",Huidigepositie,Huidigepositie2);
paulineoonk 12:e125b9fa77b9 314
paulineoonk 12:e125b9fa77b9 315 //float B = motor1.getPosition();
paulineoonk 12:e125b9fa77b9 316 //float Potmeterwaarde = potMeter2.read();
Miriam 0:2a99f692f683 317 //float positie = B%4096;
paulineoonk 12:e125b9fa77b9 318 // pc.printf("pos: %f, \r\n pos2 = %f",motor1.getPosition(),motor2.getPosition); //potmeter uitlezen. tussen 0-1. voltage, dus *3.3V
Miriam 0:2a99f692f683 319 }
Miriam 0:2a99f692f683 320 }