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@14:7cc41420a12c, 2018-05-04 (annotated)

Committer:
lucasfontenla
Date:
Fri May 04 21:00:25 2018 +0000
Revision:
14:7cc41420a12c
Parent:
13:0f385bfe3e0d
Child:
15:4604a7ee9c77
Portas atualizadas;

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