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@35:4a527ba7281a, 2018-06-11 (annotated)

Committer:
lucasfontenla
Date:
Mon Jun 11 20:05:47 2018 +0000
Revision:
35:4a527ba7281a
Parent:
34:e3090fc2e9a4
Child:
36:4d0e131146e6
Funcionando quase tudo

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