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@32:5bd3f0d073ac, 2018-06-08 (annotated)

Committer:
lucasfontenla
Date:
Fri Jun 08 20:57:57 2018 +0000
Revision:
32:5bd3f0d073ac
Parent:
31:7896ac3a5a1c
Child:
33:6ad0d169b154
atualiza??o;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lucasfontenla 21:c6c33381fc5f 1 // LIBs ------------------------------------------------------------------------------------
lucasfontenla 0:cc562f78f889 2 #include "mbed.h"
lucasfontenla 22:000890b38b32 3 #include "string"
lucasfontenla 24:92f612ca218b 4 #include "classes.h"
lucasfontenla 0:cc562f78f889 5
lucasfontenla 5:0cbf491f703d 6 // MACHINE SETUP ---------------------------------------------------------------------------
lucasfontenla 23:d64d37561b4a 7 Timer t;
lucasfontenla 24:92f612ca218b 8 IHM ihm_class;
lucasfontenla 5:0cbf491f703d 9
lucasfontenla 5:0cbf491f703d 10 // drivers output signal
lucasfontenla 14:7cc41420a12c 11 DigitalOut enable(D2);
lucasfontenla 14:7cc41420a12c 12
lucasfontenla 4:48717b42eab2 13 DigitalOut dirX(D3);
lucasfontenla 15:4604a7ee9c77 14 DigitalOut stepX(D4);
lucasfontenla 4:48717b42eab2 15
lucasfontenla 15:4604a7ee9c77 16 DigitalOut dirY(D5);
lucasfontenla 15:4604a7ee9c77 17 DigitalOut stepY(D6);
lucasfontenla 4:48717b42eab2 18
lucasfontenla 15:4604a7ee9c77 19 DigitalOut dirZ(D7);
lucasfontenla 15:4604a7ee9c77 20 DigitalOut stepZ(D8);
lucasfontenla 4:48717b42eab2 21
lucasfontenla 26:f891ff6beb33 22 DigitalOut ledXY(D10);
lucasfontenla 26:f891ff6beb33 23 DigitalOut ledZ(D11);
lucasfontenla 26:f891ff6beb33 24
lucasfontenla 30:27838f6fdfd6 25 DigitalOut glueValve(D12);
lucasfontenla 30:27838f6fdfd6 26
lucasfontenla 5:0cbf491f703d 27 // hardware input signal
lucasfontenla 5:0cbf491f703d 28 // end-of-stroke sensors
lucasfontenla 32:5bd3f0d073ac 29 InterruptIn endX(A3);
lucasfontenla 32:5bd3f0d073ac 30 InterruptIn endY(A4);
lucasfontenla 32:5bd3f0d073ac 31 InterruptIn endZ(A5);
lucasfontenla 5:0cbf491f703d 32
lucasfontenla 23:d64d37561b4a 33 // JOG Controls
lucasfontenla 9:d771a576e4fc 34 AnalogIn joyX(A0);
lucasfontenla 9:d771a576e4fc 35 AnalogIn joyY(A1);
lucasfontenla 4:48717b42eab2 36
lucasfontenla 32:5bd3f0d073ac 37 InterruptIn joyClick(A2);
lucasfontenla 4:48717b42eab2 38
lucasfontenla 5:0cbf491f703d 39 // variables definition
lucasfontenla 23:d64d37561b4a 40 int modeStatus = 1;
lucasfontenla 5:0cbf491f703d 41
lucasfontenla 4:48717b42eab2 42 float valX;
lucasfontenla 4:48717b42eab2 43 float valY;
lucasfontenla 26:f891ff6beb33 44
lucasfontenla 26:f891ff6beb33 45 int toggle = 0;
lucasfontenla 4:48717b42eab2 46
lucasfontenla 4:48717b42eab2 47 int activeX = 1;
lucasfontenla 4:48717b42eab2 48 int activeY = 1;
lucasfontenla 4:48717b42eab2 49 int activeZ = 1;
lucasfontenla 4:48717b42eab2 50
lucasfontenla 5:0cbf491f703d 51 int x_dir;
lucasfontenla 5:0cbf491f703d 52 int y_dir;
lucasfontenla 5:0cbf491f703d 53 int z_dir;
lucasfontenla 5:0cbf491f703d 54
lucasfontenla 13:0f385bfe3e0d 55 int x_plus = 0;
lucasfontenla 13:0f385bfe3e0d 56 int x_minus = 1;
lucasfontenla 27:3bbc354adea6 57 int y_plus = 1;
lucasfontenla 27:3bbc354adea6 58 int y_minus = 0;
lucasfontenla 16:3adab04d42a6 59 int z_plus = 0;
lucasfontenla 16:3adab04d42a6 60 int z_minus = 1;
lucasfontenla 9:d771a576e4fc 61
lucasfontenla 15:4604a7ee9c77 62 int x_limit = 0;
lucasfontenla 16:3adab04d42a6 63 int x_block_max = 0;
lucasfontenla 16:3adab04d42a6 64 int x_block_min = 0;
lucasfontenla 12:801e58a7137c 65
lucasfontenla 15:4604a7ee9c77 66 int y_limit = 0;
lucasfontenla 16:3adab04d42a6 67 int y_block_max = 0;
lucasfontenla 16:3adab04d42a6 68 int y_block_min = 0;
lucasfontenla 15:4604a7ee9c77 69
lucasfontenla 15:4604a7ee9c77 70 int z_limit = 0;
lucasfontenla 16:3adab04d42a6 71 int z_block_max = 0;
lucasfontenla 16:3adab04d42a6 72 int z_block_min = 0;
lucasfontenla 9:d771a576e4fc 73
lucasfontenla 32:5bd3f0d073ac 74 float ppsMax = 1500.0;
lucasfontenla 30:27838f6fdfd6 75 float glue_time = 1.0;
lucasfontenla 5:0cbf491f703d 76
lucasfontenla 7:27ea14e58788 77 int totalX = 0;
lucasfontenla 7:27ea14e58788 78 int totalY = 0;
lucasfontenla 15:4604a7ee9c77 79 int totalZ = 0;
lucasfontenla 7:27ea14e58788 80
lucasfontenla 17:55e6270adab5 81 int x_steps_to_run;
lucasfontenla 17:55e6270adab5 82 int y_steps_to_run;
lucasfontenla 17:55e6270adab5 83 int z_steps_to_run;
lucasfontenla 17:55e6270adab5 84
lucasfontenla 31:7896ac3a5a1c 85 int cleaning_glue_status = 0;
lucasfontenla 31:7896ac3a5a1c 86
lucasfontenla 18:174b4ff351b5 87 float x_distance_to_run;
lucasfontenla 18:174b4ff351b5 88 float y_distance_to_run;
lucasfontenla 18:174b4ff351b5 89 float z_distance_to_run;
lucasfontenla 18:174b4ff351b5 90
lucasfontenla 7:27ea14e58788 91 float X = 0;
lucasfontenla 7:27ea14e58788 92 float Y = 0;
lucasfontenla 15:4604a7ee9c77 93 float Z = 0;
lucasfontenla 7:27ea14e58788 94
lucasfontenla 9:d771a576e4fc 95 int endX1Status;
lucasfontenla 9:d771a576e4fc 96 int endX2Status;
lucasfontenla 9:d771a576e4fc 97
lucasfontenla 7:27ea14e58788 98 // machine parameters
lucasfontenla 8:de64672c240f 99 float xPitch = 3.0;
lucasfontenla 8:de64672c240f 100 float yPitch = 3.0;
lucasfontenla 32:5bd3f0d073ac 101 float zPitch = 6.0;
lucasfontenla 7:27ea14e58788 102
lucasfontenla 7:27ea14e58788 103 float motor_steps_by_rotation = 200.0;
lucasfontenla 7:27ea14e58788 104 float step_mode = 0.5;
lucasfontenla 7:27ea14e58788 105 float steps_by_rotation = motor_steps_by_rotation/step_mode;
lucasfontenla 7:27ea14e58788 106
lucasfontenla 23:d64d37561b4a 107 int kill_jog;
lucasfontenla 23:d64d37561b4a 108
lucasfontenla 21:c6c33381fc5f 109 // PROTOTYPE FUNCTIONS -------------------------------------------------------------------------------
lucasfontenla 23:d64d37561b4a 110 void jog(void);
lucasfontenla 23:d64d37561b4a 111 void killJog(void);
lucasfontenla 23:d64d37561b4a 112
lucasfontenla 23:d64d37561b4a 113 void automatic_run(int);
lucasfontenla 23:d64d37561b4a 114
lucasfontenla 5:0cbf491f703d 115 void move(int pps, int x_dir, int y_dir, int z_dir, int x_step, int y_step, int z_step);
lucasfontenla 27:3bbc354adea6 116 void moveZ(int, int);
lucasfontenla 17:55e6270adab5 117
lucasfontenla 26:f891ff6beb33 118 float steps_to_distance(int, float);
lucasfontenla 26:f891ff6beb33 119 int distance_to_steps(float, float);
lucasfontenla 7:27ea14e58788 120
lucasfontenla 15:4604a7ee9c77 121 void endX_press(void);
lucasfontenla 15:4604a7ee9c77 122 void endX_release(void);
lucasfontenla 11:0e22a6fc7f87 123
lucasfontenla 15:4604a7ee9c77 124 void endY_press(void);
lucasfontenla 15:4604a7ee9c77 125 void endY_release(void);
lucasfontenla 15:4604a7ee9c77 126
lucasfontenla 15:4604a7ee9c77 127 void endZ_press(void);
lucasfontenla 15:4604a7ee9c77 128 void endZ_release(void);
lucasfontenla 9:d771a576e4fc 129
lucasfontenla 26:f891ff6beb33 130 void goMachineHome(int);
lucasfontenla 26:f891ff6beb33 131 void zeroX(int);
lucasfontenla 26:f891ff6beb33 132 void zeroY(int);
lucasfontenla 26:f891ff6beb33 133 void zeroZ(int);
lucasfontenla 11:0e22a6fc7f87 134
lucasfontenla 27:3bbc354adea6 135 void virtualZero(void);
lucasfontenla 27:3bbc354adea6 136
lucasfontenla 18:174b4ff351b5 137 void savePoint(void);
lucasfontenla 18:174b4ff351b5 138 void startSavedPoints(void);
lucasfontenla 18:174b4ff351b5 139
lucasfontenla 17:55e6270adab5 140 void printDistance(void);
lucasfontenla 17:55e6270adab5 141
lucasfontenla 23:d64d37561b4a 142 void readSerial(void);
lucasfontenla 22:000890b38b32 143
lucasfontenla 26:f891ff6beb33 144 void joystickClick(void);
lucasfontenla 26:f891ff6beb33 145
lucasfontenla 30:27838f6fdfd6 146 void linear_interpolation(int x1, int y1, int x2, int y2, float speed);
lucasfontenla 27:3bbc354adea6 147 void circular_interpolation(int, int, int);
lucasfontenla 27:3bbc354adea6 148
lucasfontenla 27:3bbc354adea6 149 void test_direction(void);
lucasfontenla 26:f891ff6beb33 150
lucasfontenla 30:27838f6fdfd6 151 void cancelProgram(void);
lucasfontenla 30:27838f6fdfd6 152
lucasfontenla 32:5bd3f0d073ac 153 void moving_test(void);
lucasfontenla 32:5bd3f0d073ac 154
lucasfontenla 31:7896ac3a5a1c 155 void clean_glue(void);
lucasfontenla 31:7896ac3a5a1c 156
lucasfontenla 21:c6c33381fc5f 157 // MAIN PROGRAM ----------------------------------------------------------------------------------------------
lucasfontenla 1:b2e395e50a45 158 int main(){
lucasfontenla 31:7896ac3a5a1c 159 ihm.baud(9600);
lucasfontenla 16:3adab04d42a6 160 printf("\nStarting...\r\n");
lucasfontenla 14:7cc41420a12c 161
lucasfontenla 32:5bd3f0d073ac 162 enable = 1;
lucasfontenla 32:5bd3f0d073ac 163
lucasfontenla 24:92f612ca218b 164 t.start();
lucasfontenla 32:5bd3f0d073ac 165 /*
lucasfontenla 32:5bd3f0d073ac 166 enable = 0;
lucasfontenla 32:5bd3f0d073ac 167 while (1){
lucasfontenla 32:5bd3f0d073ac 168 moving_test();
lucasfontenla 32:5bd3f0d073ac 169 }*/
lucasfontenla 32:5bd3f0d073ac 170
lucasfontenla 32:5bd3f0d073ac 171 /*
lucasfontenla 32:5bd3f0d073ac 172 while(1){
lucasfontenla 32:5bd3f0d073ac 173 glueValve = 1;
lucasfontenla 32:5bd3f0d073ac 174 wait(1);
lucasfontenla 32:5bd3f0d073ac 175 glueValve = 0;
lucasfontenla 32:5bd3f0d073ac 176 wait(1);
lucasfontenla 32:5bd3f0d073ac 177 }*/
lucasfontenla 22:000890b38b32 178
lucasfontenla 21:c6c33381fc5f 179 // interrupções de fim de curso
lucasfontenla 22:000890b38b32 180 /*
lucasfontenla 15:4604a7ee9c77 181 endX.fall(&endX_press);
lucasfontenla 15:4604a7ee9c77 182 endX.rise(&endX_release);
lucasfontenla 5:0cbf491f703d 183
lucasfontenla 15:4604a7ee9c77 184 endY.fall(&endY_press);
lucasfontenla 15:4604a7ee9c77 185 endY.rise(&endY_release);
lucasfontenla 15:4604a7ee9c77 186
lucasfontenla 15:4604a7ee9c77 187 endZ.fall(&endZ_press);
lucasfontenla 23:d64d37561b4a 188 endZ.rise(&endZ_release);*/
lucasfontenla 20:1d98a6f51dee 189
lucasfontenla 30:27838f6fdfd6 190 joyClick.fall(&joystickClick);
lucasfontenla 27:3bbc354adea6 191
lucasfontenla 23:d64d37561b4a 192 while(1){
lucasfontenla 23:d64d37561b4a 193 if(ihm.readable()){
lucasfontenla 23:d64d37561b4a 194 readSerial();
lucasfontenla 26:f891ff6beb33 195 }
lucasfontenla 26:f891ff6beb33 196
lucasfontenla 27:3bbc354adea6 197 }
lucasfontenla 9:d771a576e4fc 198 }
lucasfontenla 9:d771a576e4fc 199
lucasfontenla 9:d771a576e4fc 200 // FUNCTIONS ----------------------------------------------------------------
lucasfontenla 23:d64d37561b4a 201
lucasfontenla 15:4604a7ee9c77 202 void endX_press(void){
lucasfontenla 15:4604a7ee9c77 203 printf("X - press\n\r");
lucasfontenla 15:4604a7ee9c77 204 if(dirX == x_plus){
lucasfontenla 16:3adab04d42a6 205 x_block_max = 1;
lucasfontenla 15:4604a7ee9c77 206 }
lucasfontenla 15:4604a7ee9c77 207 else {
lucasfontenla 16:3adab04d42a6 208 x_block_min = 1;
lucasfontenla 15:4604a7ee9c77 209 }
lucasfontenla 12:801e58a7137c 210 }
lucasfontenla 15:4604a7ee9c77 211 void endX_release(void){
lucasfontenla 15:4604a7ee9c77 212 printf("X - release\n\r");
lucasfontenla 16:3adab04d42a6 213 x_block_min = 0;
lucasfontenla 16:3adab04d42a6 214 x_block_max = 0;
lucasfontenla 12:801e58a7137c 215 }
lucasfontenla 16:3adab04d42a6 216
lucasfontenla 15:4604a7ee9c77 217 void endY_press(void){
lucasfontenla 16:3adab04d42a6 218 printf("Y - press\n\r");
lucasfontenla 16:3adab04d42a6 219 if(dirY == y_plus){
lucasfontenla 16:3adab04d42a6 220 y_block_max = 1;
lucasfontenla 16:3adab04d42a6 221 }
lucasfontenla 16:3adab04d42a6 222 else {
lucasfontenla 16:3adab04d42a6 223 y_block_min = 1;
lucasfontenla 16:3adab04d42a6 224 }
lucasfontenla 15:4604a7ee9c77 225 }
lucasfontenla 15:4604a7ee9c77 226 void endY_release(void){
lucasfontenla 15:4604a7ee9c77 227 printf("Y - release\n\r");
lucasfontenla 16:3adab04d42a6 228 y_block_min = 0;
lucasfontenla 16:3adab04d42a6 229 y_block_max = 0;
lucasfontenla 9:d771a576e4fc 230 }
lucasfontenla 9:d771a576e4fc 231
lucasfontenla 15:4604a7ee9c77 232 void endZ_press(void){
lucasfontenla 15:4604a7ee9c77 233 printf("Z - press\n\r");
lucasfontenla 16:3adab04d42a6 234 if(dirZ == z_plus){
lucasfontenla 16:3adab04d42a6 235 z_block_max = 1;
lucasfontenla 16:3adab04d42a6 236 }
lucasfontenla 16:3adab04d42a6 237 else {
lucasfontenla 16:3adab04d42a6 238 z_block_min = 1;
lucasfontenla 16:3adab04d42a6 239 }
lucasfontenla 9:d771a576e4fc 240 }
lucasfontenla 15:4604a7ee9c77 241 void endZ_release(void){
lucasfontenla 15:4604a7ee9c77 242 printf("Z - release\n\r");
lucasfontenla 16:3adab04d42a6 243 z_block_min = 0;
lucasfontenla 16:3adab04d42a6 244 z_block_max = 0;
lucasfontenla 9:d771a576e4fc 245 }
lucasfontenla 7:27ea14e58788 246
lucasfontenla 5:0cbf491f703d 247 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 248 float time = 1.0/pps/2.0;
lucasfontenla 5:0cbf491f703d 249
lucasfontenla 5:0cbf491f703d 250 dirX = x_dir;
lucasfontenla 5:0cbf491f703d 251 dirY = y_dir;
lucasfontenla 5:0cbf491f703d 252 dirZ = z_dir;
lucasfontenla 5:0cbf491f703d 253
lucasfontenla 16:3adab04d42a6 254 // X
lucasfontenla 16:3adab04d42a6 255 if((x_dir == x_plus) && x_block_max){
lucasfontenla 16:3adab04d42a6 256 x_step = 0;
lucasfontenla 16:3adab04d42a6 257 }
lucasfontenla 16:3adab04d42a6 258 if((x_dir == x_minus) && x_block_min){
lucasfontenla 12:801e58a7137c 259 x_step = 0;
lucasfontenla 12:801e58a7137c 260 }
lucasfontenla 12:801e58a7137c 261
lucasfontenla 16:3adab04d42a6 262 // Y
lucasfontenla 16:3adab04d42a6 263 if((y_dir == y_plus) && y_block_max){
lucasfontenla 16:3adab04d42a6 264 y_step = 0;
lucasfontenla 16:3adab04d42a6 265 }
lucasfontenla 16:3adab04d42a6 266 if((y_dir == y_minus) && y_block_min){
lucasfontenla 11:0e22a6fc7f87 267 y_step = 0;
lucasfontenla 11:0e22a6fc7f87 268 }
lucasfontenla 16:3adab04d42a6 269
lucasfontenla 16:3adab04d42a6 270 // Z
lucasfontenla 16:3adab04d42a6 271 if((z_dir == z_plus) && z_block_max){
lucasfontenla 16:3adab04d42a6 272 z_step = 0;
lucasfontenla 16:3adab04d42a6 273 }
lucasfontenla 16:3adab04d42a6 274 if((z_dir == z_minus) && z_block_min){
lucasfontenla 15:4604a7ee9c77 275 z_step = 0;
lucasfontenla 11:0e22a6fc7f87 276 }
lucasfontenla 11:0e22a6fc7f87 277
lucasfontenla 5:0cbf491f703d 278 int max_val;
lucasfontenla 5:0cbf491f703d 279
lucasfontenla 6:af32c68ff4cf 280 // maior número de passos -------
lucasfontenla 5:0cbf491f703d 281 if(x_step > y_step){
lucasfontenla 5:0cbf491f703d 282 if(x_step > z_step){
lucasfontenla 5:0cbf491f703d 283 max_val = x_step;
lucasfontenla 4:48717b42eab2 284 }
lucasfontenla 4:48717b42eab2 285 else {
lucasfontenla 5:0cbf491f703d 286 max_val = z_step;
lucasfontenla 4:48717b42eab2 287 }
lucasfontenla 5:0cbf491f703d 288 } else if(y_step > z_step){
lucasfontenla 5:0cbf491f703d 289 max_val = y_step;
lucasfontenla 5:0cbf491f703d 290 }
lucasfontenla 5:0cbf491f703d 291 else {
lucasfontenla 5:0cbf491f703d 292 max_val = z_step;
lucasfontenla 5:0cbf491f703d 293 }
lucasfontenla 5:0cbf491f703d 294
lucasfontenla 5:0cbf491f703d 295 for(int i = 0; i < max_val; i++){
lucasfontenla 6:af32c68ff4cf 296 if(i >= x_step){
lucasfontenla 5:0cbf491f703d 297 stepX = 0;
lucasfontenla 5:0cbf491f703d 298 }
lucasfontenla 5:0cbf491f703d 299 else { stepX = 1; }
lucasfontenla 4:48717b42eab2 300
lucasfontenla 6:af32c68ff4cf 301 if(i >= y_step){
lucasfontenla 5:0cbf491f703d 302 stepY = 0;
lucasfontenla 5:0cbf491f703d 303 }
lucasfontenla 11:0e22a6fc7f87 304 else {
lucasfontenla 11:0e22a6fc7f87 305 stepY = 1;}
lucasfontenla 5:0cbf491f703d 306
lucasfontenla 6:af32c68ff4cf 307 if(i >= z_step){
lucasfontenla 5:0cbf491f703d 308 stepZ = 0;
lucasfontenla 5:0cbf491f703d 309 }
lucasfontenla 5:0cbf491f703d 310 else { stepZ = 1; }
lucasfontenla 5:0cbf491f703d 311
lucasfontenla 5:0cbf491f703d 312 wait(time);
lucasfontenla 4:48717b42eab2 313 stepX = 0;
lucasfontenla 4:48717b42eab2 314 stepY = 0;
lucasfontenla 4:48717b42eab2 315 stepZ = 0;
lucasfontenla 5:0cbf491f703d 316 wait(time);
lucasfontenla 4:48717b42eab2 317 }
lucasfontenla 17:55e6270adab5 318
lucasfontenla 17:55e6270adab5 319 if(dirX == x_minus){
lucasfontenla 17:55e6270adab5 320 totalX-=x_step;
lucasfontenla 17:55e6270adab5 321 }
lucasfontenla 17:55e6270adab5 322 else { totalX+=x_step; }
lucasfontenla 17:55e6270adab5 323
lucasfontenla 17:55e6270adab5 324 if(dirY == y_minus){
lucasfontenla 17:55e6270adab5 325 totalY-=y_step;
lucasfontenla 17:55e6270adab5 326 } else { totalY+=y_step; }
lucasfontenla 17:55e6270adab5 327
lucasfontenla 17:55e6270adab5 328 if(dirZ == z_minus){
lucasfontenla 17:55e6270adab5 329 totalZ-=z_step;
lucasfontenla 17:55e6270adab5 330 } else { totalZ+=z_step; }
lucasfontenla 5:0cbf491f703d 331 }
lucasfontenla 17:55e6270adab5 332
lucasfontenla 12:801e58a7137c 333 void zeroX(int pps){
lucasfontenla 25:7b3826dddcba 334 printf("Zero X\n\r");
lucasfontenla 12:801e58a7137c 335 float time = 1.0/pps/2.0;
lucasfontenla 12:801e58a7137c 336
lucasfontenla 17:55e6270adab5 337 if(dirX != x_minus){
lucasfontenla 17:55e6270adab5 338 dirX = x_minus;
lucasfontenla 17:55e6270adab5 339 }
lucasfontenla 17:55e6270adab5 340
lucasfontenla 17:55e6270adab5 341 while(!x_block_min){
lucasfontenla 12:801e58a7137c 342 stepX = 1;
lucasfontenla 12:801e58a7137c 343 wait(time);
lucasfontenla 12:801e58a7137c 344 stepX = 0;
lucasfontenla 12:801e58a7137c 345 wait(time);
lucasfontenla 12:801e58a7137c 346 }
lucasfontenla 12:801e58a7137c 347
lucasfontenla 12:801e58a7137c 348 printf("X zero\n\r");
lucasfontenla 12:801e58a7137c 349
lucasfontenla 17:55e6270adab5 350 totalX = 0;
lucasfontenla 26:f891ff6beb33 351 X = steps_to_distance(totalX, xPitch);
lucasfontenla 26:f891ff6beb33 352 Y = steps_to_distance(totalY, yPitch);
lucasfontenla 26:f891ff6beb33 353 Z = steps_to_distance(totalZ, zPitch);
lucasfontenla 26:f891ff6beb33 354 ihm_class.send_position(X, Y, Z);
lucasfontenla 12:801e58a7137c 355 }
lucasfontenla 11:0e22a6fc7f87 356 void zeroY(int pps){
lucasfontenla 25:7b3826dddcba 357 printf("Zero Y\n\r");
lucasfontenla 11:0e22a6fc7f87 358 float time = 1.0/pps/2.0;
lucasfontenla 11:0e22a6fc7f87 359
lucasfontenla 17:55e6270adab5 360 if(dirY != y_minus){
lucasfontenla 17:55e6270adab5 361 dirY = y_minus;
lucasfontenla 17:55e6270adab5 362 }
lucasfontenla 17:55e6270adab5 363
lucasfontenla 17:55e6270adab5 364 while(!y_block_min){
lucasfontenla 11:0e22a6fc7f87 365 stepY = 1;
lucasfontenla 11:0e22a6fc7f87 366 wait(time);
lucasfontenla 11:0e22a6fc7f87 367 stepY = 0;
lucasfontenla 11:0e22a6fc7f87 368 wait(time);
lucasfontenla 11:0e22a6fc7f87 369 }
lucasfontenla 11:0e22a6fc7f87 370
lucasfontenla 11:0e22a6fc7f87 371 printf("Y zero\n\r");
lucasfontenla 11:0e22a6fc7f87 372
lucasfontenla 17:55e6270adab5 373 totalY = 0;
lucasfontenla 26:f891ff6beb33 374 X = steps_to_distance(totalX, xPitch);
lucasfontenla 26:f891ff6beb33 375 Y = steps_to_distance(totalY, yPitch);
lucasfontenla 26:f891ff6beb33 376 Z = steps_to_distance(totalZ, zPitch);
lucasfontenla 26:f891ff6beb33 377 ihm_class.send_position(X, Y, Z);
lucasfontenla 17:55e6270adab5 378 }
lucasfontenla 11:0e22a6fc7f87 379
lucasfontenla 25:7b3826dddcba 380 void zeroZ(int pps){
lucasfontenla 25:7b3826dddcba 381 printf("Zero Z\n\r");
lucasfontenla 25:7b3826dddcba 382 float time = 1.0/pps/2.0;
lucasfontenla 25:7b3826dddcba 383
lucasfontenla 25:7b3826dddcba 384 if(dirZ != z_minus){
lucasfontenla 25:7b3826dddcba 385 dirZ = z_minus;
lucasfontenla 25:7b3826dddcba 386 }
lucasfontenla 25:7b3826dddcba 387
lucasfontenla 25:7b3826dddcba 388 while(!z_block_min){
lucasfontenla 25:7b3826dddcba 389 stepZ = 1;
lucasfontenla 25:7b3826dddcba 390 wait(time);
lucasfontenla 25:7b3826dddcba 391 stepZ = 0;
lucasfontenla 25:7b3826dddcba 392 wait(time);
lucasfontenla 25:7b3826dddcba 393 }
lucasfontenla 25:7b3826dddcba 394
lucasfontenla 25:7b3826dddcba 395 printf("Z zero\n\r");
lucasfontenla 25:7b3826dddcba 396
lucasfontenla 25:7b3826dddcba 397 totalZ = 0;
lucasfontenla 26:f891ff6beb33 398 X = steps_to_distance(totalX, xPitch);
lucasfontenla 26:f891ff6beb33 399 Y = steps_to_distance(totalY, yPitch);
lucasfontenla 26:f891ff6beb33 400 Z = steps_to_distance(totalZ, zPitch);
lucasfontenla 26:f891ff6beb33 401 ihm_class.send_position(X, Y, Z);
lucasfontenla 25:7b3826dddcba 402 }
lucasfontenla 25:7b3826dddcba 403
lucasfontenla 17:55e6270adab5 404 float steps_to_distance(int steps, float pitch){
lucasfontenla 7:27ea14e58788 405 float delta_S = (((float)steps)/steps_by_rotation)*pitch;
lucasfontenla 7:27ea14e58788 406 return delta_S;
lucasfontenla 7:27ea14e58788 407 }
lucasfontenla 17:55e6270adab5 408
lucasfontenla 17:55e6270adab5 409 int distance_to_steps(float distance, float pitch){
lucasfontenla 17:55e6270adab5 410 int steps = (int)(distance*steps_by_rotation/pitch);
lucasfontenla 17:55e6270adab5 411 return steps;
lucasfontenla 17:55e6270adab5 412 }
lucasfontenla 17:55e6270adab5 413
lucasfontenla 17:55e6270adab5 414 void printDistance(void){
lucasfontenla 17:55e6270adab5 415 X = steps_to_distance(totalX, xPitch);
lucasfontenla 17:55e6270adab5 416 Y = steps_to_distance(totalY, yPitch);
lucasfontenla 17:55e6270adab5 417 Z = steps_to_distance(totalZ, zPitch);
lucasfontenla 17:55e6270adab5 418 printf("X: %.2f Y: %.2f Z: %.2f\r\n", X, Y, Z);
lucasfontenla 18:174b4ff351b5 419 }
lucasfontenla 18:174b4ff351b5 420
lucasfontenla 22:000890b38b32 421 void savePoint(string speed, string mode){
lucasfontenla 26:f891ff6beb33 422 if(saved == 0){
lucasfontenla 26:f891ff6beb33 423 totalX = 0;
lucasfontenla 26:f891ff6beb33 424 totalY = 0;
lucasfontenla 26:f891ff6beb33 425 totalZ = 0;
lucasfontenla 26:f891ff6beb33 426 }
lucasfontenla 26:f891ff6beb33 427
lucasfontenla 26:f891ff6beb33 428 X = steps_to_distance(totalX, xPitch);
lucasfontenla 26:f891ff6beb33 429 Y = steps_to_distance(totalY, yPitch);
lucasfontenla 32:5bd3f0d073ac 430 Z = steps_to_distance(totalZ, zPitch);
lucasfontenla 26:f891ff6beb33 431
lucasfontenla 26:f891ff6beb33 432 printf("\nSave selected\n\r");
lucasfontenla 26:f891ff6beb33 433 if(saved >= maxPoints){
lucasfontenla 26:f891ff6beb33 434 printf("Max points reached\n\r");
lucasfontenla 26:f891ff6beb33 435 } else{
lucasfontenla 26:f891ff6beb33 436 X = steps_to_distance(totalX, xPitch);
lucasfontenla 26:f891ff6beb33 437 Y = steps_to_distance(totalY, yPitch);
lucasfontenla 26:f891ff6beb33 438 Z = steps_to_distance(totalZ, zPitch);
lucasfontenla 26:f891ff6beb33 439
lucasfontenla 26:f891ff6beb33 440 points[0][saved] = X;
lucasfontenla 26:f891ff6beb33 441 points[1][saved] = Y;
lucasfontenla 26:f891ff6beb33 442 points[2][saved] = Z;
lucasfontenla 30:27838f6fdfd6 443 path[0][saved] = 1.0;
lucasfontenla 30:27838f6fdfd6 444 path[1][saved] = atof(speed.c_str());
lucasfontenla 30:27838f6fdfd6 445 path[2][saved] = atof(mode.c_str());
lucasfontenla 26:f891ff6beb33 446 saved+=1;
lucasfontenla 26:f891ff6beb33 447 }
lucasfontenla 26:f891ff6beb33 448 // save code
lucasfontenla 30:27838f6fdfd6 449 printf("Saved --> X: %.2f Y: %.2f Z: %.2f\r\n", points[0][saved-1], points[1][saved-1], points[2][saved-1]);
lucasfontenla 26:f891ff6beb33 450 printf("Path --> Speed: %.2f Mode: %.2f\n\r", path[0][saved-1], path[1][saved-1]);
lucasfontenla 26:f891ff6beb33 451 resting_points = maxPoints-saved;
lucasfontenla 32:5bd3f0d073ac 452 printf("Pontos restantes: %d\n\r", resting_points);
lucasfontenla 32:5bd3f0d073ac 453
lucasfontenla 32:5bd3f0d073ac 454 ihm_class.send_position(X, Y, Z);
lucasfontenla 18:174b4ff351b5 455 }
lucasfontenla 18:174b4ff351b5 456
lucasfontenla 23:d64d37561b4a 457 void jog(void){
lucasfontenla 30:27838f6fdfd6 458 float speed;
lucasfontenla 30:27838f6fdfd6 459
lucasfontenla 26:f891ff6beb33 460 enable = 0;
lucasfontenla 23:d64d37561b4a 461 kill_jog = 0;
lucasfontenla 23:d64d37561b4a 462 printf("JOG Selected %d\n\r", kill_jog);
lucasfontenla 23:d64d37561b4a 463
lucasfontenla 24:92f612ca218b 464 t.reset();
lucasfontenla 24:92f612ca218b 465
lucasfontenla 30:27838f6fdfd6 466 speed = ppsMax;
lucasfontenla 30:27838f6fdfd6 467
lucasfontenla 23:d64d37561b4a 468 while(1){
lucasfontenla 23:d64d37561b4a 469 if(ihm.readable()){
lucasfontenla 23:d64d37561b4a 470 readSerial();
lucasfontenla 23:d64d37561b4a 471 }
lucasfontenla 23:d64d37561b4a 472 valX = joyX;
lucasfontenla 23:d64d37561b4a 473 valY = joyY;
lucasfontenla 23:d64d37561b4a 474
lucasfontenla 32:5bd3f0d073ac 475 if(valX > 0.7 && !toggle){
lucasfontenla 32:5bd3f0d073ac 476 if(x_dir != x_plus){
lucasfontenla 32:5bd3f0d073ac 477 x_dir = x_plus;
lucasfontenla 23:d64d37561b4a 478 }
lucasfontenla 30:27838f6fdfd6 479 //speed = ppsMax*(valX - 0.52)/0.48;
lucasfontenla 23:d64d37561b4a 480 activeX = 1;
lucasfontenla 23:d64d37561b4a 481 }
lucasfontenla 32:5bd3f0d073ac 482 else if(valX < 0.4 && !toggle){
lucasfontenla 32:5bd3f0d073ac 483 if(x_dir != x_minus){
lucasfontenla 32:5bd3f0d073ac 484 x_dir = x_minus;
lucasfontenla 23:d64d37561b4a 485 }
lucasfontenla 30:27838f6fdfd6 486 //speed = ppsMax*valX/0.48;
lucasfontenla 23:d64d37561b4a 487 activeX = 1;
lucasfontenla 23:d64d37561b4a 488 }
lucasfontenla 23:d64d37561b4a 489 else{
lucasfontenla 23:d64d37561b4a 490 activeX = 0;
lucasfontenla 23:d64d37561b4a 491 }
lucasfontenla 23:d64d37561b4a 492
lucasfontenla 23:d64d37561b4a 493 //----------------------------------------------------------------------
lucasfontenla 23:d64d37561b4a 494
lucasfontenla 32:5bd3f0d073ac 495 if(valY > 0.7 && !toggle){
lucasfontenla 23:d64d37561b4a 496 if(y_dir != y_plus){
lucasfontenla 30:27838f6fdfd6 497 y_dir = y_plus;
lucasfontenla 23:d64d37561b4a 498 }
lucasfontenla 30:27838f6fdfd6 499 //speed = ppsMax*(valY - 0.52)/0.48;
lucasfontenla 23:d64d37561b4a 500 activeY = 1;
lucasfontenla 23:d64d37561b4a 501 }
lucasfontenla 32:5bd3f0d073ac 502 else if(valY < 0.4 && !toggle){
lucasfontenla 23:d64d37561b4a 503 if(y_dir != y_minus){
lucasfontenla 30:27838f6fdfd6 504 y_dir = y_minus;
lucasfontenla 23:d64d37561b4a 505 }
lucasfontenla 30:27838f6fdfd6 506 //speed = ppsMax*valY/0.48;
lucasfontenla 23:d64d37561b4a 507 activeY = 1;
lucasfontenla 23:d64d37561b4a 508 }
lucasfontenla 23:d64d37561b4a 509 else{
lucasfontenla 23:d64d37561b4a 510 activeY = 0;
lucasfontenla 23:d64d37561b4a 511 }
lucasfontenla 23:d64d37561b4a 512
lucasfontenla 23:d64d37561b4a 513 //----------------------------------------------------------------------
lucasfontenla 23:d64d37561b4a 514
lucasfontenla 32:5bd3f0d073ac 515 if(valY > 0.7 && toggle){
lucasfontenla 26:f891ff6beb33 516 if(z_dir != z_plus){
lucasfontenla 30:27838f6fdfd6 517 z_dir = z_plus;
lucasfontenla 23:d64d37561b4a 518 }
lucasfontenla 30:27838f6fdfd6 519 activeZ = 1;
lucasfontenla 30:27838f6fdfd6 520 //speed = ppsMax*(valY - 0.52)/0.48;
lucasfontenla 23:d64d37561b4a 521 }
lucasfontenla 32:5bd3f0d073ac 522 else if(valY < 0.4 && toggle){
lucasfontenla 26:f891ff6beb33 523 if(z_dir != z_minus){
lucasfontenla 30:27838f6fdfd6 524 z_dir = z_minus;
lucasfontenla 23:d64d37561b4a 525 }
lucasfontenla 32:5bd3f0d073ac 526 activeZ = -1;
lucasfontenla 30:27838f6fdfd6 527 //speed = ppsMax*(valY - 0.52)/0.48;
lucasfontenla 23:d64d37561b4a 528 }
lucasfontenla 23:d64d37561b4a 529 else {
lucasfontenla 23:d64d37561b4a 530 activeZ = 0;
lucasfontenla 23:d64d37561b4a 531 }
lucasfontenla 23:d64d37561b4a 532
lucasfontenla 30:27838f6fdfd6 533 /*
lucasfontenla 30:27838f6fdfd6 534 if(speed < 100.0){
lucasfontenla 30:27838f6fdfd6 535 speed = 100.0;
lucasfontenla 30:27838f6fdfd6 536 }*/
lucasfontenla 30:27838f6fdfd6 537
lucasfontenla 30:27838f6fdfd6 538 //printf("%f\n\r", speed);
lucasfontenla 23:d64d37561b4a 539 //----------------------------------------------------------------------
lucasfontenla 23:d64d37561b4a 540
lucasfontenla 32:5bd3f0d073ac 541 if(!toggle){
lucasfontenla 32:5bd3f0d073ac 542 move((int)speed, x_dir, y_dir, z_dir, activeX, activeY, activeZ);
lucasfontenla 32:5bd3f0d073ac 543 } else {
lucasfontenla 32:5bd3f0d073ac 544 moveZ(speed, activeZ);
lucasfontenla 32:5bd3f0d073ac 545 }
lucasfontenla 32:5bd3f0d073ac 546
lucasfontenla 23:d64d37561b4a 547 X = steps_to_distance(totalX, xPitch);
lucasfontenla 23:d64d37561b4a 548 Y = steps_to_distance(totalY, yPitch);
lucasfontenla 23:d64d37561b4a 549 Z = steps_to_distance(totalZ, zPitch);
lucasfontenla 30:27838f6fdfd6 550
lucasfontenla 26:f891ff6beb33 551 /*
lucasfontenla 24:92f612ca218b 552 if(t.read() > sendTime){
lucasfontenla 26:f891ff6beb33 553 //printf("X: %.2f Y: %.2f Z: %.2f\r\n", X, Y, Z);
lucasfontenla 24:92f612ca218b 554 X = steps_to_distance(totalX, xPitch);
lucasfontenla 24:92f612ca218b 555 Y = steps_to_distance(totalY, yPitch);
lucasfontenla 24:92f612ca218b 556 Z = steps_to_distance(totalZ, zPitch);
lucasfontenla 24:92f612ca218b 557 ihm_class.send_position(X, Y, Z);
lucasfontenla 24:92f612ca218b 558 t.reset();
lucasfontenla 26:f891ff6beb33 559 }*/
lucasfontenla 23:d64d37561b4a 560
lucasfontenla 23:d64d37561b4a 561 if(kill_jog){
lucasfontenla 23:d64d37561b4a 562 break;
lucasfontenla 23:d64d37561b4a 563 }
lucasfontenla 26:f891ff6beb33 564
lucasfontenla 26:f891ff6beb33 565 if(toggle){
lucasfontenla 26:f891ff6beb33 566 if(!ledZ){
lucasfontenla 26:f891ff6beb33 567 ledZ = 1;
lucasfontenla 26:f891ff6beb33 568 ledXY = 0;
lucasfontenla 26:f891ff6beb33 569 }
lucasfontenla 26:f891ff6beb33 570 } else {
lucasfontenla 26:f891ff6beb33 571 if(!ledXY){
lucasfontenla 26:f891ff6beb33 572 ledXY = 1;
lucasfontenla 26:f891ff6beb33 573 ledZ = 0;
lucasfontenla 26:f891ff6beb33 574 }
lucasfontenla 26:f891ff6beb33 575 }
lucasfontenla 23:d64d37561b4a 576 }
lucasfontenla 23:d64d37561b4a 577 }
lucasfontenla 27:3bbc354adea6 578
lucasfontenla 27:3bbc354adea6 579 void virtualZero(void){
lucasfontenla 32:5bd3f0d073ac 580 printf("Movendo para o zero\n\r");
lucasfontenla 27:3bbc354adea6 581 X = steps_to_distance(totalX, xPitch);
lucasfontenla 27:3bbc354adea6 582 Y = steps_to_distance(totalY, yPitch);
lucasfontenla 27:3bbc354adea6 583 Z = steps_to_distance(totalZ, zPitch);
lucasfontenla 27:3bbc354adea6 584
lucasfontenla 27:3bbc354adea6 585 int x_start = distance_to_steps(X, xPitch);
lucasfontenla 27:3bbc354adea6 586 int x_end = 0;
lucasfontenla 27:3bbc354adea6 587 int y_start = distance_to_steps(Y, yPitch);
lucasfontenla 27:3bbc354adea6 588 int y_end = 0;
lucasfontenla 27:3bbc354adea6 589 int z_start = distance_to_steps(Z, zPitch);
lucasfontenla 27:3bbc354adea6 590 int z_end = 0;
lucasfontenla 27:3bbc354adea6 591
lucasfontenla 27:3bbc354adea6 592 int z_dist = z_end - z_start;
lucasfontenla 27:3bbc354adea6 593
lucasfontenla 27:3bbc354adea6 594 if(z_dist < 0){
lucasfontenla 27:3bbc354adea6 595 z_dir = z_minus;
lucasfontenla 27:3bbc354adea6 596 z_dist = z_dist*(-1);
lucasfontenla 27:3bbc354adea6 597 } else if(z_dist > 0){
lucasfontenla 27:3bbc354adea6 598 z_dir = z_plus;
lucasfontenla 27:3bbc354adea6 599 }
lucasfontenla 27:3bbc354adea6 600
lucasfontenla 30:27838f6fdfd6 601 linear_interpolation(x_start, y_start, x_end, y_end, ppsMax);
lucasfontenla 27:3bbc354adea6 602 move(ppsMax, x_dir, y_dir, z_dir, 0, 0, z_dist);
lucasfontenla 27:3bbc354adea6 603 }
lucasfontenla 23:d64d37561b4a 604
lucasfontenla 23:d64d37561b4a 605 void automatic_run(int total_points){
lucasfontenla 27:3bbc354adea6 606 int x_start;
lucasfontenla 27:3bbc354adea6 607 int x_end;
lucasfontenla 27:3bbc354adea6 608 int y_start;
lucasfontenla 27:3bbc354adea6 609 int y_end;
lucasfontenla 27:3bbc354adea6 610 int z_start;
lucasfontenla 27:3bbc354adea6 611 int z_end;
lucasfontenla 27:3bbc354adea6 612
lucasfontenla 27:3bbc354adea6 613 int z_dist;
lucasfontenla 27:3bbc354adea6 614
lucasfontenla 30:27838f6fdfd6 615 float mode;
lucasfontenla 30:27838f6fdfd6 616 float speed;
lucasfontenla 30:27838f6fdfd6 617 float glue;
lucasfontenla 30:27838f6fdfd6 618
lucasfontenla 32:5bd3f0d073ac 619 int glue_on_point = 0;
lucasfontenla 32:5bd3f0d073ac 620
lucasfontenla 26:f891ff6beb33 621 enable = 0;
lucasfontenla 30:27838f6fdfd6 622
lucasfontenla 23:d64d37561b4a 623 printf("\nStarting automatic routine\n\r");
lucasfontenla 23:d64d37561b4a 624
lucasfontenla 27:3bbc354adea6 625 virtualZero();
lucasfontenla 32:5bd3f0d073ac 626
lucasfontenla 30:27838f6fdfd6 627 wait(1);
lucasfontenla 23:d64d37561b4a 628 // routine
lucasfontenla 23:d64d37561b4a 629
lucasfontenla 32:5bd3f0d073ac 630 printf("Total points: %d\n\r", total_points);
lucasfontenla 32:5bd3f0d073ac 631
lucasfontenla 32:5bd3f0d073ac 632 for(int i = 0; i <= total_points; i++){
lucasfontenla 27:3bbc354adea6 633 printf("Move n: %d\n\r", i);
lucasfontenla 30:27838f6fdfd6 634
lucasfontenla 32:5bd3f0d073ac 635 x_start = totalX;
lucasfontenla 32:5bd3f0d073ac 636 x_end = distance_to_steps(points[0][i], xPitch);
lucasfontenla 23:d64d37561b4a 637
lucasfontenla 32:5bd3f0d073ac 638 y_start = totalY;
lucasfontenla 32:5bd3f0d073ac 639 y_end = distance_to_steps(points[1][i], yPitch);
lucasfontenla 23:d64d37561b4a 640
lucasfontenla 32:5bd3f0d073ac 641 z_start = totalZ;
lucasfontenla 32:5bd3f0d073ac 642 z_end = distance_to_steps(points[2][i], zPitch);
lucasfontenla 27:3bbc354adea6 643
lucasfontenla 27:3bbc354adea6 644 z_dist = z_end - z_start;
lucasfontenla 23:d64d37561b4a 645
lucasfontenla 30:27838f6fdfd6 646 mode = path[0][i];
lucasfontenla 30:27838f6fdfd6 647 speed = path[1][i]*ppsMax;
lucasfontenla 30:27838f6fdfd6 648 glue = path[2][i];
lucasfontenla 30:27838f6fdfd6 649
lucasfontenla 32:5bd3f0d073ac 650 printf("X1: %.2f X2: %.2f\n\r", steps_to_distance(x_start, xPitch), steps_to_distance(x_end, xPitch));
lucasfontenla 32:5bd3f0d073ac 651 printf("Y1: %.2f Y2: %.2f\n\r", steps_to_distance(y_start, yPitch), steps_to_distance(y_end, yPitch));
lucasfontenla 32:5bd3f0d073ac 652 printf("Z1: %.2f Z2: %.2f\n\r", steps_to_distance(z_start, zPitch), steps_to_distance(z_end, zPitch));
lucasfontenla 30:27838f6fdfd6 653 printf("Glue: %.1f, Speed: %.1f\n\r", glue, speed);
lucasfontenla 30:27838f6fdfd6 654
lucasfontenla 30:27838f6fdfd6 655 moveZ(speed, z_dist);
lucasfontenla 30:27838f6fdfd6 656
lucasfontenla 30:27838f6fdfd6 657 if(glue == 1.0){
lucasfontenla 32:5bd3f0d073ac 658 glue_on_point = 1;
lucasfontenla 32:5bd3f0d073ac 659
lucasfontenla 30:27838f6fdfd6 660 } else if(glue == 0.0){
lucasfontenla 30:27838f6fdfd6 661 glueValve = 0;
lucasfontenla 30:27838f6fdfd6 662 } else if(glue == 2.0){
lucasfontenla 30:27838f6fdfd6 663 glueValve = 1;
lucasfontenla 30:27838f6fdfd6 664 }
lucasfontenla 30:27838f6fdfd6 665
lucasfontenla 30:27838f6fdfd6 666 linear_interpolation(x_start, y_start, x_end, y_end, speed);
lucasfontenla 30:27838f6fdfd6 667
lucasfontenla 30:27838f6fdfd6 668 glueValve = 0;
lucasfontenla 32:5bd3f0d073ac 669
lucasfontenla 32:5bd3f0d073ac 670 if(glue_on_point){
lucasfontenla 32:5bd3f0d073ac 671 glueValve = 1;
lucasfontenla 32:5bd3f0d073ac 672 wait(glue_time);
lucasfontenla 32:5bd3f0d073ac 673 glueValve = 0;
lucasfontenla 32:5bd3f0d073ac 674 }
lucasfontenla 23:d64d37561b4a 675 }
lucasfontenla 27:3bbc354adea6 676
lucasfontenla 23:d64d37561b4a 677 killJog();
lucasfontenla 23:d64d37561b4a 678 printf("Done\n\r");
lucasfontenla 26:f891ff6beb33 679 wait(0.1);
lucasfontenla 26:f891ff6beb33 680 ihm_class.action_complete();
lucasfontenla 26:f891ff6beb33 681 enable = 1;
lucasfontenla 30:27838f6fdfd6 682 cancelProgram();
lucasfontenla 22:000890b38b32 683 }
lucasfontenla 22:000890b38b32 684
lucasfontenla 27:3bbc354adea6 685 void moveZ(int speed, int z_dist){
lucasfontenla 27:3bbc354adea6 686 if(z_dist < 0){
lucasfontenla 27:3bbc354adea6 687 z_dir = z_minus;
lucasfontenla 27:3bbc354adea6 688 z_dist = z_dist*(-1);
lucasfontenla 27:3bbc354adea6 689 } else if(z_dist > 0){
lucasfontenla 27:3bbc354adea6 690 z_dir = z_plus;
lucasfontenla 27:3bbc354adea6 691 }
lucasfontenla 27:3bbc354adea6 692
lucasfontenla 32:5bd3f0d073ac 693 //printf("dz: %f\n\r", steps_to_distance(z_dist, zPitch));
lucasfontenla 27:3bbc354adea6 694
lucasfontenla 32:5bd3f0d073ac 695 move(speed*0.6, x_plus, y_plus, z_dir, 0, 0, z_dist);
lucasfontenla 27:3bbc354adea6 696 }
lucasfontenla 27:3bbc354adea6 697
lucasfontenla 23:d64d37561b4a 698 void killJog(void){
lucasfontenla 26:f891ff6beb33 699 enable = 1;
lucasfontenla 23:d64d37561b4a 700 kill_jog = 1;
lucasfontenla 26:f891ff6beb33 701 ledZ = 0;
lucasfontenla 26:f891ff6beb33 702 ledXY = 0;
lucasfontenla 23:d64d37561b4a 703 printf("Jog killed\n\r");
lucasfontenla 23:d64d37561b4a 704 }
lucasfontenla 23:d64d37561b4a 705
lucasfontenla 25:7b3826dddcba 706 void cancelProgram(void){
lucasfontenla 25:7b3826dddcba 707 saved = 0;
lucasfontenla 25:7b3826dddcba 708 killJog();
lucasfontenla 25:7b3826dddcba 709 }
lucasfontenla 25:7b3826dddcba 710
lucasfontenla 23:d64d37561b4a 711 void readSerial(void){
lucasfontenla 22:000890b38b32 712 char recv;
lucasfontenla 25:7b3826dddcba 713 char last_recv;
lucasfontenla 22:000890b38b32 714
lucasfontenla 31:7896ac3a5a1c 715 float timeoutTime = 1.0;
lucasfontenla 27:3bbc354adea6 716
lucasfontenla 22:000890b38b32 717 //save
lucasfontenla 22:000890b38b32 718 int save = 0;
lucasfontenla 22:000890b38b32 719 int mode = 0;
lucasfontenla 22:000890b38b32 720 int speed = 0;
lucasfontenla 22:000890b38b32 721 string mode_recv;
lucasfontenla 22:000890b38b32 722 string speed_str;
lucasfontenla 22:000890b38b32 723
lucasfontenla 23:d64d37561b4a 724 //begin
lucasfontenla 23:d64d37561b4a 725 int begin = 0;
lucasfontenla 23:d64d37561b4a 726
lucasfontenla 23:d64d37561b4a 727 //jog
lucasfontenla 23:d64d37561b4a 728 int jog_mode = 0;
lucasfontenla 23:d64d37561b4a 729 int kill_jog_mode = 0;
lucasfontenla 23:d64d37561b4a 730
lucasfontenla 25:7b3826dddcba 731 //reset
lucasfontenla 25:7b3826dddcba 732 int reset = 0;
lucasfontenla 25:7b3826dddcba 733
lucasfontenla 25:7b3826dddcba 734 //cancel
lucasfontenla 25:7b3826dddcba 735 int cancel = 0;
lucasfontenla 25:7b3826dddcba 736
lucasfontenla 25:7b3826dddcba 737 //zero
lucasfontenla 25:7b3826dddcba 738 int zero = 0;
lucasfontenla 25:7b3826dddcba 739 int machine_zero = 0;
lucasfontenla 25:7b3826dddcba 740
lucasfontenla 27:3bbc354adea6 741 //upload program
lucasfontenla 27:3bbc354adea6 742 int upload = 0;
lucasfontenla 27:3bbc354adea6 743 string upload_buffer;
lucasfontenla 27:3bbc354adea6 744
lucasfontenla 27:3bbc354adea6 745 //send saved
lucasfontenla 27:3bbc354adea6 746 int send_saved = 0;
lucasfontenla 27:3bbc354adea6 747
lucasfontenla 31:7896ac3a5a1c 748 //clean
lucasfontenla 31:7896ac3a5a1c 749 int clean = 0;
lucasfontenla 31:7896ac3a5a1c 750
lucasfontenla 31:7896ac3a5a1c 751 //delete
lucasfontenla 31:7896ac3a5a1c 752 int delete_saved = 0;
lucasfontenla 31:7896ac3a5a1c 753
lucasfontenla 27:3bbc354adea6 754 printf("\n\nReceiving...\n\r");
lucasfontenla 23:d64d37561b4a 755
lucasfontenla 23:d64d37561b4a 756 t.reset();
lucasfontenla 22:000890b38b32 757
lucasfontenla 22:000890b38b32 758 while(1){
lucasfontenla 27:3bbc354adea6 759 if(t.read() > timeoutTime){
lucasfontenla 23:d64d37561b4a 760 printf("Timeout\n\r");
lucasfontenla 23:d64d37561b4a 761 break;
lucasfontenla 23:d64d37561b4a 762 }
lucasfontenla 23:d64d37561b4a 763
lucasfontenla 22:000890b38b32 764 recv = ihm.getc();
lucasfontenla 22:000890b38b32 765
lucasfontenla 27:3bbc354adea6 766 printf("%c\n\r", recv);
lucasfontenla 23:d64d37561b4a 767
lucasfontenla 23:d64d37561b4a 768 if(recv == 'x'){
lucasfontenla 22:000890b38b32 769 if(save){
lucasfontenla 22:000890b38b32 770 savePoint(speed_str, mode_recv);
lucasfontenla 23:d64d37561b4a 771 } else if(begin){
lucasfontenla 32:5bd3f0d073ac 772 printf("Automatic: %d\n\r", saved);
lucasfontenla 23:d64d37561b4a 773 automatic_run(saved);
lucasfontenla 23:d64d37561b4a 774 } else if(jog_mode){
lucasfontenla 23:d64d37561b4a 775 if(kill_jog_mode){
lucasfontenla 23:d64d37561b4a 776 killJog();
lucasfontenla 23:d64d37561b4a 777 } else {
lucasfontenla 23:d64d37561b4a 778 jog();
lucasfontenla 23:d64d37561b4a 779 }
lucasfontenla 25:7b3826dddcba 780 } else if(reset){
lucasfontenla 25:7b3826dddcba 781 NVIC_SystemReset();
lucasfontenla 25:7b3826dddcba 782 } else if(cancel){
lucasfontenla 25:7b3826dddcba 783 cancel = 1;
lucasfontenla 25:7b3826dddcba 784 cancelProgram();
lucasfontenla 25:7b3826dddcba 785 } else if(zero){
lucasfontenla 25:7b3826dddcba 786 if(machine_zero){
lucasfontenla 26:f891ff6beb33 787 goMachineHome(ppsMax);
lucasfontenla 25:7b3826dddcba 788 } else if(last_recv == 'X'){
lucasfontenla 25:7b3826dddcba 789 totalX = 0;
lucasfontenla 25:7b3826dddcba 790 } else if(last_recv == 'Y'){
lucasfontenla 25:7b3826dddcba 791 totalY = 0;
lucasfontenla 25:7b3826dddcba 792 } else if(last_recv == 'Z'){
lucasfontenla 25:7b3826dddcba 793 totalZ = 0;
lucasfontenla 25:7b3826dddcba 794 }
lucasfontenla 27:3bbc354adea6 795 } else if(upload){
lucasfontenla 29:5b181a35ca32 796 ihm_class.read_gcode(upload_buffer);
lucasfontenla 27:3bbc354adea6 797 } else if(send_saved){
lucasfontenla 30:27838f6fdfd6 798 wait(1);
lucasfontenla 30:27838f6fdfd6 799 ihm_class.sendSaved();
lucasfontenla 30:27838f6fdfd6 800 return;
lucasfontenla 31:7896ac3a5a1c 801 } else if(clean){
lucasfontenla 31:7896ac3a5a1c 802 cleaning_glue_status = !cleaning_glue_status;
lucasfontenla 31:7896ac3a5a1c 803 clean_glue();
lucasfontenla 31:7896ac3a5a1c 804 } else if(delete_saved){
lucasfontenla 31:7896ac3a5a1c 805 saved = saved - 1;
lucasfontenla 32:5bd3f0d073ac 806 } else {
lucasfontenla 32:5bd3f0d073ac 807 printf("End\n\r");
lucasfontenla 32:5bd3f0d073ac 808 return;
lucasfontenla 25:7b3826dddcba 809 }
lucasfontenla 32:5bd3f0d073ac 810 return;
lucasfontenla 22:000890b38b32 811 }
lucasfontenla 22:000890b38b32 812
lucasfontenla 22:000890b38b32 813 if(recv == 's'){
lucasfontenla 22:000890b38b32 814 save = 1;
lucasfontenla 22:000890b38b32 815 }
lucasfontenla 22:000890b38b32 816
lucasfontenla 23:d64d37561b4a 817 if(recv == 'b'){
lucasfontenla 23:d64d37561b4a 818 begin = 1;
lucasfontenla 23:d64d37561b4a 819 }
lucasfontenla 23:d64d37561b4a 820
lucasfontenla 23:d64d37561b4a 821 if(recv == 'j'){
lucasfontenla 23:d64d37561b4a 822 jog_mode = 1;
lucasfontenla 23:d64d37561b4a 823 }
lucasfontenla 23:d64d37561b4a 824
lucasfontenla 25:7b3826dddcba 825 if(recv == 'r'){
lucasfontenla 25:7b3826dddcba 826 reset = 1;
lucasfontenla 25:7b3826dddcba 827 }
lucasfontenla 25:7b3826dddcba 828
lucasfontenla 25:7b3826dddcba 829 if(recv == 'c'){
lucasfontenla 25:7b3826dddcba 830 cancel = 1;
lucasfontenla 25:7b3826dddcba 831 }
lucasfontenla 25:7b3826dddcba 832
lucasfontenla 25:7b3826dddcba 833 if(recv == 'z'){
lucasfontenla 25:7b3826dddcba 834 zero = 1;
lucasfontenla 25:7b3826dddcba 835 }
lucasfontenla 25:7b3826dddcba 836
lucasfontenla 27:3bbc354adea6 837 if(recv == 'u'){
lucasfontenla 28:45a8ce39dd2b 838 timeoutTime = 10.0;
lucasfontenla 27:3bbc354adea6 839 upload = 1;
lucasfontenla 27:3bbc354adea6 840 }
lucasfontenla 27:3bbc354adea6 841
lucasfontenla 27:3bbc354adea6 842 if(recv == 'l'){
lucasfontenla 27:3bbc354adea6 843 send_saved = 1;
lucasfontenla 27:3bbc354adea6 844 }
lucasfontenla 27:3bbc354adea6 845
lucasfontenla 31:7896ac3a5a1c 846 if(recv == 'g'){
lucasfontenla 31:7896ac3a5a1c 847 clean = 1;
lucasfontenla 31:7896ac3a5a1c 848 }
lucasfontenla 31:7896ac3a5a1c 849
lucasfontenla 31:7896ac3a5a1c 850 if(recv == 'd'){
lucasfontenla 31:7896ac3a5a1c 851 delete_saved = 1;
lucasfontenla 31:7896ac3a5a1c 852 }
lucasfontenla 31:7896ac3a5a1c 853
lucasfontenla 22:000890b38b32 854 if(save){
lucasfontenla 22:000890b38b32 855 if(recv == 'M'){
lucasfontenla 22:000890b38b32 856 mode = 1;
lucasfontenla 22:000890b38b32 857 speed = 0;
lucasfontenla 22:000890b38b32 858 } else if(recv == 'F'){
lucasfontenla 22:000890b38b32 859 mode = 0;
lucasfontenla 22:000890b38b32 860 speed = 1;
lucasfontenla 22:000890b38b32 861 }
lucasfontenla 22:000890b38b32 862
lucasfontenla 22:000890b38b32 863 if(mode){
lucasfontenla 22:000890b38b32 864 if(recv != 'M'){
lucasfontenla 22:000890b38b32 865 mode_recv.push_back(recv);
lucasfontenla 22:000890b38b32 866 }
lucasfontenla 22:000890b38b32 867 } else if(speed){
lucasfontenla 22:000890b38b32 868 if(recv != 'F'){
lucasfontenla 22:000890b38b32 869 speed_str.push_back(recv);
lucasfontenla 22:000890b38b32 870 }
lucasfontenla 22:000890b38b32 871 }
lucasfontenla 22:000890b38b32 872 }
lucasfontenla 23:d64d37561b4a 873
lucasfontenla 23:d64d37561b4a 874 if(jog_mode && recv == 'k'){
lucasfontenla 23:d64d37561b4a 875 kill_jog_mode = 1;
lucasfontenla 23:d64d37561b4a 876 }
lucasfontenla 25:7b3826dddcba 877
lucasfontenla 25:7b3826dddcba 878 if(zero && recv == 'm'){
lucasfontenla 25:7b3826dddcba 879 machine_zero = 1;
lucasfontenla 26:f891ff6beb33 880 } else if(zero && recv != 'm' && recv != 'z'){
lucasfontenla 25:7b3826dddcba 881 last_recv = recv;
lucasfontenla 25:7b3826dddcba 882 }
lucasfontenla 27:3bbc354adea6 883
lucasfontenla 27:3bbc354adea6 884 if(upload && recv != 'u'){
lucasfontenla 27:3bbc354adea6 885 upload_buffer.push_back(recv);
lucasfontenla 27:3bbc354adea6 886 }
lucasfontenla 22:000890b38b32 887 }
lucasfontenla 24:92f612ca218b 888 t.reset();
lucasfontenla 26:f891ff6beb33 889 }
lucasfontenla 26:f891ff6beb33 890
lucasfontenla 26:f891ff6beb33 891 void goMachineHome(int ppsMax){
lucasfontenla 26:f891ff6beb33 892 /*zeroX(ppsMax);
lucasfontenla 26:f891ff6beb33 893 zeroY(ppsMax);
lucasfontenla 26:f891ff6beb33 894 zeroZ(ppsMax);*/
lucasfontenla 26:f891ff6beb33 895 printf("Machine Zero\n\r");
lucasfontenla 26:f891ff6beb33 896 wait(0.1);
lucasfontenla 26:f891ff6beb33 897 ihm_class.action_complete();
lucasfontenla 26:f891ff6beb33 898 }
lucasfontenla 26:f891ff6beb33 899
lucasfontenla 26:f891ff6beb33 900 void joystickClick(void){
lucasfontenla 26:f891ff6beb33 901 toggle = !toggle;
lucasfontenla 26:f891ff6beb33 902 }
lucasfontenla 26:f891ff6beb33 903
lucasfontenla 30:27838f6fdfd6 904 void linear_interpolation(int x1, int y1, int x2, int y2, float speed){
lucasfontenla 26:f891ff6beb33 905 int x = x1;
lucasfontenla 26:f891ff6beb33 906 int y = y1;
lucasfontenla 26:f891ff6beb33 907
lucasfontenla 26:f891ff6beb33 908 int dx = x2 - x1;
lucasfontenla 26:f891ff6beb33 909 int dy = y2 - y1;
lucasfontenla 26:f891ff6beb33 910
lucasfontenla 27:3bbc354adea6 911 int direction_x;
lucasfontenla 27:3bbc354adea6 912 int direction_y;
lucasfontenla 26:f891ff6beb33 913
lucasfontenla 26:f891ff6beb33 914 if(dx > 0 && dy > 0){
lucasfontenla 26:f891ff6beb33 915 x_dir = x_plus;
lucasfontenla 26:f891ff6beb33 916 y_dir = y_plus;
lucasfontenla 27:3bbc354adea6 917 direction_x = 1;
lucasfontenla 27:3bbc354adea6 918 direction_y = 1;
lucasfontenla 26:f891ff6beb33 919 } else if(dx > 0 && dy < 0){
lucasfontenla 26:f891ff6beb33 920 x_dir = x_plus;
lucasfontenla 26:f891ff6beb33 921 y_dir = y_minus;
lucasfontenla 27:3bbc354adea6 922 direction_x = 1;
lucasfontenla 27:3bbc354adea6 923 direction_y = -1;
lucasfontenla 26:f891ff6beb33 924 dy = dy*(-1);
lucasfontenla 26:f891ff6beb33 925 } else if(dx < 0 && dy > 0){
lucasfontenla 26:f891ff6beb33 926 x_dir = x_minus;
lucasfontenla 26:f891ff6beb33 927 y_dir = y_plus;
lucasfontenla 27:3bbc354adea6 928 direction_x = -1;
lucasfontenla 27:3bbc354adea6 929 direction_y = 1;
lucasfontenla 26:f891ff6beb33 930 dx = dx*(-1);
lucasfontenla 26:f891ff6beb33 931 } else if(dx < 0 && dy < 0){
lucasfontenla 26:f891ff6beb33 932 x_dir = x_minus;
lucasfontenla 26:f891ff6beb33 933 y_dir = y_minus;
lucasfontenla 26:f891ff6beb33 934 dx = dx*(-1);
lucasfontenla 26:f891ff6beb33 935 dy = dy*(-1);
lucasfontenla 27:3bbc354adea6 936 direction_x = -1;
lucasfontenla 30:27838f6fdfd6 937 direction_y = -1;
lucasfontenla 26:f891ff6beb33 938 } else if(dy == 0){
lucasfontenla 26:f891ff6beb33 939 if(dx > 0){
lucasfontenla 26:f891ff6beb33 940 x_dir = x_plus;
lucasfontenla 26:f891ff6beb33 941 } else if (dx < 0){
lucasfontenla 26:f891ff6beb33 942 x_dir = x_minus;
lucasfontenla 26:f891ff6beb33 943 dx = dx*(-1);
lucasfontenla 26:f891ff6beb33 944 }
lucasfontenla 26:f891ff6beb33 945 move(ppsMax, x_dir, y_dir, z_dir, dx, 0, 0);
lucasfontenla 26:f891ff6beb33 946 return;
lucasfontenla 26:f891ff6beb33 947 } else if(dx == 0){
lucasfontenla 26:f891ff6beb33 948 if(dy > 0){
lucasfontenla 26:f891ff6beb33 949 y_dir = y_plus;
lucasfontenla 26:f891ff6beb33 950 } else if (dy < 0){
lucasfontenla 26:f891ff6beb33 951 y_dir = y_minus;
lucasfontenla 26:f891ff6beb33 952 dy = dy*(-1);
lucasfontenla 26:f891ff6beb33 953 }
lucasfontenla 26:f891ff6beb33 954 move(ppsMax, x_dir, y_dir, z_dir, 0, dy, 0);
lucasfontenla 26:f891ff6beb33 955 return;
lucasfontenla 26:f891ff6beb33 956 }
lucasfontenla 26:f891ff6beb33 957
lucasfontenla 27:3bbc354adea6 958
lucasfontenla 27:3bbc354adea6 959 if(dx >= dy){
lucasfontenla 27:3bbc354adea6 960 int incI = 2*dy;
lucasfontenla 27:3bbc354adea6 961 int incS = 2*(dy - dx);
lucasfontenla 27:3bbc354adea6 962
lucasfontenla 27:3bbc354adea6 963 int d = (2*dy - dx);
lucasfontenla 26:f891ff6beb33 964
lucasfontenla 27:3bbc354adea6 965 while(x != x2){
lucasfontenla 27:3bbc354adea6 966 if(d < 0){
lucasfontenla 27:3bbc354adea6 967 d = d + incI;
lucasfontenla 27:3bbc354adea6 968 x = x + 1*direction_x;
lucasfontenla 30:27838f6fdfd6 969 move((int)speed, x_dir, y_dir, z_dir, 1, 0, 0);
lucasfontenla 27:3bbc354adea6 970 } else {
lucasfontenla 27:3bbc354adea6 971 d = d + incS;
lucasfontenla 27:3bbc354adea6 972 x = x + 1*direction_x;
lucasfontenla 27:3bbc354adea6 973 y = y + 1*direction_y;
lucasfontenla 30:27838f6fdfd6 974 move((int)speed, x_dir, y_dir, z_dir, 1, 1, 0);
lucasfontenla 27:3bbc354adea6 975 }
lucasfontenla 27:3bbc354adea6 976 }
lucasfontenla 27:3bbc354adea6 977 } else {
lucasfontenla 27:3bbc354adea6 978 int incI = 2*dx;
lucasfontenla 27:3bbc354adea6 979 int incS = 2*(dx - dy);
lucasfontenla 27:3bbc354adea6 980
lucasfontenla 27:3bbc354adea6 981 int d = (2*dx - dy);
lucasfontenla 30:27838f6fdfd6 982
lucasfontenla 27:3bbc354adea6 983 while(y != y2){
lucasfontenla 27:3bbc354adea6 984 if(d < 0){
lucasfontenla 27:3bbc354adea6 985 d = d + incI;
lucasfontenla 27:3bbc354adea6 986 y = y + 1*direction_y;
lucasfontenla 30:27838f6fdfd6 987 move((int)speed, x_dir, y_dir, z_dir, 0, 1, 0);
lucasfontenla 27:3bbc354adea6 988 } else {
lucasfontenla 27:3bbc354adea6 989 d = d + incS;
lucasfontenla 27:3bbc354adea6 990 x = x + 1*direction_x;
lucasfontenla 27:3bbc354adea6 991 y = y + 1*direction_y;
lucasfontenla 30:27838f6fdfd6 992 move((int)speed, x_dir, y_dir, z_dir, 1, 1, 0);
lucasfontenla 27:3bbc354adea6 993 }
lucasfontenla 27:3bbc354adea6 994 }
lucasfontenla 30:27838f6fdfd6 995 }
lucasfontenla 27:3bbc354adea6 996 }
lucasfontenla 27:3bbc354adea6 997
lucasfontenla 27:3bbc354adea6 998 void circular_interpolation(int r, int xc, int yc){
lucasfontenla 27:3bbc354adea6 999 printf("Circular\n\r");
lucasfontenla 27:3bbc354adea6 1000 int x = 0;
lucasfontenla 27:3bbc354adea6 1001 int y = r;
lucasfontenla 27:3bbc354adea6 1002 int d = (1-r);
lucasfontenla 27:3bbc354adea6 1003
lucasfontenla 27:3bbc354adea6 1004 while(y >= x){
lucasfontenla 27:3bbc354adea6 1005 if(d < 0){
lucasfontenla 27:3bbc354adea6 1006 d = (d + 2*x + 3);
lucasfontenla 27:3bbc354adea6 1007 x = x + 1;
lucasfontenla 27:3bbc354adea6 1008 move(ppsMax, x_plus, y_plus, z_plus, 1, 0, 0);
lucasfontenla 26:f891ff6beb33 1009 } else {
lucasfontenla 27:3bbc354adea6 1010 d = (d + 2*(x-y) + 5);
lucasfontenla 27:3bbc354adea6 1011 x = x + 1;
lucasfontenla 27:3bbc354adea6 1012 y = y - 1;
lucasfontenla 27:3bbc354adea6 1013 move(ppsMax, x_plus, y_minus, z_plus, 1, 1, 0);
lucasfontenla 26:f891ff6beb33 1014 }
lucasfontenla 27:3bbc354adea6 1015
lucasfontenla 27:3bbc354adea6 1016 }
lucasfontenla 27:3bbc354adea6 1017 while(y <= x){
lucasfontenla 27:3bbc354adea6 1018 if(d < 0){
lucasfontenla 27:3bbc354adea6 1019 d = (d + 2*x + 3);
lucasfontenla 27:3bbc354adea6 1020 x = x + 1;
lucasfontenla 27:3bbc354adea6 1021 move(ppsMax, x_plus, y_plus, z_plus, 1, 0, 0);
lucasfontenla 27:3bbc354adea6 1022 } else {
lucasfontenla 27:3bbc354adea6 1023 d = (d + 2*(x-y) + 5);
lucasfontenla 27:3bbc354adea6 1024 x = x + 1;
lucasfontenla 27:3bbc354adea6 1025 y = y - 1;
lucasfontenla 27:3bbc354adea6 1026 move(ppsMax, x_plus, y_minus, z_plus, 1, 1, 0);
lucasfontenla 27:3bbc354adea6 1027 }
lucasfontenla 27:3bbc354adea6 1028
lucasfontenla 27:3bbc354adea6 1029 }
lucasfontenla 27:3bbc354adea6 1030 }
lucasfontenla 27:3bbc354adea6 1031
lucasfontenla 27:3bbc354adea6 1032 void moving_test(void){
lucasfontenla 27:3bbc354adea6 1033 move(ppsMax, x_plus, y_plus, z_plus, 3000, 0, 0);
lucasfontenla 27:3bbc354adea6 1034 move(ppsMax, x_plus, y_plus, z_plus, 0, 3000, 0);
lucasfontenla 32:5bd3f0d073ac 1035 moveZ(ppsMax, 1000);
lucasfontenla 32:5bd3f0d073ac 1036
lucasfontenla 32:5bd3f0d073ac 1037 wait(2);
lucasfontenla 27:3bbc354adea6 1038
lucasfontenla 27:3bbc354adea6 1039 move(ppsMax, x_minus, y_minus, z_minus, 3000, 0, 0);
lucasfontenla 27:3bbc354adea6 1040 move(ppsMax, x_minus, y_minus, z_minus, 0, 3000, 0);
lucasfontenla 32:5bd3f0d073ac 1041 moveZ(ppsMax, -1000);
lucasfontenla 31:7896ac3a5a1c 1042 }
lucasfontenla 31:7896ac3a5a1c 1043
lucasfontenla 31:7896ac3a5a1c 1044 void clean_glue(void){
lucasfontenla 31:7896ac3a5a1c 1045 printf("Cleaning glue\n\r");
lucasfontenla 31:7896ac3a5a1c 1046 killJog();
lucasfontenla 31:7896ac3a5a1c 1047 if(cleaning_glue_status){
lucasfontenla 31:7896ac3a5a1c 1048 printf("Activating\n\r");
lucasfontenla 31:7896ac3a5a1c 1049 glueValve = 1;
lucasfontenla 31:7896ac3a5a1c 1050 } else {
lucasfontenla 31:7896ac3a5a1c 1051 printf("Deactivating\n\r");
lucasfontenla 31:7896ac3a5a1c 1052 glueValve = 0;
lucasfontenla 31:7896ac3a5a1c 1053 }
lucasfontenla 17:55e6270adab5 1054 }