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@36:4d0e131146e6, 2019-02-26 (annotated)

Committer:
lucasfontenla
Date:
Tue Feb 26 00:55:02 2019 +0000
Revision:
36:4d0e131146e6
Parent:
35:4a527ba7281a 
Uploading for Projeto Mecatronico 2019

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