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@34:e3090fc2e9a4, 2018-06-10 (annotated)

Committer:
lucasfontenla
Date:
Sun Jun 10 06:15:17 2018 +0000
Revision:
34:e3090fc2e9a4
Parent:
33:6ad0d169b154
Child:
35:4a527ba7281a
Comunica??o serial com wifi implementada

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