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@30:27838f6fdfd6, 2018-05-25 (annotated)

Committer:
lucasfontenla
Date:
Fri May 25 17:28:28 2018 +0000
Revision:
30:27838f6fdfd6
Parent:
29:5b181a35ca32
Child:
31:7896ac3a5a1c
Salvar pontos e executar com acionamento da v?lvula funcionando;

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