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@19:e69ff16f35aa, 2018-05-08 (annotated)

Committer:
lucasfontenla
Date:
Tue May 08 16:41:37 2018 +0000
Revision:
19:e69ff16f35aa
Parent:
18:174b4ff351b5
Child:
20:1d98a6f51dee
Fun??o de dist?ncia consertada e funcionando.; Salvamento e execu??o de pontos funcionando.; Corrigir bugs em pressionar os bot?es amarelo e azul.

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 18:174b4ff351b5 30 InterruptIn saveBtn(D13);
lucasfontenla 18:174b4ff351b5 31 InterruptIn startBtn(D12);
lucasfontenla 18:174b4ff351b5 32
lucasfontenla 17:55e6270adab5 33 DigitalIn botao(D11);
lucasfontenla 17:55e6270adab5 34
lucasfontenla 5:0cbf491f703d 35 // variables definition
lucasfontenla 17:55e6270adab5 36 int modeStatus;
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 17:55e6270adab5 53 int y_plus = 0;
lucasfontenla 17:55e6270adab5 54 int y_minus = 1;
lucasfontenla 16:3adab04d42a6 55 int z_plus = 0;
lucasfontenla 16:3adab04d42a6 56 int z_minus = 1;
lucasfontenla 9:d771a576e4fc 57
lucasfontenla 15:4604a7ee9c77 58 int x_limit = 0;
lucasfontenla 16:3adab04d42a6 59 int x_block_max = 0;
lucasfontenla 16:3adab04d42a6 60 int x_block_min = 0;
lucasfontenla 12:801e58a7137c 61
lucasfontenla 15:4604a7ee9c77 62 int y_limit = 0;
lucasfontenla 16:3adab04d42a6 63 int y_block_max = 0;
lucasfontenla 16:3adab04d42a6 64 int y_block_min = 0;
lucasfontenla 15:4604a7ee9c77 65
lucasfontenla 15:4604a7ee9c77 66 int z_limit = 0;
lucasfontenla 16:3adab04d42a6 67 int z_block_max = 0;
lucasfontenla 16:3adab04d42a6 68 int z_block_min = 0;
lucasfontenla 9:d771a576e4fc 69
lucasfontenla 5:0cbf491f703d 70 float ppsMax = 600.0;
lucasfontenla 5:0cbf491f703d 71
lucasfontenla 7:27ea14e58788 72 int totalX = 0;
lucasfontenla 7:27ea14e58788 73 int totalY = 0;
lucasfontenla 15:4604a7ee9c77 74 int totalZ = 0;
lucasfontenla 7:27ea14e58788 75
lucasfontenla 17:55e6270adab5 76 int x_steps_to_run;
lucasfontenla 17:55e6270adab5 77 int y_steps_to_run;
lucasfontenla 17:55e6270adab5 78 int z_steps_to_run;
lucasfontenla 17:55e6270adab5 79
lucasfontenla 18:174b4ff351b5 80 float x_distance_to_run;
lucasfontenla 18:174b4ff351b5 81 float y_distance_to_run;
lucasfontenla 18:174b4ff351b5 82 float z_distance_to_run;
lucasfontenla 18:174b4ff351b5 83
lucasfontenla 7:27ea14e58788 84 float X = 0;
lucasfontenla 7:27ea14e58788 85 float Y = 0;
lucasfontenla 15:4604a7ee9c77 86 float Z = 0;
lucasfontenla 7:27ea14e58788 87
lucasfontenla 9:d771a576e4fc 88 int endX1Status;
lucasfontenla 9:d771a576e4fc 89 int endX2Status;
lucasfontenla 9:d771a576e4fc 90
lucasfontenla 7:27ea14e58788 91 // machine parameters
lucasfontenla 8:de64672c240f 92 float xPitch = 3.0;
lucasfontenla 8:de64672c240f 93 float yPitch = 3.0;
lucasfontenla 15:4604a7ee9c77 94 float zPitch = 3.0;
lucasfontenla 7:27ea14e58788 95
lucasfontenla 7:27ea14e58788 96 float motor_steps_by_rotation = 200.0;
lucasfontenla 7:27ea14e58788 97 float step_mode = 0.5;
lucasfontenla 7:27ea14e58788 98 float steps_by_rotation = motor_steps_by_rotation/step_mode;
lucasfontenla 7:27ea14e58788 99
lucasfontenla 18:174b4ff351b5 100 int saved = 0;
lucasfontenla 19:e69ff16f35aa 101 const int maxPoints = 10;
lucasfontenla 18:174b4ff351b5 102 float points[3][maxPoints];
lucasfontenla 19:e69ff16f35aa 103 int pontos_restantes = 0;
lucasfontenla 18:174b4ff351b5 104
lucasfontenla 18:174b4ff351b5 105 int startSaved = 0;
lucasfontenla 18:174b4ff351b5 106
lucasfontenla 7:27ea14e58788 107 // prototype functions
lucasfontenla 9:d771a576e4fc 108 void setupPins();
lucasfontenla 5:0cbf491f703d 109 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 110
lucasfontenla 17:55e6270adab5 111 float steps_to_distance(int steps, float pitch);
lucasfontenla 17:55e6270adab5 112 int distance_to_steps(float distance, float pitch);
lucasfontenla 7:27ea14e58788 113
lucasfontenla 15:4604a7ee9c77 114 void endX_press(void);
lucasfontenla 15:4604a7ee9c77 115 void endX_release(void);
lucasfontenla 11:0e22a6fc7f87 116
lucasfontenla 15:4604a7ee9c77 117 void endY_press(void);
lucasfontenla 15:4604a7ee9c77 118 void endY_release(void);
lucasfontenla 15:4604a7ee9c77 119
lucasfontenla 15:4604a7ee9c77 120 void endZ_press(void);
lucasfontenla 15:4604a7ee9c77 121 void endZ_release(void);
lucasfontenla 9:d771a576e4fc 122
lucasfontenla 12:801e58a7137c 123 void zeroX(int pps);
lucasfontenla 11:0e22a6fc7f87 124 void zeroY(int pps);
lucasfontenla 11:0e22a6fc7f87 125
lucasfontenla 18:174b4ff351b5 126 void savePoint(void);
lucasfontenla 18:174b4ff351b5 127 void startSavedPoints(void);
lucasfontenla 18:174b4ff351b5 128
lucasfontenla 17:55e6270adab5 129 void printDistance(void);
lucasfontenla 17:55e6270adab5 130
lucasfontenla 7:27ea14e58788 131 // MAIN PROGRAM ----------------------------------------------------------------
lucasfontenla 7:27ea14e58788 132
lucasfontenla 7:27ea14e58788 133 int contador = 0;
lucasfontenla 1:b2e395e50a45 134
lucasfontenla 1:b2e395e50a45 135 int main(){
lucasfontenla 16:3adab04d42a6 136 printf("\nStarting...\r\n");
lucasfontenla 14:7cc41420a12c 137
lucasfontenla 17:55e6270adab5 138 modeStatus = botao;
lucasfontenla 17:55e6270adab5 139
lucasfontenla 14:7cc41420a12c 140 enable = 0;
lucasfontenla 12:801e58a7137c 141
lucasfontenla 15:4604a7ee9c77 142 endX.fall(&endX_press);
lucasfontenla 15:4604a7ee9c77 143 endX.rise(&endX_release);
lucasfontenla 5:0cbf491f703d 144
lucasfontenla 15:4604a7ee9c77 145 endY.fall(&endY_press);
lucasfontenla 15:4604a7ee9c77 146 endY.rise(&endY_release);
lucasfontenla 15:4604a7ee9c77 147
lucasfontenla 15:4604a7ee9c77 148 endZ.fall(&endZ_press);
lucasfontenla 15:4604a7ee9c77 149 endZ.rise(&endZ_release);
lucasfontenla 9:d771a576e4fc 150
lucasfontenla 18:174b4ff351b5 151 saveBtn.rise(&savePoint);
lucasfontenla 18:174b4ff351b5 152 startBtn.rise(&startSavedPoints);
lucasfontenla 18:174b4ff351b5 153
lucasfontenla 5:0cbf491f703d 154 if(modeStatus){
lucasfontenla 5:0cbf491f703d 155 //Código de JOG
lucasfontenla 16:3adab04d42a6 156 printf("JOG Selected\n\r");
lucasfontenla 5:0cbf491f703d 157 while(1){
lucasfontenla 5:0cbf491f703d 158 valX = joyX;
lucasfontenla 5:0cbf491f703d 159 valY = joyY;
lucasfontenla 5:0cbf491f703d 160
lucasfontenla 5:0cbf491f703d 161 valZUp = zUp;
lucasfontenla 5:0cbf491f703d 162 valZDwn = zDwn;
lucasfontenla 5:0cbf491f703d 163
lucasfontenla 5:0cbf491f703d 164 if(valX > 0.7){
lucasfontenla 14:7cc41420a12c 165 if(x_dir != x_plus){
lucasfontenla 14:7cc41420a12c 166 x_dir = x_plus;
lucasfontenla 5:0cbf491f703d 167 }
lucasfontenla 7:27ea14e58788 168 activeX = 1;
lucasfontenla 19:e69ff16f35aa 169 //totalX+=1;
lucasfontenla 5:0cbf491f703d 170 }
lucasfontenla 5:0cbf491f703d 171 else if(valX < 0.3){
lucasfontenla 14:7cc41420a12c 172 if(x_dir != x_minus){
lucasfontenla 14:7cc41420a12c 173 x_dir = x_minus;
lucasfontenla 5:0cbf491f703d 174 }
lucasfontenla 7:27ea14e58788 175 activeX = 1;
lucasfontenla 19:e69ff16f35aa 176 //totalX-=1;
lucasfontenla 5:0cbf491f703d 177 }
lucasfontenla 5:0cbf491f703d 178 else{
lucasfontenla 5:0cbf491f703d 179 activeX = 0;
lucasfontenla 4:48717b42eab2 180 }
lucasfontenla 5:0cbf491f703d 181
lucasfontenla 5:0cbf491f703d 182 //----------------------------------------------------------------------
lucasfontenla 5:0cbf491f703d 183
lucasfontenla 5:0cbf491f703d 184 if(valY > 0.7){
lucasfontenla 17:55e6270adab5 185 if(y_dir != y_plus){
lucasfontenla 17:55e6270adab5 186 y_dir = y_plus;
lucasfontenla 5:0cbf491f703d 187 }
lucasfontenla 7:27ea14e58788 188 activeY = 1;
lucasfontenla 19:e69ff16f35aa 189 //totalY+=1;
lucasfontenla 5:0cbf491f703d 190 }
lucasfontenla 5:0cbf491f703d 191 else if(valY < 0.3){
lucasfontenla 17:55e6270adab5 192 if(y_dir != y_minus){
lucasfontenla 17:55e6270adab5 193 y_dir = y_minus;
lucasfontenla 5:0cbf491f703d 194 }
lucasfontenla 7:27ea14e58788 195 activeY = 1;
lucasfontenla 19:e69ff16f35aa 196 //totalY-=1;
lucasfontenla 5:0cbf491f703d 197 }
lucasfontenla 5:0cbf491f703d 198 else{
lucasfontenla 5:0cbf491f703d 199 activeY = 0;
lucasfontenla 4:48717b42eab2 200 }
lucasfontenla 5:0cbf491f703d 201
lucasfontenla 5:0cbf491f703d 202 //----------------------------------------------------------------------
lucasfontenla 5:0cbf491f703d 203
lucasfontenla 5:0cbf491f703d 204 if(!valZUp && valZDwn){
lucasfontenla 9:d771a576e4fc 205 if(z_dir != z_minus){
lucasfontenla 15:4604a7ee9c77 206 z_dir = z_minus;
lucasfontenla 5:0cbf491f703d 207 }
lucasfontenla 15:4604a7ee9c77 208 activeZ = 1;
lucasfontenla 19:e69ff16f35aa 209 //totalZ-=1;
lucasfontenla 5:0cbf491f703d 210 }
lucasfontenla 5:0cbf491f703d 211 else if(!valZDwn && valZUp){
lucasfontenla 9:d771a576e4fc 212 if(z_dir != z_plus){
lucasfontenla 15:4604a7ee9c77 213 z_dir = z_plus;
lucasfontenla 5:0cbf491f703d 214 }
lucasfontenla 15:4604a7ee9c77 215 activeZ = 1;
lucasfontenla 19:e69ff16f35aa 216 //totalZ+=1;
lucasfontenla 5:0cbf491f703d 217 }
lucasfontenla 5:0cbf491f703d 218 else {
lucasfontenla 5:0cbf491f703d 219 activeZ = 0;
lucasfontenla 5:0cbf491f703d 220 }
lucasfontenla 5:0cbf491f703d 221
lucasfontenla 5:0cbf491f703d 222 //----------------------------------------------------------------------
lucasfontenla 5:0cbf491f703d 223
lucasfontenla 16:3adab04d42a6 224 move(ppsMax, x_dir, y_dir, z_dir, activeX, activeY, activeZ);
lucasfontenla 17:55e6270adab5 225 X = steps_to_distance(totalX, xPitch);
lucasfontenla 17:55e6270adab5 226 Y = steps_to_distance(totalY, yPitch);
lucasfontenla 17:55e6270adab5 227 Z = steps_to_distance(totalZ, zPitch);
lucasfontenla 7:27ea14e58788 228
lucasfontenla 7:27ea14e58788 229 if(contador > 10000){
lucasfontenla 15:4604a7ee9c77 230 printf("X: %.2f Y: %.2f Z: %.2f\r\n", X, Y, Z);
lucasfontenla 7:27ea14e58788 231 contador = 0;
lucasfontenla 7:27ea14e58788 232 }
lucasfontenla 7:27ea14e58788 233
lucasfontenla 7:27ea14e58788 234 contador+=1;
lucasfontenla 18:174b4ff351b5 235
lucasfontenla 18:174b4ff351b5 236 if(startSaved){
lucasfontenla 18:174b4ff351b5 237 break;
lucasfontenla 18:174b4ff351b5 238 }
lucasfontenla 4:48717b42eab2 239 }
lucasfontenla 18:174b4ff351b5 240
lucasfontenla 19:e69ff16f35aa 241 printf("\nStarting saved points\n\r");
lucasfontenla 19:e69ff16f35aa 242
lucasfontenla 19:e69ff16f35aa 243 x_distance_to_run = points[0][0] - points[0][saved-1];
lucasfontenla 19:e69ff16f35aa 244 y_distance_to_run = points[1][0] - points[1][saved-1];
lucasfontenla 19:e69ff16f35aa 245 z_distance_to_run = points[2][0] - points[2][saved-1];
lucasfontenla 19:e69ff16f35aa 246
lucasfontenla 19:e69ff16f35aa 247 printf("\nGoing to first point\n\r");
lucasfontenla 18:174b4ff351b5 248
lucasfontenla 19:e69ff16f35aa 249 if(x_distance_to_run < 0){
lucasfontenla 19:e69ff16f35aa 250 x_dir = x_minus;
lucasfontenla 19:e69ff16f35aa 251 x_distance_to_run = -1.0*x_distance_to_run;
lucasfontenla 19:e69ff16f35aa 252 } else { x_dir = x_plus; }
lucasfontenla 19:e69ff16f35aa 253
lucasfontenla 19:e69ff16f35aa 254 if(y_distance_to_run < 0){
lucasfontenla 19:e69ff16f35aa 255 y_dir = y_minus;
lucasfontenla 19:e69ff16f35aa 256 y_distance_to_run = -1.0*y_distance_to_run;
lucasfontenla 19:e69ff16f35aa 257 } else { y_dir = y_plus; }
lucasfontenla 19:e69ff16f35aa 258
lucasfontenla 19:e69ff16f35aa 259 if(z_distance_to_run < 0){
lucasfontenla 19:e69ff16f35aa 260 z_dir = z_minus;
lucasfontenla 19:e69ff16f35aa 261 z_distance_to_run = -1.0*z_distance_to_run;
lucasfontenla 19:e69ff16f35aa 262 } else { z_dir = z_plus; }
lucasfontenla 19:e69ff16f35aa 263
lucasfontenla 19:e69ff16f35aa 264 x_steps_to_run = distance_to_steps(x_distance_to_run, xPitch);
lucasfontenla 19:e69ff16f35aa 265 y_steps_to_run = distance_to_steps(y_distance_to_run, yPitch);
lucasfontenla 19:e69ff16f35aa 266 z_steps_to_run = distance_to_steps(z_distance_to_run, zPitch);
lucasfontenla 19:e69ff16f35aa 267
lucasfontenla 19:e69ff16f35aa 268 printf("deltaX: %.2f deltaY: %.2f deltaZ: %.2f\n\r", x_distance_to_run, y_distance_to_run, z_distance_to_run);
lucasfontenla 19:e69ff16f35aa 269
lucasfontenla 19:e69ff16f35aa 270 move(ppsMax, x_dir, y_dir, z_dir, x_steps_to_run, 0, 0);
lucasfontenla 19:e69ff16f35aa 271 move(ppsMax, x_dir, y_dir, z_dir, 0, y_steps_to_run, 0);
lucasfontenla 19:e69ff16f35aa 272 move(ppsMax, x_dir, y_dir, z_dir, 0, 0, z_steps_to_run);
lucasfontenla 19:e69ff16f35aa 273
lucasfontenla 19:e69ff16f35aa 274 wait(2);
lucasfontenla 19:e69ff16f35aa 275
lucasfontenla 19:e69ff16f35aa 276 printf("\nSaved points\n\r");
lucasfontenla 19:e69ff16f35aa 277
lucasfontenla 19:e69ff16f35aa 278 for(int i = 1; i < saved; i++){
lucasfontenla 19:e69ff16f35aa 279 x_distance_to_run = points[0][i]-points[0][i-1];
lucasfontenla 19:e69ff16f35aa 280 y_distance_to_run = points[1][i]-points[1][i-1];
lucasfontenla 19:e69ff16f35aa 281 z_distance_to_run = points[2][i]-points[2][i-1];
lucasfontenla 19:e69ff16f35aa 282
lucasfontenla 18:174b4ff351b5 283 printf("deltaX: %.2f deltaY: %.2f deltaZ: %.2f\n\r", x_distance_to_run, y_distance_to_run, z_distance_to_run);
lucasfontenla 18:174b4ff351b5 284
lucasfontenla 18:174b4ff351b5 285 if(x_distance_to_run < 0){
lucasfontenla 18:174b4ff351b5 286 x_dir = x_minus;
lucasfontenla 18:174b4ff351b5 287 x_distance_to_run = -1.0*x_distance_to_run;
lucasfontenla 18:174b4ff351b5 288 } else { x_dir = x_plus; }
lucasfontenla 18:174b4ff351b5 289
lucasfontenla 18:174b4ff351b5 290 if(y_distance_to_run < 0){
lucasfontenla 18:174b4ff351b5 291 y_dir = y_minus;
lucasfontenla 18:174b4ff351b5 292 y_distance_to_run = -1.0*y_distance_to_run;
lucasfontenla 18:174b4ff351b5 293 } else { y_dir = y_plus; }
lucasfontenla 18:174b4ff351b5 294
lucasfontenla 18:174b4ff351b5 295 if(z_distance_to_run < 0){
lucasfontenla 18:174b4ff351b5 296 z_dir = z_minus;
lucasfontenla 18:174b4ff351b5 297 z_distance_to_run = -1.0*z_distance_to_run;
lucasfontenla 18:174b4ff351b5 298 } else { z_dir = z_plus; }
lucasfontenla 18:174b4ff351b5 299
lucasfontenla 18:174b4ff351b5 300 x_steps_to_run = distance_to_steps(x_distance_to_run, xPitch);
lucasfontenla 18:174b4ff351b5 301 y_steps_to_run = distance_to_steps(y_distance_to_run, yPitch);
lucasfontenla 18:174b4ff351b5 302 z_steps_to_run = distance_to_steps(z_distance_to_run, zPitch);
lucasfontenla 18:174b4ff351b5 303
lucasfontenla 18:174b4ff351b5 304 move(ppsMax, x_dir, y_dir, z_dir, x_steps_to_run, 0, 0);
lucasfontenla 18:174b4ff351b5 305 move(ppsMax, x_dir, y_dir, z_dir, 0, y_steps_to_run, 0);
lucasfontenla 18:174b4ff351b5 306 move(ppsMax, x_dir, y_dir, z_dir, 0, 0, z_steps_to_run);
lucasfontenla 18:174b4ff351b5 307 }
lucasfontenla 18:174b4ff351b5 308
lucasfontenla 18:174b4ff351b5 309 printf("Done\n\r");
lucasfontenla 5:0cbf491f703d 310 }
lucasfontenla 5:0cbf491f703d 311
lucasfontenla 5:0cbf491f703d 312 else {
lucasfontenla 19:e69ff16f35aa 313 printf("Routine Selected\n\r");
lucasfontenla 18:174b4ff351b5 314
lucasfontenla 18:174b4ff351b5 315 /*
lucasfontenla 17:55e6270adab5 316 z_steps_to_run = distance_to_steps(20.0, zPitch);
lucasfontenla 17:55e6270adab5 317 move(ppsMax, x_plus, x_plus, z_minus, 0, 0, z_steps_to_run);
lucasfontenla 17:55e6270adab5 318
lucasfontenla 17:55e6270adab5 319 y_steps_to_run = distance_to_steps(50, yPitch);
lucasfontenla 17:55e6270adab5 320 move(ppsMax, x_plus, y_minus, z_plus, 0, y_steps_to_run, 0);
lucasfontenla 17:55e6270adab5 321
lucasfontenla 17:55e6270adab5 322 x_steps_to_run = distance_to_steps(50, xPitch);
lucasfontenla 17:55e6270adab5 323 move(ppsMax, x_plus, y_plus, z_plus, x_steps_to_run, 0, 0);
lucasfontenla 17:55e6270adab5 324
lucasfontenla 17:55e6270adab5 325 y_steps_to_run = distance_to_steps(50, yPitch);
lucasfontenla 17:55e6270adab5 326 move(ppsMax, x_plus, y_plus, z_plus, 0, y_steps_to_run, 0);
lucasfontenla 17:55e6270adab5 327
lucasfontenla 17:55e6270adab5 328 x_steps_to_run = distance_to_steps(50, xPitch);
lucasfontenla 17:55e6270adab5 329 move(ppsMax, x_minus, y_plus, z_plus, x_steps_to_run, 0, 0);
lucasfontenla 17:55e6270adab5 330
lucasfontenla 19:e69ff16f35aa 331 move(ppsMax, x_plus, x_plus, z_plus, 0, 0, z_steps_to_run); */
lucasfontenla 19:e69ff16f35aa 332
lucasfontenla 19:e69ff16f35aa 333
lucasfontenla 19:e69ff16f35aa 334 move(ppsMax, x_plus, y_plus, z_plus, 400, 0, 0);
lucasfontenla 19:e69ff16f35aa 335
lucasfontenla 19:e69ff16f35aa 336
lucasfontenla 17:55e6270adab5 337 printf("Done\n\r");
lucasfontenla 9:d771a576e4fc 338 }
lucasfontenla 9:d771a576e4fc 339 }
lucasfontenla 9:d771a576e4fc 340
lucasfontenla 9:d771a576e4fc 341 // FUNCTIONS ----------------------------------------------------------------
lucasfontenla 15:4604a7ee9c77 342 void endX_press(void){
lucasfontenla 15:4604a7ee9c77 343 printf("X - press\n\r");
lucasfontenla 15:4604a7ee9c77 344 if(dirX == x_plus){
lucasfontenla 16:3adab04d42a6 345 x_block_max = 1;
lucasfontenla 15:4604a7ee9c77 346 }
lucasfontenla 15:4604a7ee9c77 347 else {
lucasfontenla 16:3adab04d42a6 348 x_block_min = 1;
lucasfontenla 15:4604a7ee9c77 349 }
lucasfontenla 12:801e58a7137c 350 }
lucasfontenla 15:4604a7ee9c77 351 void endX_release(void){
lucasfontenla 15:4604a7ee9c77 352 printf("X - release\n\r");
lucasfontenla 16:3adab04d42a6 353 x_block_min = 0;
lucasfontenla 16:3adab04d42a6 354 x_block_max = 0;
lucasfontenla 12:801e58a7137c 355 }
lucasfontenla 16:3adab04d42a6 356
lucasfontenla 15:4604a7ee9c77 357 void endY_press(void){
lucasfontenla 16:3adab04d42a6 358 printf("Y - press\n\r");
lucasfontenla 16:3adab04d42a6 359 if(dirY == y_plus){
lucasfontenla 16:3adab04d42a6 360 y_block_max = 1;
lucasfontenla 16:3adab04d42a6 361 }
lucasfontenla 16:3adab04d42a6 362 else {
lucasfontenla 16:3adab04d42a6 363 y_block_min = 1;
lucasfontenla 16:3adab04d42a6 364 }
lucasfontenla 15:4604a7ee9c77 365 }
lucasfontenla 15:4604a7ee9c77 366 void endY_release(void){
lucasfontenla 15:4604a7ee9c77 367 printf("Y - release\n\r");
lucasfontenla 16:3adab04d42a6 368 y_block_min = 0;
lucasfontenla 16:3adab04d42a6 369 y_block_max = 0;
lucasfontenla 9:d771a576e4fc 370 }
lucasfontenla 9:d771a576e4fc 371
lucasfontenla 15:4604a7ee9c77 372 void endZ_press(void){
lucasfontenla 15:4604a7ee9c77 373 printf("Z - press\n\r");
lucasfontenla 16:3adab04d42a6 374 if(dirZ == z_plus){
lucasfontenla 16:3adab04d42a6 375 z_block_max = 1;
lucasfontenla 16:3adab04d42a6 376 }
lucasfontenla 16:3adab04d42a6 377 else {
lucasfontenla 16:3adab04d42a6 378 z_block_min = 1;
lucasfontenla 16:3adab04d42a6 379 }
lucasfontenla 9:d771a576e4fc 380 }
lucasfontenla 15:4604a7ee9c77 381 void endZ_release(void){
lucasfontenla 15:4604a7ee9c77 382 printf("Z - release\n\r");
lucasfontenla 16:3adab04d42a6 383 z_block_min = 0;
lucasfontenla 16:3adab04d42a6 384 z_block_max = 0;
lucasfontenla 9:d771a576e4fc 385 }
lucasfontenla 7:27ea14e58788 386
lucasfontenla 5:0cbf491f703d 387 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 388 float time = 1.0/pps/2.0;
lucasfontenla 5:0cbf491f703d 389
lucasfontenla 5:0cbf491f703d 390 dirX = x_dir;
lucasfontenla 5:0cbf491f703d 391 dirY = y_dir;
lucasfontenla 5:0cbf491f703d 392 dirZ = z_dir;
lucasfontenla 5:0cbf491f703d 393
lucasfontenla 16:3adab04d42a6 394 // X
lucasfontenla 16:3adab04d42a6 395 if((x_dir == x_plus) && x_block_max){
lucasfontenla 16:3adab04d42a6 396 x_step = 0;
lucasfontenla 16:3adab04d42a6 397 }
lucasfontenla 16:3adab04d42a6 398 if((x_dir == x_minus) && x_block_min){
lucasfontenla 12:801e58a7137c 399 x_step = 0;
lucasfontenla 12:801e58a7137c 400 }
lucasfontenla 12:801e58a7137c 401
lucasfontenla 16:3adab04d42a6 402 // Y
lucasfontenla 16:3adab04d42a6 403 if((y_dir == y_plus) && y_block_max){
lucasfontenla 16:3adab04d42a6 404 y_step = 0;
lucasfontenla 16:3adab04d42a6 405 }
lucasfontenla 16:3adab04d42a6 406 if((y_dir == y_minus) && y_block_min){
lucasfontenla 11:0e22a6fc7f87 407 y_step = 0;
lucasfontenla 11:0e22a6fc7f87 408 }
lucasfontenla 16:3adab04d42a6 409
lucasfontenla 16:3adab04d42a6 410 // Z
lucasfontenla 16:3adab04d42a6 411 if((z_dir == z_plus) && z_block_max){
lucasfontenla 16:3adab04d42a6 412 z_step = 0;
lucasfontenla 16:3adab04d42a6 413 }
lucasfontenla 16:3adab04d42a6 414 if((z_dir == z_minus) && z_block_min){
lucasfontenla 15:4604a7ee9c77 415 z_step = 0;
lucasfontenla 11:0e22a6fc7f87 416 }
lucasfontenla 11:0e22a6fc7f87 417
lucasfontenla 5:0cbf491f703d 418 int max_val;
lucasfontenla 5:0cbf491f703d 419
lucasfontenla 6:af32c68ff4cf 420 // maior número de passos -------
lucasfontenla 5:0cbf491f703d 421 if(x_step > y_step){
lucasfontenla 5:0cbf491f703d 422 if(x_step > z_step){
lucasfontenla 5:0cbf491f703d 423 max_val = x_step;
lucasfontenla 4:48717b42eab2 424 }
lucasfontenla 4:48717b42eab2 425 else {
lucasfontenla 5:0cbf491f703d 426 max_val = z_step;
lucasfontenla 4:48717b42eab2 427 }
lucasfontenla 5:0cbf491f703d 428 } else if(y_step > z_step){
lucasfontenla 5:0cbf491f703d 429 max_val = y_step;
lucasfontenla 5:0cbf491f703d 430 }
lucasfontenla 5:0cbf491f703d 431 else {
lucasfontenla 5:0cbf491f703d 432 max_val = z_step;
lucasfontenla 5:0cbf491f703d 433 }
lucasfontenla 5:0cbf491f703d 434
lucasfontenla 5:0cbf491f703d 435 for(int i = 0; i < max_val; i++){
lucasfontenla 6:af32c68ff4cf 436 if(i >= x_step){
lucasfontenla 5:0cbf491f703d 437 stepX = 0;
lucasfontenla 5:0cbf491f703d 438 }
lucasfontenla 5:0cbf491f703d 439 else { stepX = 1; }
lucasfontenla 4:48717b42eab2 440
lucasfontenla 6:af32c68ff4cf 441 if(i >= y_step){
lucasfontenla 5:0cbf491f703d 442 stepY = 0;
lucasfontenla 5:0cbf491f703d 443 }
lucasfontenla 11:0e22a6fc7f87 444 else {
lucasfontenla 11:0e22a6fc7f87 445 stepY = 1;}
lucasfontenla 5:0cbf491f703d 446
lucasfontenla 6:af32c68ff4cf 447 if(i >= z_step){
lucasfontenla 5:0cbf491f703d 448 stepZ = 0;
lucasfontenla 5:0cbf491f703d 449 }
lucasfontenla 5:0cbf491f703d 450 else { stepZ = 1; }
lucasfontenla 5:0cbf491f703d 451
lucasfontenla 5:0cbf491f703d 452 wait(time);
lucasfontenla 4:48717b42eab2 453 stepX = 0;
lucasfontenla 4:48717b42eab2 454 stepY = 0;
lucasfontenla 4:48717b42eab2 455 stepZ = 0;
lucasfontenla 5:0cbf491f703d 456 wait(time);
lucasfontenla 4:48717b42eab2 457 }
lucasfontenla 17:55e6270adab5 458
lucasfontenla 17:55e6270adab5 459 if(dirX == x_minus){
lucasfontenla 17:55e6270adab5 460 totalX-=x_step;
lucasfontenla 17:55e6270adab5 461 }
lucasfontenla 17:55e6270adab5 462 else { totalX+=x_step; }
lucasfontenla 17:55e6270adab5 463
lucasfontenla 17:55e6270adab5 464 if(dirY == y_minus){
lucasfontenla 17:55e6270adab5 465 totalY-=y_step;
lucasfontenla 17:55e6270adab5 466 } else { totalY+=y_step; }
lucasfontenla 17:55e6270adab5 467
lucasfontenla 17:55e6270adab5 468 if(dirZ == z_minus){
lucasfontenla 17:55e6270adab5 469 totalZ-=z_step;
lucasfontenla 17:55e6270adab5 470 } else { totalZ+=z_step; }
lucasfontenla 5:0cbf491f703d 471 }
lucasfontenla 17:55e6270adab5 472
lucasfontenla 12:801e58a7137c 473 void zeroX(int pps){
lucasfontenla 12:801e58a7137c 474 float time = 1.0/pps/2.0;
lucasfontenla 12:801e58a7137c 475
lucasfontenla 17:55e6270adab5 476 if(dirX != x_minus){
lucasfontenla 17:55e6270adab5 477 dirX = x_minus;
lucasfontenla 17:55e6270adab5 478 }
lucasfontenla 17:55e6270adab5 479
lucasfontenla 17:55e6270adab5 480 while(!x_block_min){
lucasfontenla 12:801e58a7137c 481 stepX = 1;
lucasfontenla 12:801e58a7137c 482 wait(time);
lucasfontenla 12:801e58a7137c 483 stepX = 0;
lucasfontenla 12:801e58a7137c 484 wait(time);
lucasfontenla 12:801e58a7137c 485 }
lucasfontenla 12:801e58a7137c 486
lucasfontenla 12:801e58a7137c 487 printf("X zero\n\r");
lucasfontenla 12:801e58a7137c 488
lucasfontenla 17:55e6270adab5 489 totalX = 0;
lucasfontenla 12:801e58a7137c 490 }
lucasfontenla 11:0e22a6fc7f87 491 void zeroY(int pps){
lucasfontenla 11:0e22a6fc7f87 492 float time = 1.0/pps/2.0;
lucasfontenla 11:0e22a6fc7f87 493
lucasfontenla 17:55e6270adab5 494 if(dirY != y_minus){
lucasfontenla 17:55e6270adab5 495 dirY = y_minus;
lucasfontenla 17:55e6270adab5 496 }
lucasfontenla 17:55e6270adab5 497
lucasfontenla 17:55e6270adab5 498 while(!y_block_min){
lucasfontenla 11:0e22a6fc7f87 499 stepY = 1;
lucasfontenla 11:0e22a6fc7f87 500 wait(time);
lucasfontenla 11:0e22a6fc7f87 501 stepY = 0;
lucasfontenla 11:0e22a6fc7f87 502 wait(time);
lucasfontenla 11:0e22a6fc7f87 503 }
lucasfontenla 11:0e22a6fc7f87 504
lucasfontenla 11:0e22a6fc7f87 505 printf("Y zero\n\r");
lucasfontenla 11:0e22a6fc7f87 506
lucasfontenla 17:55e6270adab5 507 totalY = 0;
lucasfontenla 17:55e6270adab5 508 }
lucasfontenla 11:0e22a6fc7f87 509
lucasfontenla 17:55e6270adab5 510 float steps_to_distance(int steps, float pitch){
lucasfontenla 7:27ea14e58788 511 float delta_S = (((float)steps)/steps_by_rotation)*pitch;
lucasfontenla 7:27ea14e58788 512 return delta_S;
lucasfontenla 7:27ea14e58788 513 }
lucasfontenla 17:55e6270adab5 514
lucasfontenla 17:55e6270adab5 515 int distance_to_steps(float distance, float pitch){
lucasfontenla 17:55e6270adab5 516 int steps = (int)(distance*steps_by_rotation/pitch);
lucasfontenla 17:55e6270adab5 517 return steps;
lucasfontenla 17:55e6270adab5 518 }
lucasfontenla 17:55e6270adab5 519
lucasfontenla 17:55e6270adab5 520 void printDistance(void){
lucasfontenla 17:55e6270adab5 521 X = steps_to_distance(totalX, xPitch);
lucasfontenla 17:55e6270adab5 522 Y = steps_to_distance(totalY, yPitch);
lucasfontenla 17:55e6270adab5 523 Z = steps_to_distance(totalZ, zPitch);
lucasfontenla 17:55e6270adab5 524 printf("X: %.2f Y: %.2f Z: %.2f\r\n", X, Y, Z);
lucasfontenla 18:174b4ff351b5 525 }
lucasfontenla 18:174b4ff351b5 526
lucasfontenla 18:174b4ff351b5 527 void savePoint(void){
lucasfontenla 18:174b4ff351b5 528 printf("\nSave selected\n\r");
lucasfontenla 18:174b4ff351b5 529 if(saved >= maxPoints){
lucasfontenla 18:174b4ff351b5 530 printf("Max points reached\n\r");
lucasfontenla 18:174b4ff351b5 531 } else{
lucasfontenla 18:174b4ff351b5 532 X = steps_to_distance(totalX, xPitch);
lucasfontenla 18:174b4ff351b5 533 Y = steps_to_distance(totalY, yPitch);
lucasfontenla 18:174b4ff351b5 534 Z = steps_to_distance(totalZ, zPitch);
lucasfontenla 18:174b4ff351b5 535
lucasfontenla 18:174b4ff351b5 536 points[0][saved] = X;
lucasfontenla 18:174b4ff351b5 537 points[1][saved] = Y;
lucasfontenla 18:174b4ff351b5 538 points[2][saved] = Z;
lucasfontenla 18:174b4ff351b5 539
lucasfontenla 18:174b4ff351b5 540 saved+=1;
lucasfontenla 18:174b4ff351b5 541 }
lucasfontenla 18:174b4ff351b5 542 // save code
lucasfontenla 19:e69ff16f35aa 543 printf("Saved --> X: %.2f Y: %.2f Z: %.2f\r\n", X, Y, Z);
lucasfontenla 19:e69ff16f35aa 544 pontos_restantes = maxPoints-saved;
lucasfontenla 19:e69ff16f35aa 545 printf("Pontos restantes: %d\n\n\r", pontos_restantes);
lucasfontenla 18:174b4ff351b5 546 }
lucasfontenla 18:174b4ff351b5 547
lucasfontenla 18:174b4ff351b5 548 void startSavedPoints(void){
lucasfontenla 18:174b4ff351b5 549 startSaved = 1;
lucasfontenla 17:55e6270adab5 550 }