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@28:45a8ce39dd2b, 2018-05-21 (annotated)

Committer:
lucasfontenla
Date:
Mon May 21 17:14:19 2018 +0000
Revision:
28:45a8ce39dd2b
Parent:
27:3bbc354adea6
Child:
29:5b181a35ca32
Interpretador de GCODE implementado com Serial.

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