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@33:6ad0d169b154, 2018-06-10 (annotated)

Committer:
lucasfontenla
Date:
Sun Jun 10 05:00:10 2018 +0000
Revision:
33:6ad0d169b154
Parent:
32:5bd3f0d073ac
Child:
34:e3090fc2e9a4
C?digo para emerg?ncia escrito mas n?o testado. Pronto para receber o c?digo de movimenta??o.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lucasfontenla 21:c6c33381fc5f 1 // LIBs ------------------------------------------------------------------------------------
lucasfontenla 0:cc562f78f889 2 #include "mbed.h"
lucasfontenla 22:000890b38b32 3 #include "string"
lucasfontenla 24:92f612ca218b 4 #include "classes.h"
lucasfontenla 0:cc562f78f889 5
lucasfontenla 5:0cbf491f703d 6 // MACHINE SETUP ---------------------------------------------------------------------------
lucasfontenla 23:d64d37561b4a 7 Timer t;
lucasfontenla 24:92f612ca218b 8 IHM ihm_class;
lucasfontenla 5:0cbf491f703d 9
lucasfontenla 5:0cbf491f703d 10 // drivers output signal
lucasfontenla 14:7cc41420a12c 11 DigitalOut enable(D2);
lucasfontenla 14:7cc41420a12c 12
lucasfontenla 4:48717b42eab2 13 DigitalOut dirX(D3);
lucasfontenla 15:4604a7ee9c77 14 DigitalOut stepX(D4);
lucasfontenla 4:48717b42eab2 15
lucasfontenla 15:4604a7ee9c77 16 DigitalOut dirY(D5);
lucasfontenla 15:4604a7ee9c77 17 DigitalOut stepY(D6);
lucasfontenla 4:48717b42eab2 18
lucasfontenla 15:4604a7ee9c77 19 DigitalOut dirZ(D7);
lucasfontenla 15:4604a7ee9c77 20 DigitalOut stepZ(D8);
lucasfontenla 4:48717b42eab2 21
lucasfontenla 26:f891ff6beb33 22 DigitalOut ledXY(D10);
lucasfontenla 26:f891ff6beb33 23 DigitalOut ledZ(D11);
lucasfontenla 26:f891ff6beb33 24
lucasfontenla 30:27838f6fdfd6 25 DigitalOut glueValve(D12);
lucasfontenla 30:27838f6fdfd6 26
lucasfontenla 5:0cbf491f703d 27 // hardware input signal
lucasfontenla 5:0cbf491f703d 28 // end-of-stroke sensors
lucasfontenla 32:5bd3f0d073ac 29 InterruptIn endX(A3);
lucasfontenla 32:5bd3f0d073ac 30 InterruptIn endY(A4);
lucasfontenla 32:5bd3f0d073ac 31 InterruptIn endZ(A5);
lucasfontenla 5:0cbf491f703d 32
lucasfontenla 23:d64d37561b4a 33 // JOG Controls
lucasfontenla 9:d771a576e4fc 34 AnalogIn joyX(A0);
lucasfontenla 9:d771a576e4fc 35 AnalogIn joyY(A1);
lucasfontenla 4:48717b42eab2 36
lucasfontenla 32:5bd3f0d073ac 37 InterruptIn joyClick(A2);
lucasfontenla 4:48717b42eab2 38
lucasfontenla 5:0cbf491f703d 39 // variables definition
lucasfontenla 23:d64d37561b4a 40 int modeStatus = 1;
lucasfontenla 5:0cbf491f703d 41
lucasfontenla 4:48717b42eab2 42 float valX;
lucasfontenla 4:48717b42eab2 43 float valY;
lucasfontenla 26:f891ff6beb33 44
lucasfontenla 26:f891ff6beb33 45 int toggle = 0;
lucasfontenla 4:48717b42eab2 46
lucasfontenla 4:48717b42eab2 47 int activeX = 1;
lucasfontenla 4:48717b42eab2 48 int activeY = 1;
lucasfontenla 4:48717b42eab2 49 int activeZ = 1;
lucasfontenla 4:48717b42eab2 50
lucasfontenla 5:0cbf491f703d 51 int x_dir;
lucasfontenla 5:0cbf491f703d 52 int y_dir;
lucasfontenla 5:0cbf491f703d 53 int z_dir;
lucasfontenla 5:0cbf491f703d 54
lucasfontenla 13:0f385bfe3e0d 55 int x_plus = 0;
lucasfontenla 13:0f385bfe3e0d 56 int x_minus = 1;
lucasfontenla 27:3bbc354adea6 57 int y_plus = 1;
lucasfontenla 27:3bbc354adea6 58 int y_minus = 0;
lucasfontenla 16:3adab04d42a6 59 int z_plus = 0;
lucasfontenla 16:3adab04d42a6 60 int z_minus = 1;
lucasfontenla 9:d771a576e4fc 61
lucasfontenla 15:4604a7ee9c77 62 int x_limit = 0;
lucasfontenla 16:3adab04d42a6 63 int x_block_max = 0;
lucasfontenla 16:3adab04d42a6 64 int x_block_min = 0;
lucasfontenla 12:801e58a7137c 65
lucasfontenla 15:4604a7ee9c77 66 int y_limit = 0;
lucasfontenla 16:3adab04d42a6 67 int y_block_max = 0;
lucasfontenla 16:3adab04d42a6 68 int y_block_min = 0;
lucasfontenla 15:4604a7ee9c77 69
lucasfontenla 15:4604a7ee9c77 70 int z_limit = 0;
lucasfontenla 16:3adab04d42a6 71 int z_block_max = 0;
lucasfontenla 16:3adab04d42a6 72 int z_block_min = 0;
lucasfontenla 9:d771a576e4fc 73
lucasfontenla 32:5bd3f0d073ac 74 float ppsMax = 1500.0;
lucasfontenla 30:27838f6fdfd6 75 float glue_time = 1.0;
lucasfontenla 5:0cbf491f703d 76
lucasfontenla 7:27ea14e58788 77 int totalX = 0;
lucasfontenla 7:27ea14e58788 78 int totalY = 0;
lucasfontenla 15:4604a7ee9c77 79 int totalZ = 0;
lucasfontenla 7:27ea14e58788 80
lucasfontenla 17:55e6270adab5 81 int x_steps_to_run;
lucasfontenla 17:55e6270adab5 82 int y_steps_to_run;
lucasfontenla 17:55e6270adab5 83 int z_steps_to_run;
lucasfontenla 17:55e6270adab5 84
lucasfontenla 31:7896ac3a5a1c 85 int cleaning_glue_status = 0;
lucasfontenla 31:7896ac3a5a1c 86
lucasfontenla 18:174b4ff351b5 87 float x_distance_to_run;
lucasfontenla 18:174b4ff351b5 88 float y_distance_to_run;
lucasfontenla 18:174b4ff351b5 89 float z_distance_to_run;
lucasfontenla 18:174b4ff351b5 90
lucasfontenla 7:27ea14e58788 91 float X = 0;
lucasfontenla 7:27ea14e58788 92 float Y = 0;
lucasfontenla 15:4604a7ee9c77 93 float Z = 0;
lucasfontenla 7:27ea14e58788 94
lucasfontenla 9:d771a576e4fc 95 int endX1Status;
lucasfontenla 9:d771a576e4fc 96 int endX2Status;
lucasfontenla 9:d771a576e4fc 97
lucasfontenla 7:27ea14e58788 98 // machine parameters
lucasfontenla 8:de64672c240f 99 float xPitch = 3.0;
lucasfontenla 8:de64672c240f 100 float yPitch = 3.0;
lucasfontenla 32:5bd3f0d073ac 101 float zPitch = 6.0;
lucasfontenla 7:27ea14e58788 102
lucasfontenla 7:27ea14e58788 103 float motor_steps_by_rotation = 200.0;
lucasfontenla 7:27ea14e58788 104 float step_mode = 0.5;
lucasfontenla 7:27ea14e58788 105 float steps_by_rotation = motor_steps_by_rotation/step_mode;
lucasfontenla 7:27ea14e58788 106
lucasfontenla 23:d64d37561b4a 107 int kill_jog;
lucasfontenla 23:d64d37561b4a 108
lucasfontenla 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 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 33:6ad0d169b154 868 if(recv == 'm'){
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 26:f891ff6beb33 953 /*zeroX(ppsMax);
lucasfontenla 26:f891ff6beb33 954 zeroY(ppsMax);
lucasfontenla 26:f891ff6beb33 955 zeroZ(ppsMax);*/
lucasfontenla 26:f891ff6beb33 956 printf("Machine Zero\n\r");
lucasfontenla 26:f891ff6beb33 957 wait(0.1);
lucasfontenla 26:f891ff6beb33 958 ihm_class.action_complete();
lucasfontenla 26:f891ff6beb33 959 }
lucasfontenla 26:f891ff6beb33 960
lucasfontenla 26:f891ff6beb33 961 void joystickClick(void){
lucasfontenla 26:f891ff6beb33 962 toggle = !toggle;
lucasfontenla 26:f891ff6beb33 963 }
lucasfontenla 26:f891ff6beb33 964
lucasfontenla 30:27838f6fdfd6 965 void linear_interpolation(int x1, int y1, int x2, int y2, float speed){
lucasfontenla 26:f891ff6beb33 966 int x = x1;
lucasfontenla 26:f891ff6beb33 967 int y = y1;
lucasfontenla 26:f891ff6beb33 968
lucasfontenla 26:f891ff6beb33 969 int dx = x2 - x1;
lucasfontenla 26:f891ff6beb33 970 int dy = y2 - y1;
lucasfontenla 26:f891ff6beb33 971
lucasfontenla 27:3bbc354adea6 972 int direction_x;
lucasfontenla 27:3bbc354adea6 973 int direction_y;
lucasfontenla 26:f891ff6beb33 974
lucasfontenla 26:f891ff6beb33 975 if(dx > 0 && dy > 0){
lucasfontenla 26:f891ff6beb33 976 x_dir = x_plus;
lucasfontenla 26:f891ff6beb33 977 y_dir = y_plus;
lucasfontenla 27:3bbc354adea6 978 direction_x = 1;
lucasfontenla 27:3bbc354adea6 979 direction_y = 1;
lucasfontenla 26:f891ff6beb33 980 } else if(dx > 0 && dy < 0){
lucasfontenla 26:f891ff6beb33 981 x_dir = x_plus;
lucasfontenla 26:f891ff6beb33 982 y_dir = y_minus;
lucasfontenla 27:3bbc354adea6 983 direction_x = 1;
lucasfontenla 27:3bbc354adea6 984 direction_y = -1;
lucasfontenla 26:f891ff6beb33 985 dy = dy*(-1);
lucasfontenla 26:f891ff6beb33 986 } else if(dx < 0 && dy > 0){
lucasfontenla 26:f891ff6beb33 987 x_dir = x_minus;
lucasfontenla 26:f891ff6beb33 988 y_dir = y_plus;
lucasfontenla 27:3bbc354adea6 989 direction_x = -1;
lucasfontenla 27:3bbc354adea6 990 direction_y = 1;
lucasfontenla 26:f891ff6beb33 991 dx = dx*(-1);
lucasfontenla 26:f891ff6beb33 992 } else if(dx < 0 && dy < 0){
lucasfontenla 26:f891ff6beb33 993 x_dir = x_minus;
lucasfontenla 26:f891ff6beb33 994 y_dir = y_minus;
lucasfontenla 26:f891ff6beb33 995 dx = dx*(-1);
lucasfontenla 26:f891ff6beb33 996 dy = dy*(-1);
lucasfontenla 27:3bbc354adea6 997 direction_x = -1;
lucasfontenla 30:27838f6fdfd6 998 direction_y = -1;
lucasfontenla 26:f891ff6beb33 999 } else if(dy == 0){
lucasfontenla 26:f891ff6beb33 1000 if(dx > 0){
lucasfontenla 26:f891ff6beb33 1001 x_dir = x_plus;
lucasfontenla 26:f891ff6beb33 1002 } else if (dx < 0){
lucasfontenla 26:f891ff6beb33 1003 x_dir = x_minus;
lucasfontenla 26:f891ff6beb33 1004 dx = dx*(-1);
lucasfontenla 26:f891ff6beb33 1005 }
lucasfontenla 26:f891ff6beb33 1006 move(ppsMax, x_dir, y_dir, z_dir, dx, 0, 0);
lucasfontenla 26:f891ff6beb33 1007 return;
lucasfontenla 26:f891ff6beb33 1008 } else if(dx == 0){
lucasfontenla 26:f891ff6beb33 1009 if(dy > 0){
lucasfontenla 26:f891ff6beb33 1010 y_dir = y_plus;
lucasfontenla 26:f891ff6beb33 1011 } else if (dy < 0){
lucasfontenla 26:f891ff6beb33 1012 y_dir = y_minus;
lucasfontenla 26:f891ff6beb33 1013 dy = dy*(-1);
lucasfontenla 26:f891ff6beb33 1014 }
lucasfontenla 26:f891ff6beb33 1015 move(ppsMax, x_dir, y_dir, z_dir, 0, dy, 0);
lucasfontenla 26:f891ff6beb33 1016 return;
lucasfontenla 26:f891ff6beb33 1017 }
lucasfontenla 26:f891ff6beb33 1018
lucasfontenla 27:3bbc354adea6 1019
lucasfontenla 27:3bbc354adea6 1020 if(dx >= dy){
lucasfontenla 27:3bbc354adea6 1021 int incI = 2*dy;
lucasfontenla 27:3bbc354adea6 1022 int incS = 2*(dy - dx);
lucasfontenla 27:3bbc354adea6 1023
lucasfontenla 27:3bbc354adea6 1024 int d = (2*dy - dx);
lucasfontenla 26:f891ff6beb33 1025
lucasfontenla 27:3bbc354adea6 1026 while(x != x2){
lucasfontenla 27:3bbc354adea6 1027 if(d < 0){
lucasfontenla 27:3bbc354adea6 1028 d = d + incI;
lucasfontenla 27:3bbc354adea6 1029 x = x + 1*direction_x;
lucasfontenla 30:27838f6fdfd6 1030 move((int)speed, x_dir, y_dir, z_dir, 1, 0, 0);
lucasfontenla 27:3bbc354adea6 1031 } else {
lucasfontenla 27:3bbc354adea6 1032 d = d + incS;
lucasfontenla 27:3bbc354adea6 1033 x = x + 1*direction_x;
lucasfontenla 27:3bbc354adea6 1034 y = y + 1*direction_y;
lucasfontenla 30:27838f6fdfd6 1035 move((int)speed, x_dir, y_dir, z_dir, 1, 1, 0);
lucasfontenla 27:3bbc354adea6 1036 }
lucasfontenla 27:3bbc354adea6 1037 }
lucasfontenla 27:3bbc354adea6 1038 } else {
lucasfontenla 27:3bbc354adea6 1039 int incI = 2*dx;
lucasfontenla 27:3bbc354adea6 1040 int incS = 2*(dx - dy);
lucasfontenla 27:3bbc354adea6 1041
lucasfontenla 27:3bbc354adea6 1042 int d = (2*dx - dy);
lucasfontenla 30:27838f6fdfd6 1043
lucasfontenla 27:3bbc354adea6 1044 while(y != y2){
lucasfontenla 27:3bbc354adea6 1045 if(d < 0){
lucasfontenla 27:3bbc354adea6 1046 d = d + incI;
lucasfontenla 27:3bbc354adea6 1047 y = y + 1*direction_y;
lucasfontenla 30:27838f6fdfd6 1048 move((int)speed, x_dir, y_dir, z_dir, 0, 1, 0);
lucasfontenla 27:3bbc354adea6 1049 } else {
lucasfontenla 27:3bbc354adea6 1050 d = d + incS;
lucasfontenla 27:3bbc354adea6 1051 x = x + 1*direction_x;
lucasfontenla 27:3bbc354adea6 1052 y = y + 1*direction_y;
lucasfontenla 30:27838f6fdfd6 1053 move((int)speed, x_dir, y_dir, z_dir, 1, 1, 0);
lucasfontenla 27:3bbc354adea6 1054 }
lucasfontenla 27:3bbc354adea6 1055 }
lucasfontenla 30:27838f6fdfd6 1056 }
lucasfontenla 27:3bbc354adea6 1057 }
lucasfontenla 27:3bbc354adea6 1058
lucasfontenla 27:3bbc354adea6 1059 void circular_interpolation(int r, int xc, int yc){
lucasfontenla 27:3bbc354adea6 1060 printf("Circular\n\r");
lucasfontenla 27:3bbc354adea6 1061 int x = 0;
lucasfontenla 27:3bbc354adea6 1062 int y = r;
lucasfontenla 27:3bbc354adea6 1063 int d = (1-r);
lucasfontenla 27:3bbc354adea6 1064
lucasfontenla 27:3bbc354adea6 1065 while(y >= x){
lucasfontenla 27:3bbc354adea6 1066 if(d < 0){
lucasfontenla 27:3bbc354adea6 1067 d = (d + 2*x + 3);
lucasfontenla 27:3bbc354adea6 1068 x = x + 1;
lucasfontenla 27:3bbc354adea6 1069 move(ppsMax, x_plus, y_plus, z_plus, 1, 0, 0);
lucasfontenla 26:f891ff6beb33 1070 } else {
lucasfontenla 27:3bbc354adea6 1071 d = (d + 2*(x-y) + 5);
lucasfontenla 27:3bbc354adea6 1072 x = x + 1;
lucasfontenla 27:3bbc354adea6 1073 y = y - 1;
lucasfontenla 27:3bbc354adea6 1074 move(ppsMax, x_plus, y_minus, z_plus, 1, 1, 0);
lucasfontenla 26:f891ff6beb33 1075 }
lucasfontenla 27:3bbc354adea6 1076
lucasfontenla 27:3bbc354adea6 1077 }
lucasfontenla 27:3bbc354adea6 1078 while(y <= x){
lucasfontenla 27:3bbc354adea6 1079 if(d < 0){
lucasfontenla 27:3bbc354adea6 1080 d = (d + 2*x + 3);
lucasfontenla 27:3bbc354adea6 1081 x = x + 1;
lucasfontenla 27:3bbc354adea6 1082 move(ppsMax, x_plus, y_plus, z_plus, 1, 0, 0);
lucasfontenla 27:3bbc354adea6 1083 } else {
lucasfontenla 27:3bbc354adea6 1084 d = (d + 2*(x-y) + 5);
lucasfontenla 27:3bbc354adea6 1085 x = x + 1;
lucasfontenla 27:3bbc354adea6 1086 y = y - 1;
lucasfontenla 27:3bbc354adea6 1087 move(ppsMax, x_plus, y_minus, z_plus, 1, 1, 0);
lucasfontenla 27:3bbc354adea6 1088 }
lucasfontenla 27:3bbc354adea6 1089
lucasfontenla 27:3bbc354adea6 1090 }
lucasfontenla 27:3bbc354adea6 1091 }
lucasfontenla 27:3bbc354adea6 1092
lucasfontenla 27:3bbc354adea6 1093 void moving_test(void){
lucasfontenla 27:3bbc354adea6 1094 move(ppsMax, x_plus, y_plus, z_plus, 3000, 0, 0);
lucasfontenla 27:3bbc354adea6 1095 move(ppsMax, x_plus, y_plus, z_plus, 0, 3000, 0);
lucasfontenla 32:5bd3f0d073ac 1096 moveZ(ppsMax, 1000);
lucasfontenla 32:5bd3f0d073ac 1097
lucasfontenla 32:5bd3f0d073ac 1098 wait(2);
lucasfontenla 27:3bbc354adea6 1099
lucasfontenla 27:3bbc354adea6 1100 move(ppsMax, x_minus, y_minus, z_minus, 3000, 0, 0);
lucasfontenla 27:3bbc354adea6 1101 move(ppsMax, x_minus, y_minus, z_minus, 0, 3000, 0);
lucasfontenla 32:5bd3f0d073ac 1102 moveZ(ppsMax, -1000);
lucasfontenla 31:7896ac3a5a1c 1103 }
lucasfontenla 31:7896ac3a5a1c 1104
lucasfontenla 31:7896ac3a5a1c 1105 void clean_glue(void){
lucasfontenla 31:7896ac3a5a1c 1106 printf("Cleaning glue\n\r");
lucasfontenla 31:7896ac3a5a1c 1107 killJog();
lucasfontenla 31:7896ac3a5a1c 1108 if(cleaning_glue_status){
lucasfontenla 31:7896ac3a5a1c 1109 printf("Activating\n\r");
lucasfontenla 31:7896ac3a5a1c 1110 glueValve = 1;
lucasfontenla 31:7896ac3a5a1c 1111 } else {
lucasfontenla 31:7896ac3a5a1c 1112 printf("Deactivating\n\r");
lucasfontenla 31:7896ac3a5a1c 1113 glueValve = 0;
lucasfontenla 31:7896ac3a5a1c 1114 }
lucasfontenla 17:55e6270adab5 1115 }