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@20:1d98a6f51dee, 2018-05-09 (annotated)

Committer:
lucasfontenla
Date:
Wed May 09 11:53:46 2018 +0000
Revision:
20:1d98a6f51dee
Parent:
19:e69ff16f35aa
Child:
21:c6c33381fc5f
Ponto a ponto (sem interpola??o) funcionando 100%. Menu para reload dos pontos ou restart do JOG tamb?m;

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