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@21:c6c33381fc5f, 2018-05-09 (annotated)

Committer:
lucasfontenla
Date:
Wed May 09 14:08:56 2018 +0000
Revision:
21:c6c33381fc5f
Parent:
20:1d98a6f51dee
Child:
22:000890b38b32
Coment?rios adicionados;

Who changed what in which revision?

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