Grupo T / Mbed OS GRUPOT

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.

main.cpp@13:0f385bfe3e0d, 2018-05-04 (annotated)

Committer:
lucasfontenla
Date:
Fri May 04 18:52:34 2018 +0000
Revision:
13:0f385bfe3e0d
Parent:
12:801e58a7137c
Child:
14:7cc41420a12c
Zeramento em X funcionando;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lucasfontenla 0:cc562f78f889 1 #include "mbed.h"
lucasfontenla 0:cc562f78f889 2
lucasfontenla 9:d771a576e4fc 3 // Classes ---------------------------------------------------------------------------------
lucasfontenla 9:d771a576e4fc 4 Timer debounce;
lucasfontenla 9:d771a576e4fc 5
lucasfontenla 5:0cbf491f703d 6 // MACHINE SETUP ---------------------------------------------------------------------------
lucasfontenla 5:0cbf491f703d 7
lucasfontenla 5:0cbf491f703d 8 // drivers output signal
lucasfontenla 4:48717b42eab2 9 DigitalOut stepX(D2);
lucasfontenla 4:48717b42eab2 10 DigitalOut dirX(D3);
lucasfontenla 4:48717b42eab2 11
lucasfontenla 4:48717b42eab2 12 DigitalOut stepY(D8);
lucasfontenla 4:48717b42eab2 13 DigitalOut dirY(D9);
lucasfontenla 4:48717b42eab2 14
lucasfontenla 4:48717b42eab2 15 DigitalOut stepZ(D11);
lucasfontenla 4:48717b42eab2 16 DigitalOut dirZ(D12);
lucasfontenla 4:48717b42eab2 17
lucasfontenla 5:0cbf491f703d 18 // hardware input signal
lucasfontenla 5:0cbf491f703d 19 // end-of-stroke sensors
lucasfontenla 13:0f385bfe3e0d 20 InterruptIn endX1(D14);
lucasfontenla 13:0f385bfe3e0d 21 InterruptIn endX2(D10);
lucasfontenla 13:0f385bfe3e0d 22
lucasfontenla 10:cd868ca9997c 23 InterruptIn endY1(D5);
lucasfontenla 10:cd868ca9997c 24 InterruptIn endY2(D13);
lucasfontenla 5:0cbf491f703d 25
lucasfontenla 5:0cbf491f703d 26 // IHM for development
lucasfontenla 9:d771a576e4fc 27 AnalogIn joyX(A0);
lucasfontenla 9:d771a576e4fc 28 AnalogIn joyY(A1);
lucasfontenla 4:48717b42eab2 29
lucasfontenla 8:de64672c240f 30 DigitalIn zUp(D7);
lucasfontenla 8:de64672c240f 31 DigitalIn zDwn(D6);
lucasfontenla 4:48717b42eab2 32
lucasfontenla 5:0cbf491f703d 33 // variables definition
lucasfontenla 9:d771a576e4fc 34 int modeStatus = 0;
lucasfontenla 5:0cbf491f703d 35
lucasfontenla 4:48717b42eab2 36 float valX;
lucasfontenla 4:48717b42eab2 37 float valY;
lucasfontenla 4:48717b42eab2 38 int valZUp;
lucasfontenla 4:48717b42eab2 39 int valZDwn;
lucasfontenla 4:48717b42eab2 40
lucasfontenla 4:48717b42eab2 41 int activeX = 1;
lucasfontenla 4:48717b42eab2 42 int activeY = 1;
lucasfontenla 4:48717b42eab2 43 int activeZ = 1;
lucasfontenla 4:48717b42eab2 44
lucasfontenla 5:0cbf491f703d 45 int x_dir;
lucasfontenla 5:0cbf491f703d 46 int y_dir;
lucasfontenla 5:0cbf491f703d 47 int z_dir;
lucasfontenla 5:0cbf491f703d 48
lucasfontenla 13:0f385bfe3e0d 49 int x_plus = 0;
lucasfontenla 13:0f385bfe3e0d 50 int x_minus = 1;
lucasfontenla 9:d771a576e4fc 51 int y_plus = 1;
lucasfontenla 9:d771a576e4fc 52 int y_minus = 0;
lucasfontenla 9:d771a576e4fc 53 int z_plus = 1;
lucasfontenla 9:d771a576e4fc 54 int z_minus = 0;
lucasfontenla 9:d771a576e4fc 55
lucasfontenla 12:801e58a7137c 56 int x_limit_min = 0;
lucasfontenla 12:801e58a7137c 57 int x_limit_max = 0;
lucasfontenla 12:801e58a7137c 58
lucasfontenla 10:cd868ca9997c 59 int y_limit_min = 0;
lucasfontenla 10:cd868ca9997c 60 int y_limit_max = 0;
lucasfontenla 9:d771a576e4fc 61
lucasfontenla 5:0cbf491f703d 62 float ppsMax = 600.0;
lucasfontenla 5:0cbf491f703d 63
lucasfontenla 7:27ea14e58788 64 int totalX = 0;
lucasfontenla 7:27ea14e58788 65 int totalY = 0;
lucasfontenla 7:27ea14e58788 66
lucasfontenla 7:27ea14e58788 67 float X = 0;
lucasfontenla 7:27ea14e58788 68 float Y = 0;
lucasfontenla 7:27ea14e58788 69
lucasfontenla 9:d771a576e4fc 70 int endX1Status;
lucasfontenla 9:d771a576e4fc 71 int endX2Status;
lucasfontenla 9:d771a576e4fc 72
lucasfontenla 7:27ea14e58788 73 // machine parameters
lucasfontenla 8:de64672c240f 74 float xPitch = 3.0;
lucasfontenla 8:de64672c240f 75 float yPitch = 3.0;
lucasfontenla 7:27ea14e58788 76
lucasfontenla 7:27ea14e58788 77 float motor_steps_by_rotation = 200.0;
lucasfontenla 7:27ea14e58788 78 float step_mode = 0.5;
lucasfontenla 7:27ea14e58788 79 float steps_by_rotation = motor_steps_by_rotation/step_mode;
lucasfontenla 7:27ea14e58788 80
lucasfontenla 7:27ea14e58788 81 // prototype functions
lucasfontenla 9:d771a576e4fc 82 void setupPins();
lucasfontenla 5:0cbf491f703d 83 void move(int pps, int x_dir, int y_dir, int z_dir, int x_step, int y_step, int z_step);
lucasfontenla 7:27ea14e58788 84 float distance(int steps, float pitch);
lucasfontenla 7:27ea14e58788 85
lucasfontenla 11:0e22a6fc7f87 86 void endX1Int_press(void);
lucasfontenla 11:0e22a6fc7f87 87 void endX1Int_release(void);
lucasfontenla 11:0e22a6fc7f87 88 void endX2Int_press(void);
lucasfontenla 11:0e22a6fc7f87 89 void endX2Int_release(void);
lucasfontenla 11:0e22a6fc7f87 90
lucasfontenla 10:cd868ca9997c 91 void endY1Int_press(void);
lucasfontenla 10:cd868ca9997c 92 void endY1Int_release(void);
lucasfontenla 10:cd868ca9997c 93 void endY2Int_press(void);
lucasfontenla 10:cd868ca9997c 94 void endY2Int_release(void);
lucasfontenla 9:d771a576e4fc 95
lucasfontenla 12:801e58a7137c 96 void zeroX(int pps);
lucasfontenla 11:0e22a6fc7f87 97 void zeroY(int pps);
lucasfontenla 11:0e22a6fc7f87 98
lucasfontenla 7:27ea14e58788 99 // MAIN PROGRAM ----------------------------------------------------------------
lucasfontenla 7:27ea14e58788 100
lucasfontenla 7:27ea14e58788 101 int contador = 0;
lucasfontenla 1:b2e395e50a45 102
lucasfontenla 1:b2e395e50a45 103 int main(){
lucasfontenla 4:48717b42eab2 104 printf("Starting...\r\n");
lucasfontenla 12:801e58a7137c 105
lucasfontenla 12:801e58a7137c 106 endX1.fall(&endX1Int_press);
lucasfontenla 12:801e58a7137c 107 endX1.rise(&endX1Int_release);
lucasfontenla 12:801e58a7137c 108 endX2.fall(&endX2Int_press);
lucasfontenla 12:801e58a7137c 109 endX2.rise(&endX2Int_release);
lucasfontenla 5:0cbf491f703d 110
lucasfontenla 10:cd868ca9997c 111 endY1.fall(&endY1Int_press);
lucasfontenla 10:cd868ca9997c 112 endY1.rise(&endY1Int_release);
lucasfontenla 10:cd868ca9997c 113 endY2.fall(&endY2Int_press);
lucasfontenla 10:cd868ca9997c 114 endY2.rise(&endY2Int_release);
lucasfontenla 9:d771a576e4fc 115
lucasfontenla 5:0cbf491f703d 116 if(modeStatus){
lucasfontenla 5:0cbf491f703d 117 //Código de JOG
lucasfontenla 7:27ea14e58788 118 printf("JOG Selected\n");
lucasfontenla 5:0cbf491f703d 119 while(1){
lucasfontenla 5:0cbf491f703d 120 valX = joyX;
lucasfontenla 5:0cbf491f703d 121 valY = joyY;
lucasfontenla 5:0cbf491f703d 122
lucasfontenla 5:0cbf491f703d 123 valZUp = zUp;
lucasfontenla 5:0cbf491f703d 124 valZDwn = zDwn;
lucasfontenla 5:0cbf491f703d 125
lucasfontenla 5:0cbf491f703d 126 if(valX > 0.7){
lucasfontenla 9:d771a576e4fc 127 if(x_dir != x_minus){
lucasfontenla 9:d771a576e4fc 128 x_dir = x_minus;
lucasfontenla 5:0cbf491f703d 129 }
lucasfontenla 7:27ea14e58788 130 activeX = 1;
lucasfontenla 7:27ea14e58788 131 totalX-=1;
lucasfontenla 5:0cbf491f703d 132 }
lucasfontenla 5:0cbf491f703d 133 else if(valX < 0.3){
lucasfontenla 9:d771a576e4fc 134 if(x_dir != x_plus){
lucasfontenla 9:d771a576e4fc 135 x_dir = x_plus;
lucasfontenla 5:0cbf491f703d 136 }
lucasfontenla 7:27ea14e58788 137 activeX = 1;
lucasfontenla 7:27ea14e58788 138 totalX+=1;
lucasfontenla 5:0cbf491f703d 139 }
lucasfontenla 5:0cbf491f703d 140 else{
lucasfontenla 5:0cbf491f703d 141 activeX = 0;
lucasfontenla 4:48717b42eab2 142 }
lucasfontenla 5:0cbf491f703d 143
lucasfontenla 5:0cbf491f703d 144 //----------------------------------------------------------------------
lucasfontenla 5:0cbf491f703d 145
lucasfontenla 5:0cbf491f703d 146 if(valY > 0.7){
lucasfontenla 9:d771a576e4fc 147 if(y_dir != y_plus){
lucasfontenla 9:d771a576e4fc 148 y_dir = y_plus;
lucasfontenla 5:0cbf491f703d 149 }
lucasfontenla 7:27ea14e58788 150 activeY = 1;
lucasfontenla 7:27ea14e58788 151 totalY+=1;
lucasfontenla 5:0cbf491f703d 152 }
lucasfontenla 5:0cbf491f703d 153 else if(valY < 0.3){
lucasfontenla 9:d771a576e4fc 154 if(y_dir != y_minus){
lucasfontenla 9:d771a576e4fc 155 y_dir = y_minus;
lucasfontenla 5:0cbf491f703d 156 }
lucasfontenla 7:27ea14e58788 157 activeY = 1;
lucasfontenla 7:27ea14e58788 158 totalY-=1;
lucasfontenla 5:0cbf491f703d 159 }
lucasfontenla 5:0cbf491f703d 160 else{
lucasfontenla 5:0cbf491f703d 161 activeY = 0;
lucasfontenla 4:48717b42eab2 162 }
lucasfontenla 5:0cbf491f703d 163
lucasfontenla 5:0cbf491f703d 164 //----------------------------------------------------------------------
lucasfontenla 5:0cbf491f703d 165
lucasfontenla 5:0cbf491f703d 166 if(!valZUp && valZDwn){
lucasfontenla 9:d771a576e4fc 167 if(z_dir != z_minus){
lucasfontenla 9:d771a576e4fc 168 z_dir = z_minus;
lucasfontenla 7:27ea14e58788 169 //totalZ-=1;
lucasfontenla 5:0cbf491f703d 170 }
lucasfontenla 5:0cbf491f703d 171 activeZ = 1;
lucasfontenla 5:0cbf491f703d 172 }
lucasfontenla 5:0cbf491f703d 173 else if(!valZDwn && valZUp){
lucasfontenla 9:d771a576e4fc 174 if(z_dir != z_plus){
lucasfontenla 9:d771a576e4fc 175 z_dir = z_plus;
lucasfontenla 7:27ea14e58788 176 //totalZ+=1;
lucasfontenla 5:0cbf491f703d 177 }
lucasfontenla 5:0cbf491f703d 178 activeZ = 1;
lucasfontenla 5:0cbf491f703d 179 }
lucasfontenla 5:0cbf491f703d 180 else {
lucasfontenla 5:0cbf491f703d 181 activeZ = 0;
lucasfontenla 5:0cbf491f703d 182 }
lucasfontenla 5:0cbf491f703d 183
lucasfontenla 5:0cbf491f703d 184 //----------------------------------------------------------------------
lucasfontenla 5:0cbf491f703d 185
lucasfontenla 7:27ea14e58788 186 move(ppsMax, x_dir, y_dir, z_dir, activeX, activeY, activeZ);
lucasfontenla 7:27ea14e58788 187 X = distance(totalX, xPitch);
lucasfontenla 7:27ea14e58788 188 Y = distance(totalY, yPitch);
lucasfontenla 7:27ea14e58788 189
lucasfontenla 7:27ea14e58788 190 if(contador > 10000){
lucasfontenla 7:27ea14e58788 191 X = distance(totalX, xPitch);
lucasfontenla 7:27ea14e58788 192 Y = distance(totalY, yPitch);
lucasfontenla 7:27ea14e58788 193 printf("X: %.2f Y: %.2f\r\n", X, Y);
lucasfontenla 7:27ea14e58788 194 contador = 0;
lucasfontenla 7:27ea14e58788 195 }
lucasfontenla 7:27ea14e58788 196
lucasfontenla 7:27ea14e58788 197 contador+=1;
lucasfontenla 4:48717b42eab2 198 }
lucasfontenla 5:0cbf491f703d 199 }
lucasfontenla 5:0cbf491f703d 200
lucasfontenla 5:0cbf491f703d 201 else {
lucasfontenla 11:0e22a6fc7f87 202 zeroY(ppsMax);
lucasfontenla 9:d771a576e4fc 203
lucasfontenla 11:0e22a6fc7f87 204 X = distance(totalX, xPitch);
lucasfontenla 11:0e22a6fc7f87 205 Y = distance(totalY, yPitch);
lucasfontenla 9:d771a576e4fc 206
lucasfontenla 11:0e22a6fc7f87 207 printf("X: %.2f Y: %.2f\r\n", X, Y);
lucasfontenla 13:0f385bfe3e0d 208
lucasfontenla 13:0f385bfe3e0d 209 zeroX(ppsMax);
lucasfontenla 13:0f385bfe3e0d 210
lucasfontenla 13:0f385bfe3e0d 211 X = distance(totalX, xPitch);
lucasfontenla 13:0f385bfe3e0d 212 Y = distance(totalY, yPitch);
lucasfontenla 13:0f385bfe3e0d 213
lucasfontenla 13:0f385bfe3e0d 214 printf("X: %.2f Y: %.2f\r\n", X, Y);
lucasfontenla 9:d771a576e4fc 215 }
lucasfontenla 9:d771a576e4fc 216 }
lucasfontenla 9:d771a576e4fc 217
lucasfontenla 9:d771a576e4fc 218 // FUNCTIONS ----------------------------------------------------------------
lucasfontenla 12:801e58a7137c 219 void endX1Int_press(void){
lucasfontenla 12:801e58a7137c 220 printf("X1 - press\n\r");
lucasfontenla 12:801e58a7137c 221 x_limit_min = 1;
lucasfontenla 12:801e58a7137c 222 }
lucasfontenla 12:801e58a7137c 223 void endX1Int_release(void){
lucasfontenla 12:801e58a7137c 224 printf("X1 - release\n\r");
lucasfontenla 12:801e58a7137c 225 x_limit_min = 0;
lucasfontenla 12:801e58a7137c 226 }
lucasfontenla 12:801e58a7137c 227 void endX2Int_press(void){
lucasfontenla 12:801e58a7137c 228 printf("X2 - press\n\r");
lucasfontenla 12:801e58a7137c 229 x_limit_max = 1;
lucasfontenla 12:801e58a7137c 230 }
lucasfontenla 12:801e58a7137c 231 void endX2Int_release(void){
lucasfontenla 12:801e58a7137c 232 printf("X2 - release\n\r");
lucasfontenla 12:801e58a7137c 233 x_limit_max = 0;
lucasfontenla 9:d771a576e4fc 234 }
lucasfontenla 9:d771a576e4fc 235
lucasfontenla 12:801e58a7137c 236 void endY1Int_press(void){
lucasfontenla 12:801e58a7137c 237 printf("Y1 - press\n\r");
lucasfontenla 12:801e58a7137c 238 y_limit_min = 1;
lucasfontenla 12:801e58a7137c 239 }
lucasfontenla 10:cd868ca9997c 240 void endY1Int_release(void){
lucasfontenla 10:cd868ca9997c 241 printf("Y1 - release\n\r");
lucasfontenla 12:801e58a7137c 242 y_limit_min = 0;
lucasfontenla 9:d771a576e4fc 243 }
lucasfontenla 10:cd868ca9997c 244 void endY2Int_press(void){
lucasfontenla 10:cd868ca9997c 245 printf("Y2 - press\n\r");
lucasfontenla 12:801e58a7137c 246 y_limit_max = 1;
lucasfontenla 9:d771a576e4fc 247 }
lucasfontenla 10:cd868ca9997c 248 void endY2Int_release(void){
lucasfontenla 10:cd868ca9997c 249 printf("Y2 - release\n\r");
lucasfontenla 12:801e58a7137c 250 y_limit_max = 0;
lucasfontenla 9:d771a576e4fc 251 }
lucasfontenla 7:27ea14e58788 252
lucasfontenla 5:0cbf491f703d 253 void move(int pps, int x_dir, int y_dir, int z_dir, int x_step, int y_step, int z_step){
lucasfontenla 5:0cbf491f703d 254 float time = 1.0/pps/2.0;
lucasfontenla 5:0cbf491f703d 255
lucasfontenla 5:0cbf491f703d 256 dirX = x_dir;
lucasfontenla 5:0cbf491f703d 257 dirY = y_dir;
lucasfontenla 5:0cbf491f703d 258 dirZ = z_dir;
lucasfontenla 5:0cbf491f703d 259
lucasfontenla 12:801e58a7137c 260 if((x_dir == x_minus) && x_limit_min){
lucasfontenla 12:801e58a7137c 261 x_step = 0;
lucasfontenla 12:801e58a7137c 262 }
lucasfontenla 12:801e58a7137c 263 if((x_dir == x_plus) && x_limit_max){
lucasfontenla 12:801e58a7137c 264 x_step = 0;
lucasfontenla 12:801e58a7137c 265 }
lucasfontenla 12:801e58a7137c 266
lucasfontenla 11:0e22a6fc7f87 267 if((y_dir == y_minus) && y_limit_min){
lucasfontenla 11:0e22a6fc7f87 268 y_step = 0;
lucasfontenla 11:0e22a6fc7f87 269 }
lucasfontenla 11:0e22a6fc7f87 270 if((y_dir == y_plus) && y_limit_max){
lucasfontenla 11:0e22a6fc7f87 271 y_step = 0;
lucasfontenla 11:0e22a6fc7f87 272 }
lucasfontenla 11:0e22a6fc7f87 273
lucasfontenla 5:0cbf491f703d 274 int max_val;
lucasfontenla 5:0cbf491f703d 275
lucasfontenla 6:af32c68ff4cf 276 // maior número de passos -------
lucasfontenla 5:0cbf491f703d 277 if(x_step > y_step){
lucasfontenla 5:0cbf491f703d 278 if(x_step > z_step){
lucasfontenla 5:0cbf491f703d 279 max_val = x_step;
lucasfontenla 4:48717b42eab2 280 }
lucasfontenla 4:48717b42eab2 281 else {
lucasfontenla 5:0cbf491f703d 282 max_val = z_step;
lucasfontenla 4:48717b42eab2 283 }
lucasfontenla 5:0cbf491f703d 284 } else if(y_step > z_step){
lucasfontenla 5:0cbf491f703d 285 max_val = y_step;
lucasfontenla 5:0cbf491f703d 286 }
lucasfontenla 5:0cbf491f703d 287 else {
lucasfontenla 5:0cbf491f703d 288 max_val = z_step;
lucasfontenla 5:0cbf491f703d 289 }
lucasfontenla 5:0cbf491f703d 290
lucasfontenla 5:0cbf491f703d 291 for(int i = 0; i < max_val; i++){
lucasfontenla 6:af32c68ff4cf 292 if(i >= x_step){
lucasfontenla 5:0cbf491f703d 293 stepX = 0;
lucasfontenla 5:0cbf491f703d 294 }
lucasfontenla 5:0cbf491f703d 295 else { stepX = 1; }
lucasfontenla 4:48717b42eab2 296
lucasfontenla 6:af32c68ff4cf 297 if(i >= y_step){
lucasfontenla 5:0cbf491f703d 298 stepY = 0;
lucasfontenla 5:0cbf491f703d 299 }
lucasfontenla 11:0e22a6fc7f87 300 else {
lucasfontenla 11:0e22a6fc7f87 301 stepY = 1;}
lucasfontenla 5:0cbf491f703d 302
lucasfontenla 6:af32c68ff4cf 303 if(i >= z_step){
lucasfontenla 5:0cbf491f703d 304 stepZ = 0;
lucasfontenla 5:0cbf491f703d 305 }
lucasfontenla 5:0cbf491f703d 306 else { stepZ = 1; }
lucasfontenla 5:0cbf491f703d 307
lucasfontenla 5:0cbf491f703d 308 wait(time);
lucasfontenla 4:48717b42eab2 309 stepX = 0;
lucasfontenla 4:48717b42eab2 310 stepY = 0;
lucasfontenla 4:48717b42eab2 311 stepZ = 0;
lucasfontenla 5:0cbf491f703d 312 wait(time);
lucasfontenla 4:48717b42eab2 313 }
lucasfontenla 5:0cbf491f703d 314 }
lucasfontenla 7:27ea14e58788 315
lucasfontenla 12:801e58a7137c 316 void zeroX(int pps){
lucasfontenla 12:801e58a7137c 317 float time = 1.0/pps/2.0;
lucasfontenla 12:801e58a7137c 318
lucasfontenla 12:801e58a7137c 319 while(!x_limit_min){
lucasfontenla 12:801e58a7137c 320 if(dirX != x_minus){
lucasfontenla 12:801e58a7137c 321 dirX = x_minus;
lucasfontenla 12:801e58a7137c 322 }
lucasfontenla 12:801e58a7137c 323 stepX = 1;
lucasfontenla 12:801e58a7137c 324 wait(time);
lucasfontenla 12:801e58a7137c 325 stepX = 0;
lucasfontenla 12:801e58a7137c 326 wait(time);
lucasfontenla 12:801e58a7137c 327 }
lucasfontenla 12:801e58a7137c 328
lucasfontenla 12:801e58a7137c 329 printf("X zero\n\r");
lucasfontenla 12:801e58a7137c 330
lucasfontenla 12:801e58a7137c 331 totalX = 0.0;
lucasfontenla 12:801e58a7137c 332 }
lucasfontenla 12:801e58a7137c 333
lucasfontenla 11:0e22a6fc7f87 334 void zeroY(int pps){
lucasfontenla 11:0e22a6fc7f87 335 float time = 1.0/pps/2.0;
lucasfontenla 11:0e22a6fc7f87 336
lucasfontenla 11:0e22a6fc7f87 337 while(!y_limit_min){
lucasfontenla 11:0e22a6fc7f87 338 if(dirY != y_minus){
lucasfontenla 11:0e22a6fc7f87 339 dirY = y_minus;
lucasfontenla 11:0e22a6fc7f87 340 }
lucasfontenla 11:0e22a6fc7f87 341 stepY = 1;
lucasfontenla 11:0e22a6fc7f87 342 wait(time);
lucasfontenla 11:0e22a6fc7f87 343 stepY = 0;
lucasfontenla 11:0e22a6fc7f87 344 wait(time);
lucasfontenla 11:0e22a6fc7f87 345 }
lucasfontenla 11:0e22a6fc7f87 346
lucasfontenla 11:0e22a6fc7f87 347 printf("Y zero\n\r");
lucasfontenla 11:0e22a6fc7f87 348
lucasfontenla 11:0e22a6fc7f87 349 totalY = 0.0;
lucasfontenla 11:0e22a6fc7f87 350 }
lucasfontenla 11:0e22a6fc7f87 351
lucasfontenla 7:27ea14e58788 352 float distance(int steps, float pitch){
lucasfontenla 7:27ea14e58788 353 float delta_S = (((float)steps)/steps_by_rotation)*pitch;
lucasfontenla 7:27ea14e58788 354 return delta_S;
lucasfontenla 7:27ea14e58788 355 }