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@16:3adab04d42a6, 2018-05-08 (annotated)

Committer:
lucasfontenla
Date:
Tue May 08 11:41:56 2018 +0000
Revision:
16:3adab04d42a6
Parent:
15:4604a7ee9c77
Child:
17:55e6270adab5
Fins de curso implementados em paralelos (X, Y e Z), movimenta??o em JOG e c?lculo da dist?ncia funcionando 100%.

Who changed what in which revision?

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