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@17:55e6270adab5, 2018-05-08 (annotated)

Committer:
lucasfontenla
Date:
Tue May 08 13:06:10 2018 +0000
Revision:
17:55e6270adab5
Parent:
16:3adab04d42a6
Child:
18:174b4ff351b5
Zeramento em X e Y funcionando. Fun??o que retorna o n?mero de passos para uma certa dist?ncia funcionando. Implementado c?digo para desenho de um quadrado.;

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