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@18:174b4ff351b5, 2018-05-08 (annotated)

Committer:
lucasfontenla
Date:
Tue May 08 15:40:39 2018 +0000
Revision:
18:174b4ff351b5
Parent:
17:55e6270adab5
Child:
19:e69ff16f35aa
Registra pontos e executa funcionando. Problemas com o c?lculo da dist?ncia.

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