christophe vermaelen / Mbed 2 deprecated TEST_ASSER_MOT_ENCODEUR_V2

pour bastos

Dependencies:   mbed

lib.cpp@0:e2ce7aa2a4a4, 2022-04-06 (annotated)

Committer:
vermaelen
Date:
Wed Apr 06 12:25:35 2022 +0000
Revision:
0:e2ce7aa2a4a4
v2

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vermaelen 0:e2ce7aa2a4a4 1 #include "mbed.h"
vermaelen 0:e2ce7aa2a4a4 2 #include "lib.h"
vermaelen 0:e2ce7aa2a4a4 3 Timer temps;
vermaelen 0:e2ce7aa2a4a4 4 DigitalOut sensG(D7);
vermaelen 0:e2ce7aa2a4a4 5 DigitalOut sensD(D9);
vermaelen 0:e2ce7aa2a4a4 6 PwmOut motG(D6);
vermaelen 0:e2ce7aa2a4a4 7 PwmOut motD(D8);
vermaelen 0:e2ce7aa2a4a4 8 DigitalIn BG(D14);
vermaelen 0:e2ce7aa2a4a4 9 InterruptIn AG(D13);
vermaelen 0:e2ce7aa2a4a4 10 InterruptIn AD(D12);
vermaelen 0:e2ce7aa2a4a4 11 DigitalIn BD(D11);
vermaelen 0:e2ce7aa2a4a4 12 int cptG=0,cptD=0;
vermaelen 0:e2ce7aa2a4a4 13 int cPosG=0,cPosD=0;
vermaelen 0:e2ce7aa2a4a4 14 int erD,somErD=0;
vermaelen 0:e2ce7aa2a4a4 15 int erG,somErG=0;
vermaelen 0:e2ce7aa2a4a4 16 int cmdD=0,cmdG=0;
vermaelen 0:e2ce7aa2a4a4 17 float cPos;
vermaelen 0:e2ce7aa2a4a4 18 Ticker tic;
vermaelen 0:e2ce7aa2a4a4 19 Ticker tac;
vermaelen 0:e2ce7aa2a4a4 20 void asserPos()
vermaelen 0:e2ce7aa2a4a4 21 {
vermaelen 0:e2ce7aa2a4a4 22 erD=cPosD-cptD;
vermaelen 0:e2ce7aa2a4a4 23 somErD=somErD+erD;
vermaelen 0:e2ce7aa2a4a4 24 if(somErD>100000)somErD=100000;
vermaelen 0:e2ce7aa2a4a4 25 if(somErD<-100000)somErD=-100000;
vermaelen 0:e2ce7aa2a4a4 26 cmdD=K*erD+K*Te*somErD/Ti;
vermaelen 0:e2ce7aa2a4a4 27
vermaelen 0:e2ce7aa2a4a4 28 erG=cPosG-cptG;
vermaelen 0:e2ce7aa2a4a4 29 somErG=somErG+erG;
vermaelen 0:e2ce7aa2a4a4 30 if(somErG>100000)somErG=100000;
vermaelen 0:e2ce7aa2a4a4 31 if(somErG<-100000)somErG=-100000;
vermaelen 0:e2ce7aa2a4a4 32 cmdG=K*erG+K*Te*somErG/Ti;
vermaelen 0:e2ce7aa2a4a4 33
vermaelen 0:e2ce7aa2a4a4 34 mot(cmdG,cmdD);
vermaelen 0:e2ce7aa2a4a4 35 }
vermaelen 0:e2ce7aa2a4a4 36 void distance(int d)//distance passee en mm
vermaelen 0:e2ce7aa2a4a4 37 {
vermaelen 0:e2ce7aa2a4a4 38 int signe;
vermaelen 0:e2ce7aa2a4a4 39 if(d>0) signe=0;
vermaelen 0:e2ce7aa2a4a4 40 else {
vermaelen 0:e2ce7aa2a4a4 41 signe=1;
vermaelen 0:e2ce7aa2a4a4 42 d=-1*d;
vermaelen 0:e2ce7aa2a4a4 43 }
vermaelen 0:e2ce7aa2a4a4 44 cptG=0;
vermaelen 0:e2ce7aa2a4a4 45 cptD=0;
vermaelen 0:e2ce7aa2a4a4 46 float dlim=1000*V*V/A; //valeur limite entre le profil en trapeze et celui en triangle
vermaelen 0:e2ce7aa2a4a4 47
vermaelen 0:e2ce7aa2a4a4 48 if(DEBUG) printf("Debut asservissement de distance... \n\rDistance a parcourir %d mm... \n\r",d);
vermaelen 0:e2ce7aa2a4a4 49 if(d>dlim) { // profil en trapeze
vermaelen 0:e2ce7aa2a4a4 50
vermaelen 0:e2ce7aa2a4a4 51 float t0=V/A;
vermaelen 0:e2ce7aa2a4a4 52 float t1=d/(V*1000.0);
vermaelen 0:e2ce7aa2a4a4 53 if(DEBUG) printf("duree prevue : %.1f s \n\r",t0+t1);
vermaelen 0:e2ce7aa2a4a4 54 temps.reset();
vermaelen 0:e2ce7aa2a4a4 55 temps.start();
vermaelen 0:e2ce7aa2a4a4 56 cPosG=0;
vermaelen 0:e2ce7aa2a4a4 57 cPosD=0;
vermaelen 0:e2ce7aa2a4a4 58 tic.attach(&asserPos,Te); // on lance l'asservissement de position
vermaelen 0:e2ce7aa2a4a4 59 float t=0;
vermaelen 0:e2ce7aa2a4a4 60 while(t<t0) {
vermaelen 0:e2ce7aa2a4a4 61 t=temps.read();
vermaelen 0:e2ce7aa2a4a4 62 cPos=A*t*t/2.0;
vermaelen 0:e2ce7aa2a4a4 63 if(signe==0)cPosG=cPos*1000.0/mmPULSE;
vermaelen 0:e2ce7aa2a4a4 64 else cPosG=-1*cPos*1000.0/mmPULSE;
vermaelen 0:e2ce7aa2a4a4 65 if(signe==0)cPosD=cPos*1000.0/mmPULSE;
vermaelen 0:e2ce7aa2a4a4 66 else cPosD=-1*cPos*1000.0/mmPULSE;
vermaelen 0:e2ce7aa2a4a4 67 }
vermaelen 0:e2ce7aa2a4a4 68 while(t<t1) {
vermaelen 0:e2ce7aa2a4a4 69 t=temps.read();
vermaelen 0:e2ce7aa2a4a4 70 cPos=(A*t0*t0/2.0)+(V*(t-t0));
vermaelen 0:e2ce7aa2a4a4 71 if(signe==0)cPosG=cPos*1000.0/mmPULSE;
vermaelen 0:e2ce7aa2a4a4 72 else cPosG=-1*cPos*1000.0/mmPULSE;
vermaelen 0:e2ce7aa2a4a4 73 if(signe==0)cPosD=cPos*1000.0/mmPULSE;
vermaelen 0:e2ce7aa2a4a4 74 else cPosD=-1*cPos*1000.0/mmPULSE;
vermaelen 0:e2ce7aa2a4a4 75 }
vermaelen 0:e2ce7aa2a4a4 76 while(t<(t1+t0)) {
vermaelen 0:e2ce7aa2a4a4 77 t=temps.read();
vermaelen 0:e2ce7aa2a4a4 78 cPos=A*t*t/(-2.0)+(V+A*t1)*t+(-1*A*t1*t1/2.0)-V*t1+A*t0*t0/2.0+V*(t1-t0);
vermaelen 0:e2ce7aa2a4a4 79 if(signe==0)cPosG=cPos*1000.0/mmPULSE;
vermaelen 0:e2ce7aa2a4a4 80 else cPosG=-1*cPos*1000.0/mmPULSE;
vermaelen 0:e2ce7aa2a4a4 81 if(signe==0)cPosD=cPos*1000.0/mmPULSE;
vermaelen 0:e2ce7aa2a4a4 82 else cPosD=-1*cPos*1000.0/mmPULSE;
vermaelen 0:e2ce7aa2a4a4 83 }
vermaelen 0:e2ce7aa2a4a4 84 while(t<(t1+t0+DELAY)) {
vermaelen 0:e2ce7aa2a4a4 85 t=temps.read();
vermaelen 0:e2ce7aa2a4a4 86 }
vermaelen 0:e2ce7aa2a4a4 87 if(DEBUG) printf("distance parcourue = %d mm -- mode trapeze\n\r",d);
vermaelen 0:e2ce7aa2a4a4 88 tic.detach();
vermaelen 0:e2ce7aa2a4a4 89 mot(0,0);
vermaelen 0:e2ce7aa2a4a4 90 } else { //profil en triangle
vermaelen 0:e2ce7aa2a4a4 91 float tA=sqrt(d/(1000.0*A));
vermaelen 0:e2ce7aa2a4a4 92 float t2A=2*tA;
vermaelen 0:e2ce7aa2a4a4 93 if(DEBUG) printf("duree prevue : %.1f s \n\r",t2A);
vermaelen 0:e2ce7aa2a4a4 94 temps.reset();
vermaelen 0:e2ce7aa2a4a4 95 temps.start();
vermaelen 0:e2ce7aa2a4a4 96 cPosG=0;
vermaelen 0:e2ce7aa2a4a4 97 cPosD=0;
vermaelen 0:e2ce7aa2a4a4 98 tac.attach(&asserPos,Te);
vermaelen 0:e2ce7aa2a4a4 99 float t=0;
vermaelen 0:e2ce7aa2a4a4 100 while(t<tA) {
vermaelen 0:e2ce7aa2a4a4 101 t=temps.read();
vermaelen 0:e2ce7aa2a4a4 102 cPos=A*t*t/2;
vermaelen 0:e2ce7aa2a4a4 103 if(signe==0)cPosG=cPos*1000.0/mmPULSE;
vermaelen 0:e2ce7aa2a4a4 104 else cPosG=-1*cPos*1000.0/mmPULSE;
vermaelen 0:e2ce7aa2a4a4 105 if(signe==0)cPosD=cPos*1000.0/mmPULSE;
vermaelen 0:e2ce7aa2a4a4 106 else cPosD=-1*cPos*1000.0/mmPULSE;
vermaelen 0:e2ce7aa2a4a4 107 }
vermaelen 0:e2ce7aa2a4a4 108 while(t<t2A) {
vermaelen 0:e2ce7aa2a4a4 109 t=temps.read();
vermaelen 0:e2ce7aa2a4a4 110 cPos=(2*A*tA*t)-(A*t*t/2)-d/1000.0;
vermaelen 0:e2ce7aa2a4a4 111 if(signe==0)cPosG=cPos*1000.0/mmPULSE;
vermaelen 0:e2ce7aa2a4a4 112 else cPosG=-1*cPos*1000.0/mmPULSE;
vermaelen 0:e2ce7aa2a4a4 113 if(signe==0)cPosD=cPos*1000.0/mmPULSE;
vermaelen 0:e2ce7aa2a4a4 114 else cPosD=-1*cPos*1000.0/mmPULSE;
vermaelen 0:e2ce7aa2a4a4 115 }
vermaelen 0:e2ce7aa2a4a4 116 while(t<(t2A+DELAY)) {
vermaelen 0:e2ce7aa2a4a4 117 t=temps.read();
vermaelen 0:e2ce7aa2a4a4 118 }
vermaelen 0:e2ce7aa2a4a4 119 if(DEBUG) printf("distance parcourue = %d mm -- mode triangle\n\r",d);
vermaelen 0:e2ce7aa2a4a4 120 tac.detach();
vermaelen 0:e2ce7aa2a4a4 121 mot(0,0);
vermaelen 0:e2ce7aa2a4a4 122 }
vermaelen 0:e2ce7aa2a4a4 123 temps.stop();
vermaelen 0:e2ce7aa2a4a4 124 temps.reset();
vermaelen 0:e2ce7aa2a4a4 125
vermaelen 0:e2ce7aa2a4a4 126 }
vermaelen 0:e2ce7aa2a4a4 127 void rotation(int deg)
vermaelen 0:e2ce7aa2a4a4 128 {
vermaelen 0:e2ce7aa2a4a4 129
vermaelen 0:e2ce7aa2a4a4 130 int d=deg*ENTREAXE/360.0;
vermaelen 0:e2ce7aa2a4a4 131 int signe;
vermaelen 0:e2ce7aa2a4a4 132 if(d>0) signe=0;
vermaelen 0:e2ce7aa2a4a4 133 else {
vermaelen 0:e2ce7aa2a4a4 134 signe=1;
vermaelen 0:e2ce7aa2a4a4 135 d=-1*d;
vermaelen 0:e2ce7aa2a4a4 136 }
vermaelen 0:e2ce7aa2a4a4 137 cptG=0;
vermaelen 0:e2ce7aa2a4a4 138 cptD=0;
vermaelen 0:e2ce7aa2a4a4 139 float dlim=1000*V*V/A;
vermaelen 0:e2ce7aa2a4a4 140
vermaelen 0:e2ce7aa2a4a4 141 if(DEBUG) printf("Debut asservissement de distance... \n\rDistance a parcourir %d mm... \n\r",d);
vermaelen 0:e2ce7aa2a4a4 142 if(d>dlim) {
vermaelen 0:e2ce7aa2a4a4 143
vermaelen 0:e2ce7aa2a4a4 144 float t0=V/A;
vermaelen 0:e2ce7aa2a4a4 145 float t1=d/(V*1000.0);
vermaelen 0:e2ce7aa2a4a4 146 if(DEBUG) printf("duree prevue : %.1f s \n\r",t0+t1);
vermaelen 0:e2ce7aa2a4a4 147 temps.reset();
vermaelen 0:e2ce7aa2a4a4 148 temps.start();
vermaelen 0:e2ce7aa2a4a4 149 cPosG=0;
vermaelen 0:e2ce7aa2a4a4 150 cPosD=0;
vermaelen 0:e2ce7aa2a4a4 151 tic.attach(&asserPos,Te);
vermaelen 0:e2ce7aa2a4a4 152 float t=0;
vermaelen 0:e2ce7aa2a4a4 153 while(t<t0) {
vermaelen 0:e2ce7aa2a4a4 154 t=temps.read();
vermaelen 0:e2ce7aa2a4a4 155 cPos=A*t*t/2.0;
vermaelen 0:e2ce7aa2a4a4 156 if(signe==0) cPosG=cPos*1000.0/mmPULSE;
vermaelen 0:e2ce7aa2a4a4 157 else cPosG=-1*cPos*1000.0/mmPULSE;
vermaelen 0:e2ce7aa2a4a4 158 if(signe==0) cPosD=-1*cPos*1000.0/mmPULSE;
vermaelen 0:e2ce7aa2a4a4 159 else cPosD=cPos*1000.0/mmPULSE;
vermaelen 0:e2ce7aa2a4a4 160 }
vermaelen 0:e2ce7aa2a4a4 161 while(t<t1) {
vermaelen 0:e2ce7aa2a4a4 162 t=temps.read();
vermaelen 0:e2ce7aa2a4a4 163 cPos=(A*t0*t0/2.0)+(V*(t-t0));
vermaelen 0:e2ce7aa2a4a4 164 if(signe==0) cPosG=cPos*1000.0/mmPULSE;
vermaelen 0:e2ce7aa2a4a4 165 else cPosG=-1*cPos*1000.0/mmPULSE;
vermaelen 0:e2ce7aa2a4a4 166 if(signe==0) cPosD=-1*cPos*1000.0/mmPULSE;
vermaelen 0:e2ce7aa2a4a4 167 else cPosD=cPos*1000.0/mmPULSE;
vermaelen 0:e2ce7aa2a4a4 168 }
vermaelen 0:e2ce7aa2a4a4 169 while(t<(t1+t0)) {
vermaelen 0:e2ce7aa2a4a4 170 t=temps.read();
vermaelen 0:e2ce7aa2a4a4 171 cPos=A*t*t/(-2.0)+(V+A*t1)*t+(-1*A*t1*t1/2.0)-V*t1+A*t0*t0/2.0+V*(t1-t0);
vermaelen 0:e2ce7aa2a4a4 172 if(signe==0) cPosG=cPos*1000.0/mmPULSE;
vermaelen 0:e2ce7aa2a4a4 173 else cPosG=-1*cPos*1000.0/mmPULSE;
vermaelen 0:e2ce7aa2a4a4 174 if(signe==0) cPosD=-1*cPos*1000.0/mmPULSE;
vermaelen 0:e2ce7aa2a4a4 175 else cPosD=cPos*1000.0/mmPULSE;
vermaelen 0:e2ce7aa2a4a4 176 }
vermaelen 0:e2ce7aa2a4a4 177 while(t<(t1+t0+DELAY)) {
vermaelen 0:e2ce7aa2a4a4 178 t=temps.read();
vermaelen 0:e2ce7aa2a4a4 179 }
vermaelen 0:e2ce7aa2a4a4 180 if(DEBUG) printf("distance parcourue = %d mm -- mode trapeze\n\r",d);
vermaelen 0:e2ce7aa2a4a4 181 tic.detach();
vermaelen 0:e2ce7aa2a4a4 182 mot(0,0);
vermaelen 0:e2ce7aa2a4a4 183 } else {
vermaelen 0:e2ce7aa2a4a4 184 float tA=sqrt(d/(1000.0*A));
vermaelen 0:e2ce7aa2a4a4 185 float t2A=2*tA;
vermaelen 0:e2ce7aa2a4a4 186 if(DEBUG) printf("duree prevue : %.1f s \n\r",t2A);
vermaelen 0:e2ce7aa2a4a4 187 temps.reset();
vermaelen 0:e2ce7aa2a4a4 188 temps.start();
vermaelen 0:e2ce7aa2a4a4 189 cPosG=0;
vermaelen 0:e2ce7aa2a4a4 190 cPosD=0;
vermaelen 0:e2ce7aa2a4a4 191 tac.attach(&asserPos,Te);
vermaelen 0:e2ce7aa2a4a4 192 float t=0;
vermaelen 0:e2ce7aa2a4a4 193 while(t<tA) {
vermaelen 0:e2ce7aa2a4a4 194 t=temps.read();
vermaelen 0:e2ce7aa2a4a4 195 cPos=A*t*t/2;
vermaelen 0:e2ce7aa2a4a4 196 if(signe==0) cPosG=cPos*1000.0/mmPULSE;
vermaelen 0:e2ce7aa2a4a4 197 else cPosG=-1*cPos*1000.0/mmPULSE;
vermaelen 0:e2ce7aa2a4a4 198 if(signe==0) cPosD=-1*cPos*1000.0/mmPULSE;
vermaelen 0:e2ce7aa2a4a4 199 else cPosD=cPos*1000.0/mmPULSE;
vermaelen 0:e2ce7aa2a4a4 200 }
vermaelen 0:e2ce7aa2a4a4 201 while(t<t2A) {
vermaelen 0:e2ce7aa2a4a4 202 t=temps.read();
vermaelen 0:e2ce7aa2a4a4 203 cPos=(2*A*tA*t)-(A*t*t/2)-d/1000.0;
vermaelen 0:e2ce7aa2a4a4 204 if(signe==0) cPosG=cPos*1000.0/mmPULSE;
vermaelen 0:e2ce7aa2a4a4 205 else cPosG=-1*cPos*1000.0/mmPULSE;
vermaelen 0:e2ce7aa2a4a4 206 if(signe==0) cPosD=-1*cPos*1000.0/mmPULSE;
vermaelen 0:e2ce7aa2a4a4 207 else cPosD=cPos*1000.0/mmPULSE;
vermaelen 0:e2ce7aa2a4a4 208 }
vermaelen 0:e2ce7aa2a4a4 209 while(t<(t2A+DELAY)) {
vermaelen 0:e2ce7aa2a4a4 210 t=temps.read();
vermaelen 0:e2ce7aa2a4a4 211 }
vermaelen 0:e2ce7aa2a4a4 212 if(DEBUG) printf("distance parcourue = %d mm -- mode triangle\n\r",d);
vermaelen 0:e2ce7aa2a4a4 213 tac.detach();
vermaelen 0:e2ce7aa2a4a4 214 mot(0,0);
vermaelen 0:e2ce7aa2a4a4 215 }
vermaelen 0:e2ce7aa2a4a4 216 temps.stop();
vermaelen 0:e2ce7aa2a4a4 217 temps.reset();
vermaelen 0:e2ce7aa2a4a4 218
vermaelen 0:e2ce7aa2a4a4 219
vermaelen 0:e2ce7aa2a4a4 220 }
vermaelen 0:e2ce7aa2a4a4 221 void init()
vermaelen 0:e2ce7aa2a4a4 222 {
vermaelen 0:e2ce7aa2a4a4 223 motG.period_us(100);
vermaelen 0:e2ce7aa2a4a4 224 motD.period_us(100);
vermaelen 0:e2ce7aa2a4a4 225 mot(0,0);
vermaelen 0:e2ce7aa2a4a4 226 AG.rise(&countGr);
vermaelen 0:e2ce7aa2a4a4 227 AG.fall(&countGf);
vermaelen 0:e2ce7aa2a4a4 228 AD.rise(&countDr);
vermaelen 0:e2ce7aa2a4a4 229 AD.fall(&countDf);
vermaelen 0:e2ce7aa2a4a4 230 //tic.attach(&asserPos,Te);
vermaelen 0:e2ce7aa2a4a4 231
vermaelen 0:e2ce7aa2a4a4 232 }
vermaelen 0:e2ce7aa2a4a4 233 void countGr()
vermaelen 0:e2ce7aa2a4a4 234 {
vermaelen 0:e2ce7aa2a4a4 235 if(BG==1)
vermaelen 0:e2ce7aa2a4a4 236 cptG++;
vermaelen 0:e2ce7aa2a4a4 237 else
vermaelen 0:e2ce7aa2a4a4 238 cptG--;
vermaelen 0:e2ce7aa2a4a4 239 }
vermaelen 0:e2ce7aa2a4a4 240 void countDr()
vermaelen 0:e2ce7aa2a4a4 241 {
vermaelen 0:e2ce7aa2a4a4 242 if(BD==0)
vermaelen 0:e2ce7aa2a4a4 243 cptD++;
vermaelen 0:e2ce7aa2a4a4 244 else
vermaelen 0:e2ce7aa2a4a4 245 cptD--;
vermaelen 0:e2ce7aa2a4a4 246 }
vermaelen 0:e2ce7aa2a4a4 247 void countGf()
vermaelen 0:e2ce7aa2a4a4 248 {
vermaelen 0:e2ce7aa2a4a4 249 if(BG==0)
vermaelen 0:e2ce7aa2a4a4 250 cptG++;
vermaelen 0:e2ce7aa2a4a4 251 else
vermaelen 0:e2ce7aa2a4a4 252 cptG--;
vermaelen 0:e2ce7aa2a4a4 253 }
vermaelen 0:e2ce7aa2a4a4 254 void countDf()
vermaelen 0:e2ce7aa2a4a4 255 {
vermaelen 0:e2ce7aa2a4a4 256 if(BD==1)
vermaelen 0:e2ce7aa2a4a4 257 cptD++;
vermaelen 0:e2ce7aa2a4a4 258 else
vermaelen 0:e2ce7aa2a4a4 259 cptD--;
vermaelen 0:e2ce7aa2a4a4 260 }
vermaelen 0:e2ce7aa2a4a4 261
vermaelen 0:e2ce7aa2a4a4 262
vermaelen 0:e2ce7aa2a4a4 263 void aff()
vermaelen 0:e2ce7aa2a4a4 264 {
vermaelen 0:e2ce7aa2a4a4 265 float vD=30*cptD/(22.0*5.0*Te);
vermaelen 0:e2ce7aa2a4a4 266 float vG=30*cptG/(22.0*5.0*Te);
vermaelen 0:e2ce7aa2a4a4 267 int valD=100*motD.read();
vermaelen 0:e2ce7aa2a4a4 268 int valG=100*motG.read();
vermaelen 0:e2ce7aa2a4a4 269 //printf("cptG=%d -- motG=%.3f cm/s -- cptD=%d -- motD=%.3f cm/s -- valD=%d\n\r",cptG,vG,cptD,vD,valD);
vermaelen 0:e2ce7aa2a4a4 270
vermaelen 0:e2ce7aa2a4a4 271
vermaelen 0:e2ce7aa2a4a4 272 }
vermaelen 0:e2ce7aa2a4a4 273 int borne(int a)
vermaelen 0:e2ce7aa2a4a4 274 {
vermaelen 0:e2ce7aa2a4a4 275 if(a>100) a=100;
vermaelen 0:e2ce7aa2a4a4 276 if(a<-100) a=-100;
vermaelen 0:e2ce7aa2a4a4 277 return a;
vermaelen 0:e2ce7aa2a4a4 278 }
vermaelen 0:e2ce7aa2a4a4 279 void mot(int g,int d)
vermaelen 0:e2ce7aa2a4a4 280 {
vermaelen 0:e2ce7aa2a4a4 281 g=borne(g);
vermaelen 0:e2ce7aa2a4a4 282 d=borne(d);
vermaelen 0:e2ce7aa2a4a4 283 if(g>0) {
vermaelen 0:e2ce7aa2a4a4 284 sensG.write(1);
vermaelen 0:e2ce7aa2a4a4 285 g=100-g;
vermaelen 0:e2ce7aa2a4a4 286 motG.pulsewidth_us(g);
vermaelen 0:e2ce7aa2a4a4 287 } else {
vermaelen 0:e2ce7aa2a4a4 288 sensG.write(0);
vermaelen 0:e2ce7aa2a4a4 289 g=100+g;
vermaelen 0:e2ce7aa2a4a4 290 motG.pulsewidth_us(g);
vermaelen 0:e2ce7aa2a4a4 291 }
vermaelen 0:e2ce7aa2a4a4 292 if(d>0) {
vermaelen 0:e2ce7aa2a4a4 293 sensD.write(1);
vermaelen 0:e2ce7aa2a4a4 294 d=100-d;
vermaelen 0:e2ce7aa2a4a4 295 motD.pulsewidth_us(d);
vermaelen 0:e2ce7aa2a4a4 296 } else {
vermaelen 0:e2ce7aa2a4a4 297 sensD.write(0);
vermaelen 0:e2ce7aa2a4a4 298 d=100+d;
vermaelen 0:e2ce7aa2a4a4 299 motD.pulsewidth_us(d);
vermaelen 0:e2ce7aa2a4a4 300 }
vermaelen 0:e2ce7aa2a4a4 301 }