Jitse Giesen / Mbed 2 deprecated Koppeling

Hier Roodmans

Dependencies:   mbed

main.cpp@5:aa191ca932ab, 2017-10-24 (annotated)

Committer:
Jitse_Giesen
Date:
Tue Oct 24 07:45:59 2017 +0000
Revision:
5:aa191ca932ab
Parent:
4:050dd85ffcf5 
rood fix deze shit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Jitse_Giesen 0:3e98322ac0e2 1 #include "mbed.h"
Jitse_Giesen 0:3e98322ac0e2 2 #include "math.h"
Jitse_Giesen 4:050dd85ffcf5 3 #include "encoder.h"
Jitse_Giesen 4:050dd85ffcf5 4 #include "QEI.h"
Jitse_Giesen 0:3e98322ac0e2 5
Jitse_Giesen 4:050dd85ffcf5 6 DigitalIn button1(D8);
Jitse_Giesen 4:050dd85ffcf5 7 DigitalIn button2(D9);
Jitse_Giesen 4:050dd85ffcf5 8 //AnalogIn potMeterIn(A1);
Jitse_Giesen 4:050dd85ffcf5 9 float potMeterIn; // = 0.01; snelheid in, zonder potmeter gebruik
Jitse_Giesen 4:050dd85ffcf5 10
Jitse_Giesen 4:050dd85ffcf5 11 DigitalOut motor1DirectionPin(D4);
Jitse_Giesen 4:050dd85ffcf5 12 PwmOut motor1MagnitudePin(D5);
Jitse_Giesen 4:050dd85ffcf5 13
Jitse_Giesen 4:050dd85ffcf5 14 QEI Encoder(D12,D13,NC,32);
Jitse_Giesen 4:050dd85ffcf5 15
Jitse_Giesen 4:050dd85ffcf5 16
Jitse_Giesen 4:050dd85ffcf5 17 int counts = Encoder.getPulses();
Jitse_Giesen 4:050dd85ffcf5 18 int delta;
Jitse_Giesen 0:3e98322ac0e2 19 DigitalOut led(LED_RED);
Jitse_Giesen 0:3e98322ac0e2 20 DigitalOut ledd(LED_GREEN);
Jitse_Giesen 2:263a53d72bb7 21 DigitalOut leddd(LED_BLUE);
Jitse_Giesen 0:3e98322ac0e2 22 InterruptIn button(SW2);
Jitse_Giesen 0:3e98322ac0e2 23 Timer t;
Jitse_Giesen 0:3e98322ac0e2 24 Serial pc(USBTX, USBRX);
Jitse_Giesen 5:aa191ca932ab 25
Jitse_Giesen 5:aa191ca932ab 26 int Xin=-5;
Jitse_Giesen 5:aa191ca932ab 27 /*float huidigetijdX;
Jitse_Giesen 0:3e98322ac0e2 28 void ledtX(){
Jitse_Giesen 0:3e98322ac0e2 29 t.reset();
Jitse_Giesen 0:3e98322ac0e2 30 Xin++;
Jitse_Giesen 0:3e98322ac0e2 31 pc.printf("Xin is %i\n",Xin);
Jitse_Giesen 0:3e98322ac0e2 32 ledd=0;
Jitse_Giesen 0:3e98322ac0e2 33 led=1;
Jitse_Giesen 0:3e98322ac0e2 34 wait(0.5);
Jitse_Giesen 0:3e98322ac0e2 35 ledd=1;
Jitse_Giesen 0:3e98322ac0e2 36 led=0;
Jitse_Giesen 0:3e98322ac0e2 37 }
Jitse_Giesen 0:3e98322ac0e2 38
Jitse_Giesen 0:3e98322ac0e2 39 int tellerX(){
Jitse_Giesen 0:3e98322ac0e2 40 ledd=1;
Jitse_Giesen 2:263a53d72bb7 41 leddd=1;
Jitse_Giesen 5:aa191ca932ab 42 while(true){
Jitse_Giesen 0:3e98322ac0e2 43 button.fall(ledtX);
Jitse_Giesen 0:3e98322ac0e2 44 /*if (EMG>threshold){
Jitse_Giesen 5:aa191ca932ab 45 ledtX(); // dit is wat je uiteindelijk wil dat er staat
Jitse_Giesen 5:aa191ca932ab 46 }*//*
Jitse_Giesen 0:3e98322ac0e2 47 t.start();
Jitse_Giesen 0:3e98322ac0e2 48 huidigetijdX=t.read();
Jitse_Giesen 3:08bf605844eb 49 if (huidigetijdX>2){
Jitse_Giesen 0:3e98322ac0e2 50 led=1; // ga door naar het volgende programma
Jitse_Giesen 2:263a53d72bb7 51 return 0;
Jitse_Giesen 0:3e98322ac0e2 52 }
Jitse_Giesen 0:3e98322ac0e2 53 }
Jitse_Giesen 0:3e98322ac0e2 54 }
Jitse_Giesen 5:aa191ca932ab 55 */int Yin=-3;/*
Jitse_Giesen 0:3e98322ac0e2 56 float huidigetijdY;
Jitse_Giesen 0:3e98322ac0e2 57 void ledtY(){
Jitse_Giesen 0:3e98322ac0e2 58 t.reset();
Jitse_Giesen 0:3e98322ac0e2 59 Yin++;
Jitse_Giesen 0:3e98322ac0e2 60 pc.printf("Yin is %i\n",Yin);
Jitse_Giesen 0:3e98322ac0e2 61 ledd=0;
Jitse_Giesen 2:263a53d72bb7 62 leddd=1;
Jitse_Giesen 0:3e98322ac0e2 63 wait(0.5);
Jitse_Giesen 0:3e98322ac0e2 64 ledd=1;
Jitse_Giesen 2:263a53d72bb7 65 leddd=0;
Jitse_Giesen 0:3e98322ac0e2 66 }
Jitse_Giesen 0:3e98322ac0e2 67
Jitse_Giesen 0:3e98322ac0e2 68 int tellerY(){
Jitse_Giesen 0:3e98322ac0e2 69 t.reset();
Jitse_Giesen 2:263a53d72bb7 70 ledd=1;
Jitse_Giesen 2:263a53d72bb7 71 leddd=0;
Jitse_Giesen 0:3e98322ac0e2 72 while(true){
Jitse_Giesen 0:3e98322ac0e2 73 button.fall(ledtY);
Jitse_Giesen 0:3e98322ac0e2 74 /*if (EMG>threshold){
Jitse_Giesen 0:3e98322ac0e2 75 Piek(); // dit is wat je uiteindelijk wil dat er staat
Jitse_Giesen 5:aa191ca932ab 76 }*//*
Jitse_Giesen 0:3e98322ac0e2 77 t.start();
Jitse_Giesen 0:3e98322ac0e2 78 huidigetijdY=t.read();
Jitse_Giesen 3:08bf605844eb 79 if (huidigetijdY>2){
Jitse_Giesen 2:263a53d72bb7 80 leddd=1;
Jitse_Giesen 2:263a53d72bb7 81 button.fall(0);
Jitse_Giesen 3:08bf605844eb 82 return 0; // ga door naar het volgende programma
Jitse_Giesen 0:3e98322ac0e2 83 }
Jitse_Giesen 0:3e98322ac0e2 84 }
Jitse_Giesen 5:aa191ca932ab 85 }*/
Jitse_Giesen 2:263a53d72bb7 86 int B;
Jitse_Giesen 0:3e98322ac0e2 87 float Pox = 0;
Jitse_Giesen 0:3e98322ac0e2 88 float Poy = 0;
Jitse_Giesen 0:3e98322ac0e2 89 float Pbx = 0;
Jitse_Giesen 0:3e98322ac0e2 90 float Pby = 887;
Jitse_Giesen 0:3e98322ac0e2 91 float Prx = 768;
Jitse_Giesen 0:3e98322ac0e2 92 float Pry = 443;
Jitse_Giesen 0:3e98322ac0e2 93 float Pex=121;
Jitse_Giesen 0:3e98322ac0e2 94 float Pey=308;
Jitse_Giesen 0:3e98322ac0e2 95 float diamtrklosje=20;
Jitse_Giesen 0:3e98322ac0e2 96 float pi=3.14159265359;
Jitse_Giesen 2:263a53d72bb7 97 float omtrekklosje=diamtrklosje*pi;
Jitse_Giesen 0:3e98322ac0e2 98 float Lou;
Jitse_Giesen 0:3e98322ac0e2 99 float Lbu;
Jitse_Giesen 0:3e98322ac0e2 100 float Lru;
Jitse_Giesen 2:263a53d72bb7 101 float dLod;
Jitse_Giesen 2:263a53d72bb7 102 float dLbd;
Jitse_Giesen 2:263a53d72bb7 103 float dLrd;
Jitse_Giesen 0:3e98322ac0e2 104 float roto;
Jitse_Giesen 0:3e98322ac0e2 105 float rotb;
Jitse_Giesen 0:3e98322ac0e2 106 float rotr;
Jitse_Giesen 2:263a53d72bb7 107 float rotzo;
Jitse_Giesen 2:263a53d72bb7 108 float rotzb;
Jitse_Giesen 2:263a53d72bb7 109 float rotzr;
Jitse_Giesen 0:3e98322ac0e2 110 float counto;
Jitse_Giesen 0:3e98322ac0e2 111 float countb;
Jitse_Giesen 2:263a53d72bb7 112 float countr;
Jitse_Giesen 2:263a53d72bb7 113 float countzo;
Jitse_Giesen 2:263a53d72bb7 114 float countzb;
Jitse_Giesen 2:263a53d72bb7 115 float countzr;
Jitse_Giesen 0:3e98322ac0e2 116 float Psx;
Jitse_Giesen 0:3e98322ac0e2 117 float Psy;
Jitse_Giesen 0:3e98322ac0e2 118 float Vex;
Jitse_Giesen 0:3e98322ac0e2 119 float Vey;
Jitse_Giesen 4:050dd85ffcf5 120 float Kz=0.7; // nadersnelheid instellen
Jitse_Giesen 0:3e98322ac0e2 121 float modVe;
Jitse_Giesen 0:3e98322ac0e2 122 float Vmax=20;
Jitse_Giesen 0:3e98322ac0e2 123 float Pstx;
Jitse_Giesen 0:3e98322ac0e2 124 float Psty;
Jitse_Giesen 0:3e98322ac0e2 125 float T=0.02;//seconds
Jitse_Giesen 0:3e98322ac0e2 126
Jitse_Giesen 4:050dd85ffcf5 127
Jitse_Giesen 4:050dd85ffcf5 128 //Deel om motor(en) aan te sturen--------------------------------------------
Jitse_Giesen 4:050dd85ffcf5 129
Jitse_Giesen 4:050dd85ffcf5 130 void calcdelta(void) {
Jitse_Giesen 4:050dd85ffcf5 131 delta = (counto - Encoder.getPulses());
Jitse_Giesen 4:050dd85ffcf5 132 }
Jitse_Giesen 4:050dd85ffcf5 133
Jitse_Giesen 4:050dd85ffcf5 134 //Encoder motor1(D12,D13); //Gear ratio = encoder ratio = 131.25:1 &
Jitse_Giesen 4:050dd85ffcf5 135 // factor 64: To compute the counts per revolution of the gearbox output, multiply the gear ratio by 64.
Jitse_Giesen 4:050dd85ffcf5 136
Jitse_Giesen 4:050dd85ffcf5 137 float referenceVelocity;
Jitse_Giesen 4:050dd85ffcf5 138 float motorValue;
Jitse_Giesen 4:050dd85ffcf5 139
Jitse_Giesen 4:050dd85ffcf5 140 Ticker controlmotor1; // één ticker van maken?
Jitse_Giesen 4:050dd85ffcf5 141 Ticker calculatedelta;
Jitse_Giesen 4:050dd85ffcf5 142 Ticker feedbacktick;
Jitse_Giesen 4:050dd85ffcf5 143 Ticker printdata; //aparte ticker om print pc aan te kunnen spreken zonder get te worden van hoeveelheid geprinte waardes
Jitse_Giesen 4:050dd85ffcf5 144
Jitse_Giesen 4:050dd85ffcf5 145 float GetReferenceVelocity()
Jitse_Giesen 4:050dd85ffcf5 146 {
Jitse_Giesen 4:050dd85ffcf5 147 // Returns reference velocity in rad/s.
Jitse_Giesen 4:050dd85ffcf5 148 // Positive value means clockwise rotation.
Jitse_Giesen 5:aa191ca932ab 149 float maxVelocity=Vex*25+Vey*25; // max 8.4 in rad/s of course!
Jitse_Giesen 5:aa191ca932ab 150 referenceVelocity = (-1)*potMeterIn * maxVelocity;
Jitse_Giesen 5:aa191ca932ab 151
Jitse_Giesen 5:aa191ca932ab 152 if (Encoder.getPulses() < (counto+1))
Jitse_Giesen 5:aa191ca932ab 153 { potMeterIn = 0.1;
Jitse_Giesen 4:050dd85ffcf5 154 }
Jitse_Giesen 5:aa191ca932ab 155 else if (Encoder.getPulses() > (counto-1))
Jitse_Giesen 5:aa191ca932ab 156 { potMeterIn = -0.1;
Jitse_Giesen 4:050dd85ffcf5 157 }
Jitse_Giesen 4:050dd85ffcf5 158 else
Jitse_Giesen 4:050dd85ffcf5 159 { potMeterIn = 0;
Jitse_Giesen 4:050dd85ffcf5 160 }
Jitse_Giesen 4:050dd85ffcf5 161
Jitse_Giesen 4:050dd85ffcf5 162 return referenceVelocity;
Jitse_Giesen 4:050dd85ffcf5 163 }
Jitse_Giesen 4:050dd85ffcf5 164
Jitse_Giesen 4:050dd85ffcf5 165
Jitse_Giesen 4:050dd85ffcf5 166 void SetMotor1(float motorValue)
Jitse_Giesen 4:050dd85ffcf5 167 {
Jitse_Giesen 4:050dd85ffcf5 168 // Given -1<=motorValue<=1, this sets the PWM and direction
Jitse_Giesen 4:050dd85ffcf5 169 // bits for motor 1. Positive value makes motor rotating
Jitse_Giesen 4:050dd85ffcf5 170 // clockwise. motorValues outside range are truncated to
Jitse_Giesen 4:050dd85ffcf5 171 // within range
Jitse_Giesen 4:050dd85ffcf5 172 if (motorValue >=0) motor1DirectionPin=1;
Jitse_Giesen 4:050dd85ffcf5 173 else motor1DirectionPin=0;
Jitse_Giesen 4:050dd85ffcf5 174 if (fabs(motorValue)>1) motor1MagnitudePin = 1;
Jitse_Giesen 4:050dd85ffcf5 175 else motor1MagnitudePin = fabs(motorValue);
Jitse_Giesen 4:050dd85ffcf5 176
Jitse_Giesen 4:050dd85ffcf5 177 }
Jitse_Giesen 4:050dd85ffcf5 178
Jitse_Giesen 4:050dd85ffcf5 179 float FeedForwardControl(float referenceVelocity)
Jitse_Giesen 4:050dd85ffcf5 180 {
Jitse_Giesen 4:050dd85ffcf5 181 // very simple linear feed-forward control
Jitse_Giesen 4:050dd85ffcf5 182 const float MotorGain=8.4; // unit: (rad/s) / PWM, max 8.4
Jitse_Giesen 4:050dd85ffcf5 183 float motorValue = referenceVelocity / MotorGain;
Jitse_Giesen 4:050dd85ffcf5 184 return motorValue;
Jitse_Giesen 4:050dd85ffcf5 185 }
Jitse_Giesen 4:050dd85ffcf5 186
Jitse_Giesen 5:aa191ca932ab 187 void MeasureAndControl()
Jitse_Giesen 4:050dd85ffcf5 188 {
Jitse_Giesen 4:050dd85ffcf5 189 // This function measures the potmeter position, extracts a
Jitse_Giesen 4:050dd85ffcf5 190 // reference velocity from it, and controls the motor with
Jitse_Giesen 4:050dd85ffcf5 191 // a simple FeedForward controller. Call this from a Ticker.
Jitse_Giesen 4:050dd85ffcf5 192 float referenceVelocity = GetReferenceVelocity();
Jitse_Giesen 4:050dd85ffcf5 193 float motorValue = FeedForwardControl(referenceVelocity);
Jitse_Giesen 4:050dd85ffcf5 194 SetMotor1(motorValue);
Jitse_Giesen 4:050dd85ffcf5 195 }
Jitse_Giesen 4:050dd85ffcf5 196
Jitse_Giesen 5:aa191ca932ab 197 void readdata()
Jitse_Giesen 4:050dd85ffcf5 198 { //pc.printf("CurrentState = %i \r\n",Encoder.getCurrentState());
Jitse_Giesen 4:050dd85ffcf5 199 pc.printf("Pulses = %i \r\n",Encoder.getPulses());
Jitse_Giesen 4:050dd85ffcf5 200 //pc.printf("Revolutions = %i \r\n",Encoder.getRevolutions());
Jitse_Giesen 4:050dd85ffcf5 201 pc.printf("Delta = %i \r\n",delta);
Jitse_Giesen 4:050dd85ffcf5 202 }
Jitse_Giesen 4:050dd85ffcf5 203
Jitse_Giesen 4:050dd85ffcf5 204 // einde deel motor------------------------------------------------------------------------------------
Jitse_Giesen 4:050dd85ffcf5 205
Jitse_Giesen 0:3e98322ac0e2 206 Ticker loop;
Jitse_Giesen 0:3e98322ac0e2 207 float touwlengtes(){
Jitse_Giesen 0:3e98322ac0e2 208 Lou=sqrt(pow((Pstx-Pox),2)+pow((Psty-Poy),2)); // rekent touwlengtes uit die nodig zijn voor de nieuwe positie aan de hand van de ingegeven coördinaten en de posities van de palen
Jitse_Giesen 0:3e98322ac0e2 209 Lbu=sqrt(pow((Pstx-Pox),2)+pow((Psty-Pby),2));
Jitse_Giesen 0:3e98322ac0e2 210 Lru=sqrt(pow((Pstx-Prx),2)+pow((Psty-Pry),2));
Jitse_Giesen 0:3e98322ac0e2 211 return 0;
Jitse_Giesen 0:3e98322ac0e2 212 }
Jitse_Giesen 0:3e98322ac0e2 213 float turns(){
Jitse_Giesen 2:263a53d72bb7 214
Jitse_Giesen 0:3e98322ac0e2 215 roto=Lou/omtrekklosje;
Jitse_Giesen 0:3e98322ac0e2 216 rotb=Lbu/omtrekklosje;
Jitse_Giesen 0:3e98322ac0e2 217 rotr=Lru/omtrekklosje;
Jitse_Giesen 4:050dd85ffcf5 218 counto=roto*4200;
Jitse_Giesen 4:050dd85ffcf5 219 //counto = (int)(counto + 0.5); // omzetten van rotaties naar counts
Jitse_Giesen 0:3e98322ac0e2 220 countb=rotb*4200;
Jitse_Giesen 4:050dd85ffcf5 221 //countb = (int)(countb + 0.5);
Jitse_Giesen 0:3e98322ac0e2 222 countr=rotr*4200;
Jitse_Giesen 4:050dd85ffcf5 223 //countr = (int)(countr + 0.5);
Jitse_Giesen 0:3e98322ac0e2 224 return 0;
Jitse_Giesen 0:3e98322ac0e2 225 }
Jitse_Giesen 0:3e98322ac0e2 226
Jitse_Giesen 0:3e98322ac0e2 227
Jitse_Giesen 0:3e98322ac0e2 228 float Pst(){
Jitse_Giesen 0:3e98322ac0e2 229 Pstx=Pex+Vex*T;
Jitse_Giesen 0:3e98322ac0e2 230 Psty=Pey+Vey*T;
Jitse_Giesen 0:3e98322ac0e2 231 touwlengtes();
Jitse_Giesen 5:aa191ca932ab 232 Pex=Pstx;
Jitse_Giesen 0:3e98322ac0e2 233 Pey=Psty;
Jitse_Giesen 0:3e98322ac0e2 234 //pc.printf("een stappie verder\n\r x=%.2f\n\r y=%.2f\n\r",Pstx,Psty);
Jitse_Giesen 0:3e98322ac0e2 235 //pc.printf("met lengtes:\n\r Lo=%.2f Lb=%.2f Lr=%.2f\n\r",Lou,Lbu,Lru);
Jitse_Giesen 0:3e98322ac0e2 236 turns();
Jitse_Giesen 0:3e98322ac0e2 237 //pc.printf("rotatie per motor:\n\r o=%.2f b=%.2f r=%.2f\n\r",roto,rotb,rotr);
Jitse_Giesen 0:3e98322ac0e2 238 pc.printf("counts per motor:\n\r o=%.2f b=%.2f r=%.2f\n\r",counto,countb,countr);
Jitse_Giesen 0:3e98322ac0e2 239 /*float R;
Jitse_Giesen 0:3e98322ac0e2 240 R=Vex/Vey; // met dit stukje kan je zien dat de verhouding tussen Vex en Vey constand is en de end efector dus een rechte lijn maakt
Jitse_Giesen 0:3e98322ac0e2 241 pc.printf("\n\r R=%f",R);*/
Jitse_Giesen 0:3e98322ac0e2 242 return 0;
Jitse_Giesen 0:3e98322ac0e2 243 }
Jitse_Giesen 2:263a53d72bb7 244
Jitse_Giesen 0:3e98322ac0e2 245 float Ps(){
Jitse_Giesen 0:3e98322ac0e2 246 Psx=(Xin)*30+121;
Jitse_Giesen 0:3e98322ac0e2 247 Psy=(Yin)*30+308;
Jitse_Giesen 0:3e98322ac0e2 248 pc.printf("x=%.2f \n\r y=%.2f \n\r",Psx,Psy);
Jitse_Giesen 0:3e98322ac0e2 249 return 0;
Jitse_Giesen 0:3e98322ac0e2 250 }
Jitse_Giesen 0:3e98322ac0e2 251 void Ve(){
Jitse_Giesen 0:3e98322ac0e2 252 Vex=Kz*(Psx-Pex);
Jitse_Giesen 0:3e98322ac0e2 253 Vey=Kz*(Psy-Pey);
Jitse_Giesen 0:3e98322ac0e2 254 modVe=sqrt(pow(Vex,2)+pow(Vey,2));
Jitse_Giesen 0:3e98322ac0e2 255 if(modVe>Vmax){
Jitse_Giesen 0:3e98322ac0e2 256 Vex=(Vex/modVe)*Vmax;
Jitse_Giesen 0:3e98322ac0e2 257 Vey=(Vey/modVe)*Vmax;
Jitse_Giesen 0:3e98322ac0e2 258 }
Jitse_Giesen 0:3e98322ac0e2 259 Pst();
Jitse_Giesen 0:3e98322ac0e2 260 pc.printf("Vex=%.2f \r\n Vey=%.2f \r\n",Vex,Vey);
Jitse_Giesen 0:3e98322ac0e2 261 if((abs(Vex)<0.01f)&&(abs(Vey)<0.01f)){
Jitse_Giesen 2:263a53d72bb7 262 B=5;
Jitse_Giesen 2:263a53d72bb7 263 loop.detach();
Jitse_Giesen 2:263a53d72bb7 264 }
Jitse_Giesen 0:3e98322ac0e2 265 }
Jitse_Giesen 0:3e98322ac0e2 266
Jitse_Giesen 0:3e98322ac0e2 267 int calculator(){
Jitse_Giesen 0:3e98322ac0e2 268 Ps();
Jitse_Giesen 0:3e98322ac0e2 269 loop.attach(&Ve,0.02);
Jitse_Giesen 0:3e98322ac0e2 270 return 0;
Jitse_Giesen 0:3e98322ac0e2 271 }
Jitse_Giesen 0:3e98322ac0e2 272
Jitse_Giesen 2:263a53d72bb7 273 void zakker(){
Jitse_Giesen 2:263a53d72bb7 274 while(1){
Jitse_Giesen 2:263a53d72bb7 275 wait(1);
Jitse_Giesen 5:aa191ca932ab 276 if(B==5){ //misschien moet je hier als voorwaarden een delta is 1 zetten // hierdoor wacht dit programma totdat de beweging klaar is
Jitse_Giesen 4:050dd85ffcf5 277 dLod=sqrt(pow(Lou,2)+pow(397.85,2))-Lou; //dit is wat je motoren moeten doen om te zakken
Jitse_Giesen 2:263a53d72bb7 278 dLbd=sqrt(pow(Lbu,2)+pow(397.85,2))-Lbu;
Jitse_Giesen 2:263a53d72bb7 279 dLrd=sqrt(pow(Lru,2)+pow(397.85,2))-Lru;
Jitse_Giesen 2:263a53d72bb7 280 rotzo=dLod/omtrekklosje;
Jitse_Giesen 2:263a53d72bb7 281 rotzb=dLbd/omtrekklosje;
Jitse_Giesen 2:263a53d72bb7 282 rotzr=dLrd/omtrekklosje;
Jitse_Giesen 2:263a53d72bb7 283 countzo=rotzo*4200;
Jitse_Giesen 2:263a53d72bb7 284 countzb=rotzb*4200;
Jitse_Giesen 2:263a53d72bb7 285 countzr=rotzr*4200;
Jitse_Giesen 2:263a53d72bb7 286
Jitse_Giesen 2:263a53d72bb7 287 pc.printf("o=%.2fb=%.2fr=%.2f",countzo,countzb,countzr); // hier moet komen te staan hoe het zakken gaat
Jitse_Giesen 2:263a53d72bb7 288 }
Jitse_Giesen 2:263a53d72bb7 289 }
Jitse_Giesen 0:3e98322ac0e2 290 }
Jitse_Giesen 0:3e98322ac0e2 291
Jitse_Giesen 0:3e98322ac0e2 292 int main()
Jitse_Giesen 0:3e98322ac0e2 293 {
Jitse_Giesen 5:aa191ca932ab 294 pc.baud(115200);
Jitse_Giesen 5:aa191ca932ab 295 //tellerX();
Jitse_Giesen 5:aa191ca932ab 296 //tellerY();
Jitse_Giesen 0:3e98322ac0e2 297 calculator();
Jitse_Giesen 4:050dd85ffcf5 298 controlmotor1.attach(&MeasureAndControl, 0.01);
Jitse_Giesen 4:050dd85ffcf5 299 calculatedelta.attach(&calcdelta, 0.01);
Jitse_Giesen 4:050dd85ffcf5 300 printdata.attach(&readdata, 0.5);
Jitse_Giesen 4:050dd85ffcf5 301 //zakker();
Jitse_Giesen 4:050dd85ffcf5 302
Jitse_Giesen 0:3e98322ac0e2 303 return 0;
Jitse_Giesen 0:3e98322ac0e2 304 }