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@31:7896ac3a5a1c, 2018-05-25 (annotated)

Committer:
lucasfontenla
Date:
Fri May 25 23:53:27 2018 +0000
Revision:
31:7896ac3a5a1c
Parent:
30:27838f6fdfd6
Child:
32:5bd3f0d073ac
Fun??es deletar e limpeza de cola implementadas. Ainda n?o testada a integra??o com a IHM.

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