Grupo T
/
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@26:f891ff6beb33, 2018-05-17 (annotated)
- Committer:
- lucasfontenla
- Date:
- Thu May 17 23:08:43 2018 +0000
- Revision:
- 26:f891ff6beb33
- Parent:
- 25:7b3826dddcba
- Child:
- 27:3bbc354adea6
INTERPOLA??O LINEAR FUNCIONANDO
Who changed what in which revision?
| User | Revision | Line number | New 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 | 17:55e6270adab5 | 55 | int y_plus = 0; |
| lucasfontenla | 17:55e6270adab5 | 56 | int y_minus = 1; |
| 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 | 24:92f612ca218b | 72 | float ppsMax = 1000.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 | 18:174b4ff351b5 | 102 | int saved = 0; |
| lucasfontenla | 19:e69ff16f35aa | 103 | const int maxPoints = 10; |
| lucasfontenla | 22:000890b38b32 | 104 | float points[3][maxPoints]; // [0] X; [1] Y; [2] Z; |
| lucasfontenla | 22:000890b38b32 | 105 | float path[2][maxPoints]; // [0] speed; [1] mode; |
| lucasfontenla | 22:000890b38b32 | 106 | int resting_points = 0; |
| lucasfontenla | 18:174b4ff351b5 | 107 | |
| lucasfontenla | 23:d64d37561b4a | 108 | int kill_jog; |
| lucasfontenla | 23:d64d37561b4a | 109 | |
| lucasfontenla | 21:c6c33381fc5f | 110 | // PROTOTYPE FUNCTIONS ------------------------------------------------------------------------------- |
| lucasfontenla | 23:d64d37561b4a | 111 | void jog(void); |
| lucasfontenla | 23:d64d37561b4a | 112 | void killJog(void); |
| lucasfontenla | 23:d64d37561b4a | 113 | |
| lucasfontenla | 23:d64d37561b4a | 114 | void automatic_run(int); |
| lucasfontenla | 23:d64d37561b4a | 115 | |
| lucasfontenla | 5:0cbf491f703d | 116 | void move(int pps, int x_dir, int y_dir, int z_dir, int x_step, int y_step, int z_step); |
| lucasfontenla | 17:55e6270adab5 | 117 | |
| lucasfontenla | 26:f891ff6beb33 | 118 | float steps_to_distance(int, float); |
| lucasfontenla | 26:f891ff6beb33 | 119 | int distance_to_steps(float, float); |
| lucasfontenla | 7:27ea14e58788 | 120 | |
| lucasfontenla | 15:4604a7ee9c77 | 121 | void endX_press(void); |
| lucasfontenla | 15:4604a7ee9c77 | 122 | void endX_release(void); |
| lucasfontenla | 11:0e22a6fc7f87 | 123 | |
| lucasfontenla | 15:4604a7ee9c77 | 124 | void endY_press(void); |
| lucasfontenla | 15:4604a7ee9c77 | 125 | void endY_release(void); |
| lucasfontenla | 15:4604a7ee9c77 | 126 | |
| lucasfontenla | 15:4604a7ee9c77 | 127 | void endZ_press(void); |
| lucasfontenla | 15:4604a7ee9c77 | 128 | void endZ_release(void); |
| lucasfontenla | 9:d771a576e4fc | 129 | |
| lucasfontenla | 26:f891ff6beb33 | 130 | void goMachineHome(int); |
| lucasfontenla | 26:f891ff6beb33 | 131 | void zeroX(int); |
| lucasfontenla | 26:f891ff6beb33 | 132 | void zeroY(int); |
| lucasfontenla | 26:f891ff6beb33 | 133 | void zeroZ(int); |
| lucasfontenla | 11:0e22a6fc7f87 | 134 | |
| lucasfontenla | 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 | 26:f891ff6beb33 | 144 | void linear_interpolation(int x1, int y1, int x2, int y2); |
| 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 | 26:f891ff6beb33 | 165 | while(1){ |
| lucasfontenla | 26:f891ff6beb33 | 166 | linear_interpolation(0, 0, 0, -5000); |
| lucasfontenla | 26:f891ff6beb33 | 167 | linear_interpolation(0, -5000, 5000, 0); |
| lucasfontenla | 26:f891ff6beb33 | 168 | linear_interpolation(5000, 0, 5000, 2500); |
| lucasfontenla | 26:f891ff6beb33 | 169 | linear_interpolation(5000, 2500, 0, 0); |
| lucasfontenla | 26:f891ff6beb33 | 170 | } |
| lucasfontenla | 26:f891ff6beb33 | 171 | |
| lucasfontenla | 26:f891ff6beb33 | 172 | joyClick.fall(&joystickClick); |
| lucasfontenla | 26:f891ff6beb33 | 173 | /* |
| lucasfontenla | 23:d64d37561b4a | 174 | while(1){ |
| lucasfontenla | 23:d64d37561b4a | 175 | if(ihm.readable()){ |
| lucasfontenla | 23:d64d37561b4a | 176 | readSerial(); |
| lucasfontenla | 26:f891ff6beb33 | 177 | } |
| lucasfontenla | 26:f891ff6beb33 | 178 | |
| lucasfontenla | 26:f891ff6beb33 | 179 | }*/ |
| lucasfontenla | 9:d771a576e4fc | 180 | } |
| lucasfontenla | 9:d771a576e4fc | 181 | |
| lucasfontenla | 9:d771a576e4fc | 182 | // FUNCTIONS ---------------------------------------------------------------- |
| lucasfontenla | 23:d64d37561b4a | 183 | |
| lucasfontenla | 15:4604a7ee9c77 | 184 | void endX_press(void){ |
| lucasfontenla | 15:4604a7ee9c77 | 185 | printf("X - press\n\r"); |
| lucasfontenla | 15:4604a7ee9c77 | 186 | if(dirX == x_plus){ |
| lucasfontenla | 16:3adab04d42a6 | 187 | x_block_max = 1; |
| lucasfontenla | 15:4604a7ee9c77 | 188 | } |
| lucasfontenla | 15:4604a7ee9c77 | 189 | else { |
| lucasfontenla | 16:3adab04d42a6 | 190 | x_block_min = 1; |
| lucasfontenla | 15:4604a7ee9c77 | 191 | } |
| lucasfontenla | 12:801e58a7137c | 192 | } |
| lucasfontenla | 15:4604a7ee9c77 | 193 | void endX_release(void){ |
| lucasfontenla | 15:4604a7ee9c77 | 194 | printf("X - release\n\r"); |
| lucasfontenla | 16:3adab04d42a6 | 195 | x_block_min = 0; |
| lucasfontenla | 16:3adab04d42a6 | 196 | x_block_max = 0; |
| lucasfontenla | 12:801e58a7137c | 197 | } |
| lucasfontenla | 16:3adab04d42a6 | 198 | |
| lucasfontenla | 15:4604a7ee9c77 | 199 | void endY_press(void){ |
| lucasfontenla | 16:3adab04d42a6 | 200 | printf("Y - press\n\r"); |
| lucasfontenla | 16:3adab04d42a6 | 201 | if(dirY == y_plus){ |
| lucasfontenla | 16:3adab04d42a6 | 202 | y_block_max = 1; |
| lucasfontenla | 16:3adab04d42a6 | 203 | } |
| lucasfontenla | 16:3adab04d42a6 | 204 | else { |
| lucasfontenla | 16:3adab04d42a6 | 205 | y_block_min = 1; |
| lucasfontenla | 16:3adab04d42a6 | 206 | } |
| lucasfontenla | 15:4604a7ee9c77 | 207 | } |
| lucasfontenla | 15:4604a7ee9c77 | 208 | void endY_release(void){ |
| lucasfontenla | 15:4604a7ee9c77 | 209 | printf("Y - release\n\r"); |
| lucasfontenla | 16:3adab04d42a6 | 210 | y_block_min = 0; |
| lucasfontenla | 16:3adab04d42a6 | 211 | y_block_max = 0; |
| lucasfontenla | 9:d771a576e4fc | 212 | } |
| lucasfontenla | 9:d771a576e4fc | 213 | |
| lucasfontenla | 15:4604a7ee9c77 | 214 | void endZ_press(void){ |
| lucasfontenla | 15:4604a7ee9c77 | 215 | printf("Z - press\n\r"); |
| lucasfontenla | 16:3adab04d42a6 | 216 | if(dirZ == z_plus){ |
| lucasfontenla | 16:3adab04d42a6 | 217 | z_block_max = 1; |
| lucasfontenla | 16:3adab04d42a6 | 218 | } |
| lucasfontenla | 16:3adab04d42a6 | 219 | else { |
| lucasfontenla | 16:3adab04d42a6 | 220 | z_block_min = 1; |
| lucasfontenla | 16:3adab04d42a6 | 221 | } |
| lucasfontenla | 9:d771a576e4fc | 222 | } |
| lucasfontenla | 15:4604a7ee9c77 | 223 | void endZ_release(void){ |
| lucasfontenla | 15:4604a7ee9c77 | 224 | printf("Z - release\n\r"); |
| lucasfontenla | 16:3adab04d42a6 | 225 | z_block_min = 0; |
| lucasfontenla | 16:3adab04d42a6 | 226 | z_block_max = 0; |
| lucasfontenla | 9:d771a576e4fc | 227 | } |
| lucasfontenla | 7:27ea14e58788 | 228 | |
| lucasfontenla | 5:0cbf491f703d | 229 | 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 | 230 | float time = 1.0/pps/2.0; |
| lucasfontenla | 5:0cbf491f703d | 231 | |
| lucasfontenla | 5:0cbf491f703d | 232 | dirX = x_dir; |
| lucasfontenla | 5:0cbf491f703d | 233 | dirY = y_dir; |
| lucasfontenla | 5:0cbf491f703d | 234 | dirZ = z_dir; |
| lucasfontenla | 5:0cbf491f703d | 235 | |
| lucasfontenla | 16:3adab04d42a6 | 236 | // X |
| lucasfontenla | 16:3adab04d42a6 | 237 | if((x_dir == x_plus) && x_block_max){ |
| lucasfontenla | 16:3adab04d42a6 | 238 | x_step = 0; |
| lucasfontenla | 16:3adab04d42a6 | 239 | } |
| lucasfontenla | 16:3adab04d42a6 | 240 | if((x_dir == x_minus) && x_block_min){ |
| lucasfontenla | 12:801e58a7137c | 241 | x_step = 0; |
| lucasfontenla | 12:801e58a7137c | 242 | } |
| lucasfontenla | 12:801e58a7137c | 243 | |
| lucasfontenla | 16:3adab04d42a6 | 244 | // Y |
| lucasfontenla | 16:3adab04d42a6 | 245 | if((y_dir == y_plus) && y_block_max){ |
| lucasfontenla | 16:3adab04d42a6 | 246 | y_step = 0; |
| lucasfontenla | 16:3adab04d42a6 | 247 | } |
| lucasfontenla | 16:3adab04d42a6 | 248 | if((y_dir == y_minus) && y_block_min){ |
| lucasfontenla | 11:0e22a6fc7f87 | 249 | y_step = 0; |
| lucasfontenla | 11:0e22a6fc7f87 | 250 | } |
| lucasfontenla | 16:3adab04d42a6 | 251 | |
| lucasfontenla | 16:3adab04d42a6 | 252 | // Z |
| lucasfontenla | 16:3adab04d42a6 | 253 | if((z_dir == z_plus) && z_block_max){ |
| lucasfontenla | 16:3adab04d42a6 | 254 | z_step = 0; |
| lucasfontenla | 16:3adab04d42a6 | 255 | } |
| lucasfontenla | 16:3adab04d42a6 | 256 | if((z_dir == z_minus) && z_block_min){ |
| lucasfontenla | 15:4604a7ee9c77 | 257 | z_step = 0; |
| lucasfontenla | 11:0e22a6fc7f87 | 258 | } |
| lucasfontenla | 11:0e22a6fc7f87 | 259 | |
| lucasfontenla | 5:0cbf491f703d | 260 | int max_val; |
| lucasfontenla | 5:0cbf491f703d | 261 | |
| lucasfontenla | 6:af32c68ff4cf | 262 | // maior número de passos ------- |
| lucasfontenla | 5:0cbf491f703d | 263 | if(x_step > y_step){ |
| lucasfontenla | 5:0cbf491f703d | 264 | if(x_step > z_step){ |
| lucasfontenla | 5:0cbf491f703d | 265 | max_val = x_step; |
| lucasfontenla | 4:48717b42eab2 | 266 | } |
| lucasfontenla | 4:48717b42eab2 | 267 | else { |
| lucasfontenla | 5:0cbf491f703d | 268 | max_val = z_step; |
| lucasfontenla | 4:48717b42eab2 | 269 | } |
| lucasfontenla | 5:0cbf491f703d | 270 | } else if(y_step > z_step){ |
| lucasfontenla | 5:0cbf491f703d | 271 | max_val = y_step; |
| lucasfontenla | 5:0cbf491f703d | 272 | } |
| lucasfontenla | 5:0cbf491f703d | 273 | else { |
| lucasfontenla | 5:0cbf491f703d | 274 | max_val = z_step; |
| lucasfontenla | 5:0cbf491f703d | 275 | } |
| lucasfontenla | 5:0cbf491f703d | 276 | |
| lucasfontenla | 5:0cbf491f703d | 277 | for(int i = 0; i < max_val; i++){ |
| lucasfontenla | 6:af32c68ff4cf | 278 | if(i >= x_step){ |
| lucasfontenla | 5:0cbf491f703d | 279 | stepX = 0; |
| lucasfontenla | 5:0cbf491f703d | 280 | } |
| lucasfontenla | 5:0cbf491f703d | 281 | else { stepX = 1; } |
| lucasfontenla | 4:48717b42eab2 | 282 | |
| lucasfontenla | 6:af32c68ff4cf | 283 | if(i >= y_step){ |
| lucasfontenla | 5:0cbf491f703d | 284 | stepY = 0; |
| lucasfontenla | 5:0cbf491f703d | 285 | } |
| lucasfontenla | 11:0e22a6fc7f87 | 286 | else { |
| lucasfontenla | 11:0e22a6fc7f87 | 287 | stepY = 1;} |
| lucasfontenla | 5:0cbf491f703d | 288 | |
| lucasfontenla | 6:af32c68ff4cf | 289 | if(i >= z_step){ |
| lucasfontenla | 5:0cbf491f703d | 290 | stepZ = 0; |
| lucasfontenla | 5:0cbf491f703d | 291 | } |
| lucasfontenla | 5:0cbf491f703d | 292 | else { stepZ = 1; } |
| lucasfontenla | 5:0cbf491f703d | 293 | |
| lucasfontenla | 5:0cbf491f703d | 294 | wait(time); |
| lucasfontenla | 4:48717b42eab2 | 295 | stepX = 0; |
| lucasfontenla | 4:48717b42eab2 | 296 | stepY = 0; |
| lucasfontenla | 4:48717b42eab2 | 297 | stepZ = 0; |
| lucasfontenla | 5:0cbf491f703d | 298 | wait(time); |
| lucasfontenla | 4:48717b42eab2 | 299 | } |
| lucasfontenla | 17:55e6270adab5 | 300 | |
| lucasfontenla | 17:55e6270adab5 | 301 | if(dirX == x_minus){ |
| lucasfontenla | 17:55e6270adab5 | 302 | totalX-=x_step; |
| lucasfontenla | 17:55e6270adab5 | 303 | } |
| lucasfontenla | 17:55e6270adab5 | 304 | else { totalX+=x_step; } |
| lucasfontenla | 17:55e6270adab5 | 305 | |
| lucasfontenla | 17:55e6270adab5 | 306 | if(dirY == y_minus){ |
| lucasfontenla | 17:55e6270adab5 | 307 | totalY-=y_step; |
| lucasfontenla | 17:55e6270adab5 | 308 | } else { totalY+=y_step; } |
| lucasfontenla | 17:55e6270adab5 | 309 | |
| lucasfontenla | 17:55e6270adab5 | 310 | if(dirZ == z_minus){ |
| lucasfontenla | 17:55e6270adab5 | 311 | totalZ-=z_step; |
| lucasfontenla | 17:55e6270adab5 | 312 | } else { totalZ+=z_step; } |
| lucasfontenla | 5:0cbf491f703d | 313 | } |
| lucasfontenla | 17:55e6270adab5 | 314 | |
| lucasfontenla | 12:801e58a7137c | 315 | void zeroX(int pps){ |
| lucasfontenla | 25:7b3826dddcba | 316 | printf("Zero X\n\r"); |
| lucasfontenla | 12:801e58a7137c | 317 | float time = 1.0/pps/2.0; |
| lucasfontenla | 12:801e58a7137c | 318 | |
| lucasfontenla | 17:55e6270adab5 | 319 | if(dirX != x_minus){ |
| lucasfontenla | 17:55e6270adab5 | 320 | dirX = x_minus; |
| lucasfontenla | 17:55e6270adab5 | 321 | } |
| lucasfontenla | 17:55e6270adab5 | 322 | |
| lucasfontenla | 17:55e6270adab5 | 323 | while(!x_block_min){ |
| lucasfontenla | 12:801e58a7137c | 324 | stepX = 1; |
| lucasfontenla | 12:801e58a7137c | 325 | wait(time); |
| lucasfontenla | 12:801e58a7137c | 326 | stepX = 0; |
| lucasfontenla | 12:801e58a7137c | 327 | wait(time); |
| lucasfontenla | 12:801e58a7137c | 328 | } |
| lucasfontenla | 12:801e58a7137c | 329 | |
| lucasfontenla | 12:801e58a7137c | 330 | printf("X zero\n\r"); |
| lucasfontenla | 12:801e58a7137c | 331 | |
| lucasfontenla | 17:55e6270adab5 | 332 | totalX = 0; |
| lucasfontenla | 26:f891ff6beb33 | 333 | X = steps_to_distance(totalX, xPitch); |
| lucasfontenla | 26:f891ff6beb33 | 334 | Y = steps_to_distance(totalY, yPitch); |
| lucasfontenla | 26:f891ff6beb33 | 335 | Z = steps_to_distance(totalZ, zPitch); |
| lucasfontenla | 26:f891ff6beb33 | 336 | ihm_class.send_position(X, Y, Z); |
| lucasfontenla | 12:801e58a7137c | 337 | } |
| lucasfontenla | 11:0e22a6fc7f87 | 338 | void zeroY(int pps){ |
| lucasfontenla | 25:7b3826dddcba | 339 | printf("Zero Y\n\r"); |
| lucasfontenla | 11:0e22a6fc7f87 | 340 | float time = 1.0/pps/2.0; |
| lucasfontenla | 11:0e22a6fc7f87 | 341 | |
| lucasfontenla | 17:55e6270adab5 | 342 | if(dirY != y_minus){ |
| lucasfontenla | 17:55e6270adab5 | 343 | dirY = y_minus; |
| lucasfontenla | 17:55e6270adab5 | 344 | } |
| lucasfontenla | 17:55e6270adab5 | 345 | |
| lucasfontenla | 17:55e6270adab5 | 346 | while(!y_block_min){ |
| lucasfontenla | 11:0e22a6fc7f87 | 347 | stepY = 1; |
| lucasfontenla | 11:0e22a6fc7f87 | 348 | wait(time); |
| lucasfontenla | 11:0e22a6fc7f87 | 349 | stepY = 0; |
| lucasfontenla | 11:0e22a6fc7f87 | 350 | wait(time); |
| lucasfontenla | 11:0e22a6fc7f87 | 351 | } |
| lucasfontenla | 11:0e22a6fc7f87 | 352 | |
| lucasfontenla | 11:0e22a6fc7f87 | 353 | printf("Y zero\n\r"); |
| lucasfontenla | 11:0e22a6fc7f87 | 354 | |
| lucasfontenla | 17:55e6270adab5 | 355 | totalY = 0; |
| lucasfontenla | 26:f891ff6beb33 | 356 | X = steps_to_distance(totalX, xPitch); |
| lucasfontenla | 26:f891ff6beb33 | 357 | Y = steps_to_distance(totalY, yPitch); |
| lucasfontenla | 26:f891ff6beb33 | 358 | Z = steps_to_distance(totalZ, zPitch); |
| lucasfontenla | 26:f891ff6beb33 | 359 | ihm_class.send_position(X, Y, Z); |
| lucasfontenla | 17:55e6270adab5 | 360 | } |
| lucasfontenla | 11:0e22a6fc7f87 | 361 | |
| lucasfontenla | 25:7b3826dddcba | 362 | void zeroZ(int pps){ |
| lucasfontenla | 25:7b3826dddcba | 363 | printf("Zero Z\n\r"); |
| lucasfontenla | 25:7b3826dddcba | 364 | float time = 1.0/pps/2.0; |
| lucasfontenla | 25:7b3826dddcba | 365 | |
| lucasfontenla | 25:7b3826dddcba | 366 | if(dirZ != z_minus){ |
| lucasfontenla | 25:7b3826dddcba | 367 | dirZ = z_minus; |
| lucasfontenla | 25:7b3826dddcba | 368 | } |
| lucasfontenla | 25:7b3826dddcba | 369 | |
| lucasfontenla | 25:7b3826dddcba | 370 | while(!z_block_min){ |
| lucasfontenla | 25:7b3826dddcba | 371 | stepZ = 1; |
| lucasfontenla | 25:7b3826dddcba | 372 | wait(time); |
| lucasfontenla | 25:7b3826dddcba | 373 | stepZ = 0; |
| lucasfontenla | 25:7b3826dddcba | 374 | wait(time); |
| lucasfontenla | 25:7b3826dddcba | 375 | } |
| lucasfontenla | 25:7b3826dddcba | 376 | |
| lucasfontenla | 25:7b3826dddcba | 377 | printf("Z zero\n\r"); |
| lucasfontenla | 25:7b3826dddcba | 378 | |
| lucasfontenla | 25:7b3826dddcba | 379 | totalZ = 0; |
| lucasfontenla | 26:f891ff6beb33 | 380 | X = steps_to_distance(totalX, xPitch); |
| lucasfontenla | 26:f891ff6beb33 | 381 | Y = steps_to_distance(totalY, yPitch); |
| lucasfontenla | 26:f891ff6beb33 | 382 | Z = steps_to_distance(totalZ, zPitch); |
| lucasfontenla | 26:f891ff6beb33 | 383 | ihm_class.send_position(X, Y, Z); |
| lucasfontenla | 25:7b3826dddcba | 384 | } |
| lucasfontenla | 25:7b3826dddcba | 385 | |
| lucasfontenla | 17:55e6270adab5 | 386 | float steps_to_distance(int steps, float pitch){ |
| lucasfontenla | 7:27ea14e58788 | 387 | float delta_S = (((float)steps)/steps_by_rotation)*pitch; |
| lucasfontenla | 7:27ea14e58788 | 388 | return delta_S; |
| lucasfontenla | 7:27ea14e58788 | 389 | } |
| lucasfontenla | 17:55e6270adab5 | 390 | |
| lucasfontenla | 17:55e6270adab5 | 391 | int distance_to_steps(float distance, float pitch){ |
| lucasfontenla | 17:55e6270adab5 | 392 | int steps = (int)(distance*steps_by_rotation/pitch); |
| lucasfontenla | 17:55e6270adab5 | 393 | return steps; |
| lucasfontenla | 17:55e6270adab5 | 394 | } |
| lucasfontenla | 17:55e6270adab5 | 395 | |
| lucasfontenla | 17:55e6270adab5 | 396 | void printDistance(void){ |
| lucasfontenla | 17:55e6270adab5 | 397 | X = steps_to_distance(totalX, xPitch); |
| lucasfontenla | 17:55e6270adab5 | 398 | Y = steps_to_distance(totalY, yPitch); |
| lucasfontenla | 17:55e6270adab5 | 399 | Z = steps_to_distance(totalZ, zPitch); |
| lucasfontenla | 17:55e6270adab5 | 400 | printf("X: %.2f Y: %.2f Z: %.2f\r\n", X, Y, Z); |
| lucasfontenla | 18:174b4ff351b5 | 401 | } |
| lucasfontenla | 18:174b4ff351b5 | 402 | |
| lucasfontenla | 22:000890b38b32 | 403 | void savePoint(string speed, string mode){ |
| lucasfontenla | 26:f891ff6beb33 | 404 | if(saved == 0){ |
| lucasfontenla | 26:f891ff6beb33 | 405 | totalX = 0; |
| lucasfontenla | 26:f891ff6beb33 | 406 | totalY = 0; |
| lucasfontenla | 26:f891ff6beb33 | 407 | totalZ = 0; |
| lucasfontenla | 26:f891ff6beb33 | 408 | } |
| lucasfontenla | 26:f891ff6beb33 | 409 | |
| lucasfontenla | 26:f891ff6beb33 | 410 | X = steps_to_distance(totalX, xPitch); |
| lucasfontenla | 26:f891ff6beb33 | 411 | Y = steps_to_distance(totalY, yPitch); |
| lucasfontenla | 26:f891ff6beb33 | 412 | Z = steps_to_distance(totalZ, zPitch); |
| lucasfontenla | 26:f891ff6beb33 | 413 | |
| lucasfontenla | 26:f891ff6beb33 | 414 | ihm_class.send_position(X, Y, Z); |
| lucasfontenla | 26:f891ff6beb33 | 415 | |
| lucasfontenla | 26:f891ff6beb33 | 416 | printf("\nSave selected\n\r"); |
| lucasfontenla | 26:f891ff6beb33 | 417 | if(saved >= maxPoints){ |
| lucasfontenla | 26:f891ff6beb33 | 418 | printf("Max points reached\n\r"); |
| lucasfontenla | 26:f891ff6beb33 | 419 | } else{ |
| lucasfontenla | 26:f891ff6beb33 | 420 | X = steps_to_distance(totalX, xPitch); |
| lucasfontenla | 26:f891ff6beb33 | 421 | Y = steps_to_distance(totalY, yPitch); |
| lucasfontenla | 26:f891ff6beb33 | 422 | Z = steps_to_distance(totalZ, zPitch); |
| lucasfontenla | 26:f891ff6beb33 | 423 | |
| lucasfontenla | 26:f891ff6beb33 | 424 | points[0][saved] = X; |
| lucasfontenla | 26:f891ff6beb33 | 425 | points[1][saved] = Y; |
| lucasfontenla | 26:f891ff6beb33 | 426 | points[2][saved] = Z; |
| lucasfontenla | 26:f891ff6beb33 | 427 | path[0][saved] = atof(speed.c_str()); |
| lucasfontenla | 26:f891ff6beb33 | 428 | path[1][saved] = atof(mode.c_str()); |
| lucasfontenla | 26:f891ff6beb33 | 429 | saved+=1; |
| lucasfontenla | 26:f891ff6beb33 | 430 | } |
| lucasfontenla | 26:f891ff6beb33 | 431 | // save code |
| lucasfontenla | 26:f891ff6beb33 | 432 | printf("Saved --> X: %.2f Y: %.2f Z: %.2f\r\n", X, Y, Z); |
| lucasfontenla | 26:f891ff6beb33 | 433 | printf("Path --> Speed: %.2f Mode: %.2f\n\r", path[0][saved-1], path[1][saved-1]); |
| lucasfontenla | 26:f891ff6beb33 | 434 | resting_points = maxPoints-saved; |
| lucasfontenla | 26:f891ff6beb33 | 435 | printf("Pontos restantes: %d\n\r", resting_points); |
| lucasfontenla | 18:174b4ff351b5 | 436 | } |
| lucasfontenla | 18:174b4ff351b5 | 437 | |
| lucasfontenla | 23:d64d37561b4a | 438 | void jog(void){ |
| lucasfontenla | 26:f891ff6beb33 | 439 | enable = 0; |
| lucasfontenla | 23:d64d37561b4a | 440 | kill_jog = 0; |
| lucasfontenla | 23:d64d37561b4a | 441 | printf("JOG Selected %d\n\r", kill_jog); |
| lucasfontenla | 23:d64d37561b4a | 442 | |
| lucasfontenla | 24:92f612ca218b | 443 | t.reset(); |
| lucasfontenla | 24:92f612ca218b | 444 | |
| lucasfontenla | 23:d64d37561b4a | 445 | while(1){ |
| lucasfontenla | 23:d64d37561b4a | 446 | if(ihm.readable()){ |
| lucasfontenla | 23:d64d37561b4a | 447 | readSerial(); |
| lucasfontenla | 23:d64d37561b4a | 448 | } |
| lucasfontenla | 23:d64d37561b4a | 449 | valX = joyX; |
| lucasfontenla | 23:d64d37561b4a | 450 | valY = joyY; |
| lucasfontenla | 23:d64d37561b4a | 451 | |
| lucasfontenla | 26:f891ff6beb33 | 452 | if(valX > 0.8 && !toggle){ |
| lucasfontenla | 26:f891ff6beb33 | 453 | if(x_dir != x_minus){ |
| lucasfontenla | 26:f891ff6beb33 | 454 | x_dir = x_minus; |
| lucasfontenla | 23:d64d37561b4a | 455 | } |
| lucasfontenla | 23:d64d37561b4a | 456 | activeX = 1; |
| lucasfontenla | 23:d64d37561b4a | 457 | } |
| lucasfontenla | 26:f891ff6beb33 | 458 | else if(valX < 0.3 && !toggle){ |
| lucasfontenla | 26:f891ff6beb33 | 459 | if(x_dir != x_plus){ |
| lucasfontenla | 26:f891ff6beb33 | 460 | x_dir = x_plus; |
| lucasfontenla | 23:d64d37561b4a | 461 | } |
| lucasfontenla | 23:d64d37561b4a | 462 | activeX = 1; |
| lucasfontenla | 23:d64d37561b4a | 463 | } |
| lucasfontenla | 23:d64d37561b4a | 464 | else{ |
| lucasfontenla | 23:d64d37561b4a | 465 | activeX = 0; |
| lucasfontenla | 23:d64d37561b4a | 466 | } |
| lucasfontenla | 23:d64d37561b4a | 467 | |
| lucasfontenla | 23:d64d37561b4a | 468 | //---------------------------------------------------------------------- |
| lucasfontenla | 23:d64d37561b4a | 469 | |
| lucasfontenla | 26:f891ff6beb33 | 470 | if(valY > 0.8 && !toggle){ |
| lucasfontenla | 23:d64d37561b4a | 471 | if(y_dir != y_plus){ |
| lucasfontenla | 23:d64d37561b4a | 472 | y_dir = y_plus; |
| lucasfontenla | 23:d64d37561b4a | 473 | } |
| lucasfontenla | 23:d64d37561b4a | 474 | activeY = 1; |
| lucasfontenla | 23:d64d37561b4a | 475 | } |
| lucasfontenla | 26:f891ff6beb33 | 476 | else if(valY < 0.3 && !toggle){ |
| lucasfontenla | 23:d64d37561b4a | 477 | if(y_dir != y_minus){ |
| lucasfontenla | 23:d64d37561b4a | 478 | y_dir = y_minus; |
| lucasfontenla | 23:d64d37561b4a | 479 | } |
| lucasfontenla | 23:d64d37561b4a | 480 | activeY = 1; |
| lucasfontenla | 23:d64d37561b4a | 481 | } |
| lucasfontenla | 23:d64d37561b4a | 482 | else{ |
| lucasfontenla | 23:d64d37561b4a | 483 | activeY = 0; |
| lucasfontenla | 23:d64d37561b4a | 484 | } |
| lucasfontenla | 23:d64d37561b4a | 485 | |
| lucasfontenla | 23:d64d37561b4a | 486 | //---------------------------------------------------------------------- |
| lucasfontenla | 23:d64d37561b4a | 487 | |
| lucasfontenla | 26:f891ff6beb33 | 488 | if(valY > 0.8 && toggle){ |
| lucasfontenla | 26:f891ff6beb33 | 489 | if(z_dir != z_plus){ |
| lucasfontenla | 26:f891ff6beb33 | 490 | z_dir = z_plus; |
| lucasfontenla | 23:d64d37561b4a | 491 | } |
| lucasfontenla | 23:d64d37561b4a | 492 | activeZ = 1; |
| lucasfontenla | 23:d64d37561b4a | 493 | } |
| lucasfontenla | 26:f891ff6beb33 | 494 | else if(valY < 0.3 && toggle){ |
| lucasfontenla | 26:f891ff6beb33 | 495 | if(z_dir != z_minus){ |
| lucasfontenla | 26:f891ff6beb33 | 496 | z_dir = z_minus; |
| lucasfontenla | 23:d64d37561b4a | 497 | } |
| lucasfontenla | 23:d64d37561b4a | 498 | activeZ = 1; |
| lucasfontenla | 23:d64d37561b4a | 499 | } |
| lucasfontenla | 23:d64d37561b4a | 500 | else { |
| lucasfontenla | 23:d64d37561b4a | 501 | activeZ = 0; |
| lucasfontenla | 23:d64d37561b4a | 502 | } |
| lucasfontenla | 23:d64d37561b4a | 503 | |
| lucasfontenla | 23:d64d37561b4a | 504 | //---------------------------------------------------------------------- |
| lucasfontenla | 23:d64d37561b4a | 505 | |
| lucasfontenla | 23:d64d37561b4a | 506 | move(ppsMax, x_dir, y_dir, z_dir, activeX, activeY, activeZ); |
| lucasfontenla | 23:d64d37561b4a | 507 | X = steps_to_distance(totalX, xPitch); |
| lucasfontenla | 23:d64d37561b4a | 508 | Y = steps_to_distance(totalY, yPitch); |
| lucasfontenla | 23:d64d37561b4a | 509 | Z = steps_to_distance(totalZ, zPitch); |
| lucasfontenla | 26:f891ff6beb33 | 510 | /* |
| lucasfontenla | 24:92f612ca218b | 511 | if(t.read() > sendTime){ |
| lucasfontenla | 26:f891ff6beb33 | 512 | //printf("X: %.2f Y: %.2f Z: %.2f\r\n", X, Y, Z); |
| lucasfontenla | 24:92f612ca218b | 513 | X = steps_to_distance(totalX, xPitch); |
| lucasfontenla | 24:92f612ca218b | 514 | Y = steps_to_distance(totalY, yPitch); |
| lucasfontenla | 24:92f612ca218b | 515 | Z = steps_to_distance(totalZ, zPitch); |
| lucasfontenla | 24:92f612ca218b | 516 | ihm_class.send_position(X, Y, Z); |
| lucasfontenla | 24:92f612ca218b | 517 | t.reset(); |
| lucasfontenla | 26:f891ff6beb33 | 518 | }*/ |
| lucasfontenla | 23:d64d37561b4a | 519 | |
| lucasfontenla | 23:d64d37561b4a | 520 | if(kill_jog){ |
| lucasfontenla | 23:d64d37561b4a | 521 | break; |
| lucasfontenla | 23:d64d37561b4a | 522 | } |
| lucasfontenla | 26:f891ff6beb33 | 523 | |
| lucasfontenla | 26:f891ff6beb33 | 524 | if(toggle){ |
| lucasfontenla | 26:f891ff6beb33 | 525 | if(!ledZ){ |
| lucasfontenla | 26:f891ff6beb33 | 526 | ledZ = 1; |
| lucasfontenla | 26:f891ff6beb33 | 527 | ledXY = 0; |
| lucasfontenla | 26:f891ff6beb33 | 528 | } |
| lucasfontenla | 26:f891ff6beb33 | 529 | } else { |
| lucasfontenla | 26:f891ff6beb33 | 530 | if(!ledXY){ |
| lucasfontenla | 26:f891ff6beb33 | 531 | ledXY = 1; |
| lucasfontenla | 26:f891ff6beb33 | 532 | ledZ = 0; |
| lucasfontenla | 26:f891ff6beb33 | 533 | } |
| lucasfontenla | 26:f891ff6beb33 | 534 | } |
| lucasfontenla | 23:d64d37561b4a | 535 | } |
| lucasfontenla | 23:d64d37561b4a | 536 | } |
| lucasfontenla | 23:d64d37561b4a | 537 | |
| lucasfontenla | 23:d64d37561b4a | 538 | void automatic_run(int total_points){ |
| lucasfontenla | 26:f891ff6beb33 | 539 | enable = 0; |
| lucasfontenla | 23:d64d37561b4a | 540 | printf("\nStarting automatic routine\n\r"); |
| lucasfontenla | 23:d64d37561b4a | 541 | |
| lucasfontenla | 23:d64d37561b4a | 542 | // home |
| lucasfontenla | 23:d64d37561b4a | 543 | x_distance_to_run = points[0][0] - points[0][total_points-1]; |
| lucasfontenla | 23:d64d37561b4a | 544 | y_distance_to_run = points[1][0] - points[1][total_points-1]; |
| lucasfontenla | 23:d64d37561b4a | 545 | z_distance_to_run = points[2][0] - points[2][total_points-1]; |
| lucasfontenla | 23:d64d37561b4a | 546 | |
| lucasfontenla | 23:d64d37561b4a | 547 | printf("\nGoing to first point\n\r"); |
| lucasfontenla | 23:d64d37561b4a | 548 | |
| lucasfontenla | 23:d64d37561b4a | 549 | if(x_distance_to_run < 0){ |
| lucasfontenla | 23:d64d37561b4a | 550 | x_dir = x_minus; |
| lucasfontenla | 23:d64d37561b4a | 551 | x_distance_to_run = -1.0*x_distance_to_run; |
| lucasfontenla | 23:d64d37561b4a | 552 | } else { x_dir = x_plus; } |
| lucasfontenla | 23:d64d37561b4a | 553 | |
| lucasfontenla | 23:d64d37561b4a | 554 | if(y_distance_to_run < 0){ |
| lucasfontenla | 23:d64d37561b4a | 555 | y_dir = y_minus; |
| lucasfontenla | 23:d64d37561b4a | 556 | y_distance_to_run = -1.0*y_distance_to_run; |
| lucasfontenla | 23:d64d37561b4a | 557 | } else { y_dir = y_plus; } |
| lucasfontenla | 23:d64d37561b4a | 558 | |
| lucasfontenla | 23:d64d37561b4a | 559 | if(z_distance_to_run < 0){ |
| lucasfontenla | 23:d64d37561b4a | 560 | z_dir = z_minus; |
| lucasfontenla | 23:d64d37561b4a | 561 | z_distance_to_run = -1.0*z_distance_to_run; |
| lucasfontenla | 23:d64d37561b4a | 562 | } else { z_dir = z_plus; } |
| lucasfontenla | 23:d64d37561b4a | 563 | |
| lucasfontenla | 23:d64d37561b4a | 564 | x_steps_to_run = distance_to_steps(x_distance_to_run, xPitch); |
| lucasfontenla | 23:d64d37561b4a | 565 | y_steps_to_run = distance_to_steps(y_distance_to_run, yPitch); |
| lucasfontenla | 23:d64d37561b4a | 566 | z_steps_to_run = distance_to_steps(z_distance_to_run, zPitch); |
| lucasfontenla | 23:d64d37561b4a | 567 | |
| lucasfontenla | 23:d64d37561b4a | 568 | printf("deltaX: %.2f deltaY: %.2f deltaZ: %.2f\n\r", x_distance_to_run, y_distance_to_run, z_distance_to_run); |
| lucasfontenla | 23:d64d37561b4a | 569 | |
| lucasfontenla | 23:d64d37561b4a | 570 | move(ppsMax, x_dir, y_dir, z_dir, x_steps_to_run, 0, 0); |
| lucasfontenla | 23:d64d37561b4a | 571 | move(ppsMax, x_dir, y_dir, z_dir, 0, y_steps_to_run, 0); |
| lucasfontenla | 23:d64d37561b4a | 572 | move(ppsMax, x_dir, y_dir, z_dir, 0, 0, z_steps_to_run); |
| lucasfontenla | 23:d64d37561b4a | 573 | |
| lucasfontenla | 23:d64d37561b4a | 574 | wait(2); |
| lucasfontenla | 23:d64d37561b4a | 575 | |
| lucasfontenla | 23:d64d37561b4a | 576 | printf("\nSaved points\n\r"); |
| lucasfontenla | 23:d64d37561b4a | 577 | |
| lucasfontenla | 23:d64d37561b4a | 578 | // routine |
| lucasfontenla | 23:d64d37561b4a | 579 | |
| lucasfontenla | 23:d64d37561b4a | 580 | for(int i = 1; i < total_points; i++){ |
| lucasfontenla | 23:d64d37561b4a | 581 | x_distance_to_run = points[0][i]-points[0][i-1]; |
| lucasfontenla | 23:d64d37561b4a | 582 | y_distance_to_run = points[1][i]-points[1][i-1]; |
| lucasfontenla | 23:d64d37561b4a | 583 | z_distance_to_run = points[2][i]-points[2][i-1]; |
| lucasfontenla | 23:d64d37561b4a | 584 | |
| lucasfontenla | 23:d64d37561b4a | 585 | printf("deltaX: %.2f deltaY: %.2f deltaZ: %.2f\n\r", x_distance_to_run, y_distance_to_run, z_distance_to_run); |
| lucasfontenla | 23:d64d37561b4a | 586 | |
| lucasfontenla | 23:d64d37561b4a | 587 | if(x_distance_to_run < 0){ |
| lucasfontenla | 23:d64d37561b4a | 588 | x_dir = x_minus; |
| lucasfontenla | 23:d64d37561b4a | 589 | x_distance_to_run = -1.0*x_distance_to_run; |
| lucasfontenla | 23:d64d37561b4a | 590 | } else { x_dir = x_plus; } |
| lucasfontenla | 23:d64d37561b4a | 591 | |
| lucasfontenla | 23:d64d37561b4a | 592 | if(y_distance_to_run < 0){ |
| lucasfontenla | 23:d64d37561b4a | 593 | y_dir = y_minus; |
| lucasfontenla | 23:d64d37561b4a | 594 | y_distance_to_run = -1.0*y_distance_to_run; |
| lucasfontenla | 23:d64d37561b4a | 595 | } else { y_dir = y_plus; } |
| lucasfontenla | 23:d64d37561b4a | 596 | |
| lucasfontenla | 23:d64d37561b4a | 597 | if(z_distance_to_run < 0){ |
| lucasfontenla | 23:d64d37561b4a | 598 | z_dir = z_minus; |
| lucasfontenla | 23:d64d37561b4a | 599 | z_distance_to_run = -1.0*z_distance_to_run; |
| lucasfontenla | 23:d64d37561b4a | 600 | } else { z_dir = z_plus; } |
| lucasfontenla | 23:d64d37561b4a | 601 | |
| lucasfontenla | 23:d64d37561b4a | 602 | x_steps_to_run = distance_to_steps(x_distance_to_run, xPitch); |
| lucasfontenla | 23:d64d37561b4a | 603 | y_steps_to_run = distance_to_steps(y_distance_to_run, yPitch); |
| lucasfontenla | 23:d64d37561b4a | 604 | z_steps_to_run = distance_to_steps(z_distance_to_run, zPitch); |
| lucasfontenla | 23:d64d37561b4a | 605 | |
| lucasfontenla | 23:d64d37561b4a | 606 | move(ppsMax, x_dir, y_dir, z_dir, x_steps_to_run, 0, 0); |
| lucasfontenla | 23:d64d37561b4a | 607 | move(ppsMax, x_dir, y_dir, z_dir, 0, y_steps_to_run, 0); |
| lucasfontenla | 23:d64d37561b4a | 608 | move(ppsMax, x_dir, y_dir, z_dir, 0, 0, z_steps_to_run); |
| lucasfontenla | 23:d64d37561b4a | 609 | } |
| lucasfontenla | 23:d64d37561b4a | 610 | killJog(); |
| lucasfontenla | 23:d64d37561b4a | 611 | printf("Done\n\r"); |
| lucasfontenla | 26:f891ff6beb33 | 612 | wait(0.1); |
| lucasfontenla | 26:f891ff6beb33 | 613 | ihm_class.action_complete(); |
| lucasfontenla | 26:f891ff6beb33 | 614 | enable = 1; |
| lucasfontenla | 22:000890b38b32 | 615 | } |
| lucasfontenla | 22:000890b38b32 | 616 | |
| lucasfontenla | 23:d64d37561b4a | 617 | void killJog(void){ |
| lucasfontenla | 26:f891ff6beb33 | 618 | enable = 1; |
| lucasfontenla | 23:d64d37561b4a | 619 | kill_jog = 1; |
| lucasfontenla | 26:f891ff6beb33 | 620 | ledZ = 0; |
| lucasfontenla | 26:f891ff6beb33 | 621 | ledXY = 0; |
| lucasfontenla | 23:d64d37561b4a | 622 | printf("Jog killed\n\r"); |
| lucasfontenla | 23:d64d37561b4a | 623 | } |
| lucasfontenla | 23:d64d37561b4a | 624 | |
| lucasfontenla | 25:7b3826dddcba | 625 | void cancelProgram(void){ |
| lucasfontenla | 25:7b3826dddcba | 626 | saved = 0; |
| lucasfontenla | 25:7b3826dddcba | 627 | killJog(); |
| lucasfontenla | 25:7b3826dddcba | 628 | } |
| lucasfontenla | 25:7b3826dddcba | 629 | |
| lucasfontenla | 23:d64d37561b4a | 630 | void readSerial(void){ |
| lucasfontenla | 22:000890b38b32 | 631 | char recv; |
| lucasfontenla | 25:7b3826dddcba | 632 | char last_recv; |
| lucasfontenla | 22:000890b38b32 | 633 | |
| lucasfontenla | 22:000890b38b32 | 634 | //save |
| lucasfontenla | 22:000890b38b32 | 635 | int save = 0; |
| lucasfontenla | 22:000890b38b32 | 636 | int mode = 0; |
| lucasfontenla | 22:000890b38b32 | 637 | int speed = 0; |
| lucasfontenla | 22:000890b38b32 | 638 | string mode_recv; |
| lucasfontenla | 22:000890b38b32 | 639 | string speed_str; |
| lucasfontenla | 22:000890b38b32 | 640 | |
| lucasfontenla | 23:d64d37561b4a | 641 | //begin |
| lucasfontenla | 23:d64d37561b4a | 642 | int begin = 0; |
| lucasfontenla | 23:d64d37561b4a | 643 | |
| lucasfontenla | 23:d64d37561b4a | 644 | //jog |
| lucasfontenla | 23:d64d37561b4a | 645 | int jog_mode = 0; |
| lucasfontenla | 23:d64d37561b4a | 646 | int kill_jog_mode = 0; |
| lucasfontenla | 23:d64d37561b4a | 647 | |
| lucasfontenla | 25:7b3826dddcba | 648 | //reset |
| lucasfontenla | 25:7b3826dddcba | 649 | int reset = 0; |
| lucasfontenla | 25:7b3826dddcba | 650 | |
| lucasfontenla | 25:7b3826dddcba | 651 | //cancel |
| lucasfontenla | 25:7b3826dddcba | 652 | int cancel = 0; |
| lucasfontenla | 25:7b3826dddcba | 653 | |
| lucasfontenla | 25:7b3826dddcba | 654 | //zero |
| lucasfontenla | 25:7b3826dddcba | 655 | int zero = 0; |
| lucasfontenla | 25:7b3826dddcba | 656 | int machine_zero = 0; |
| lucasfontenla | 25:7b3826dddcba | 657 | |
| lucasfontenla | 26:f891ff6beb33 | 658 | //printf("\n\nReceiving...\n\r"); |
| lucasfontenla | 23:d64d37561b4a | 659 | |
| lucasfontenla | 23:d64d37561b4a | 660 | t.reset(); |
| lucasfontenla | 22:000890b38b32 | 661 | |
| lucasfontenla | 22:000890b38b32 | 662 | while(1){ |
| lucasfontenla | 23:d64d37561b4a | 663 | if(t.read()>0.2){ |
| lucasfontenla | 23:d64d37561b4a | 664 | printf("Timeout\n\r"); |
| lucasfontenla | 23:d64d37561b4a | 665 | break; |
| lucasfontenla | 23:d64d37561b4a | 666 | } |
| lucasfontenla | 23:d64d37561b4a | 667 | |
| lucasfontenla | 22:000890b38b32 | 668 | recv = ihm.getc(); |
| lucasfontenla | 22:000890b38b32 | 669 | |
| lucasfontenla | 26:f891ff6beb33 | 670 | //printf("%c\n\r", recv); |
| lucasfontenla | 23:d64d37561b4a | 671 | |
| lucasfontenla | 23:d64d37561b4a | 672 | if(recv == 'x'){ |
| lucasfontenla | 22:000890b38b32 | 673 | if(save){ |
| lucasfontenla | 22:000890b38b32 | 674 | savePoint(speed_str, mode_recv); |
| lucasfontenla | 23:d64d37561b4a | 675 | } else if(begin){ |
| lucasfontenla | 23:d64d37561b4a | 676 | automatic_run(saved); |
| lucasfontenla | 23:d64d37561b4a | 677 | } else if(jog_mode){ |
| lucasfontenla | 23:d64d37561b4a | 678 | if(kill_jog_mode){ |
| lucasfontenla | 23:d64d37561b4a | 679 | killJog(); |
| lucasfontenla | 23:d64d37561b4a | 680 | } else { |
| lucasfontenla | 23:d64d37561b4a | 681 | jog(); |
| lucasfontenla | 23:d64d37561b4a | 682 | } |
| lucasfontenla | 25:7b3826dddcba | 683 | } else if(reset){ |
| lucasfontenla | 25:7b3826dddcba | 684 | NVIC_SystemReset(); |
| lucasfontenla | 25:7b3826dddcba | 685 | } else if(cancel){ |
| lucasfontenla | 25:7b3826dddcba | 686 | cancel = 1; |
| lucasfontenla | 25:7b3826dddcba | 687 | cancelProgram(); |
| lucasfontenla | 25:7b3826dddcba | 688 | } else if(zero){ |
| lucasfontenla | 25:7b3826dddcba | 689 | if(machine_zero){ |
| lucasfontenla | 26:f891ff6beb33 | 690 | goMachineHome(ppsMax); |
| lucasfontenla | 25:7b3826dddcba | 691 | } else if(last_recv == 'X'){ |
| lucasfontenla | 25:7b3826dddcba | 692 | totalX = 0; |
| lucasfontenla | 25:7b3826dddcba | 693 | } else if(last_recv == 'Y'){ |
| lucasfontenla | 25:7b3826dddcba | 694 | totalY = 0; |
| lucasfontenla | 25:7b3826dddcba | 695 | } else if(last_recv == 'Z'){ |
| lucasfontenla | 25:7b3826dddcba | 696 | totalZ = 0; |
| lucasfontenla | 25:7b3826dddcba | 697 | } |
| lucasfontenla | 25:7b3826dddcba | 698 | } |
| lucasfontenla | 22:000890b38b32 | 699 | break; |
| lucasfontenla | 22:000890b38b32 | 700 | } |
| lucasfontenla | 22:000890b38b32 | 701 | |
| lucasfontenla | 22:000890b38b32 | 702 | if(recv == 's'){ |
| lucasfontenla | 22:000890b38b32 | 703 | save = 1; |
| lucasfontenla | 22:000890b38b32 | 704 | } |
| lucasfontenla | 22:000890b38b32 | 705 | |
| lucasfontenla | 23:d64d37561b4a | 706 | if(recv == 'b'){ |
| lucasfontenla | 23:d64d37561b4a | 707 | begin = 1; |
| lucasfontenla | 23:d64d37561b4a | 708 | } |
| lucasfontenla | 23:d64d37561b4a | 709 | |
| lucasfontenla | 23:d64d37561b4a | 710 | if(recv == 'j'){ |
| lucasfontenla | 23:d64d37561b4a | 711 | jog_mode = 1; |
| lucasfontenla | 23:d64d37561b4a | 712 | } |
| lucasfontenla | 23:d64d37561b4a | 713 | |
| lucasfontenla | 25:7b3826dddcba | 714 | if(recv == 'r'){ |
| lucasfontenla | 25:7b3826dddcba | 715 | reset = 1; |
| lucasfontenla | 25:7b3826dddcba | 716 | } |
| lucasfontenla | 25:7b3826dddcba | 717 | |
| lucasfontenla | 25:7b3826dddcba | 718 | if(recv == 'c'){ |
| lucasfontenla | 25:7b3826dddcba | 719 | cancel = 1; |
| lucasfontenla | 25:7b3826dddcba | 720 | } |
| lucasfontenla | 25:7b3826dddcba | 721 | |
| lucasfontenla | 25:7b3826dddcba | 722 | if(recv == 'z'){ |
| lucasfontenla | 25:7b3826dddcba | 723 | zero = 1; |
| lucasfontenla | 25:7b3826dddcba | 724 | } |
| lucasfontenla | 25:7b3826dddcba | 725 | |
| lucasfontenla | 22:000890b38b32 | 726 | if(save){ |
| lucasfontenla | 22:000890b38b32 | 727 | if(recv == 'M'){ |
| lucasfontenla | 22:000890b38b32 | 728 | mode = 1; |
| lucasfontenla | 22:000890b38b32 | 729 | speed = 0; |
| lucasfontenla | 22:000890b38b32 | 730 | } else if(recv == 'F'){ |
| lucasfontenla | 22:000890b38b32 | 731 | mode = 0; |
| lucasfontenla | 22:000890b38b32 | 732 | speed = 1; |
| lucasfontenla | 22:000890b38b32 | 733 | } |
| lucasfontenla | 22:000890b38b32 | 734 | |
| lucasfontenla | 22:000890b38b32 | 735 | if(mode){ |
| lucasfontenla | 22:000890b38b32 | 736 | if(recv != 'M'){ |
| lucasfontenla | 22:000890b38b32 | 737 | mode_recv.push_back(recv); |
| lucasfontenla | 22:000890b38b32 | 738 | } |
| lucasfontenla | 22:000890b38b32 | 739 | } else if(speed){ |
| lucasfontenla | 22:000890b38b32 | 740 | if(recv != 'F'){ |
| lucasfontenla | 22:000890b38b32 | 741 | speed_str.push_back(recv); |
| lucasfontenla | 22:000890b38b32 | 742 | } |
| lucasfontenla | 22:000890b38b32 | 743 | } |
| lucasfontenla | 22:000890b38b32 | 744 | } |
| lucasfontenla | 23:d64d37561b4a | 745 | |
| lucasfontenla | 23:d64d37561b4a | 746 | if(jog_mode && recv == 'k'){ |
| lucasfontenla | 23:d64d37561b4a | 747 | kill_jog_mode = 1; |
| lucasfontenla | 23:d64d37561b4a | 748 | } |
| lucasfontenla | 25:7b3826dddcba | 749 | |
| lucasfontenla | 25:7b3826dddcba | 750 | if(zero && recv == 'm'){ |
| lucasfontenla | 25:7b3826dddcba | 751 | machine_zero = 1; |
| lucasfontenla | 26:f891ff6beb33 | 752 | } else if(zero && recv != 'm' && recv != 'z'){ |
| lucasfontenla | 25:7b3826dddcba | 753 | last_recv = recv; |
| lucasfontenla | 25:7b3826dddcba | 754 | } |
| lucasfontenla | 22:000890b38b32 | 755 | } |
| lucasfontenla | 24:92f612ca218b | 756 | t.reset(); |
| lucasfontenla | 26:f891ff6beb33 | 757 | } |
| lucasfontenla | 26:f891ff6beb33 | 758 | |
| lucasfontenla | 26:f891ff6beb33 | 759 | void goMachineHome(int ppsMax){ |
| lucasfontenla | 26:f891ff6beb33 | 760 | /*zeroX(ppsMax); |
| lucasfontenla | 26:f891ff6beb33 | 761 | zeroY(ppsMax); |
| lucasfontenla | 26:f891ff6beb33 | 762 | zeroZ(ppsMax);*/ |
| lucasfontenla | 26:f891ff6beb33 | 763 | printf("Machine Zero\n\r"); |
| lucasfontenla | 26:f891ff6beb33 | 764 | wait(0.1); |
| lucasfontenla | 26:f891ff6beb33 | 765 | ihm_class.action_complete(); |
| lucasfontenla | 26:f891ff6beb33 | 766 | } |
| lucasfontenla | 26:f891ff6beb33 | 767 | |
| lucasfontenla | 26:f891ff6beb33 | 768 | void joystickClick(void){ |
| lucasfontenla | 26:f891ff6beb33 | 769 | toggle = !toggle; |
| lucasfontenla | 26:f891ff6beb33 | 770 | } |
| lucasfontenla | 26:f891ff6beb33 | 771 | |
| lucasfontenla | 26:f891ff6beb33 | 772 | void linear_interpolation(int x1, int y1, int x2, int y2){ |
| lucasfontenla | 26:f891ff6beb33 | 773 | int x = x1; |
| lucasfontenla | 26:f891ff6beb33 | 774 | int y = y1; |
| lucasfontenla | 26:f891ff6beb33 | 775 | |
| lucasfontenla | 26:f891ff6beb33 | 776 | int dx = x2 - x1; |
| lucasfontenla | 26:f891ff6beb33 | 777 | int dy = y2 - y1; |
| lucasfontenla | 26:f891ff6beb33 | 778 | |
| lucasfontenla | 26:f891ff6beb33 | 779 | int direction; |
| lucasfontenla | 26:f891ff6beb33 | 780 | |
| lucasfontenla | 26:f891ff6beb33 | 781 | printf("dx: %d dy: %d\n\r", dx, dy); |
| lucasfontenla | 26:f891ff6beb33 | 782 | |
| lucasfontenla | 26:f891ff6beb33 | 783 | if(dx > 0 && dy > 0){ |
| lucasfontenla | 26:f891ff6beb33 | 784 | x_dir = x_plus; |
| lucasfontenla | 26:f891ff6beb33 | 785 | y_dir = y_plus; |
| lucasfontenla | 26:f891ff6beb33 | 786 | direction = 1; |
| lucasfontenla | 26:f891ff6beb33 | 787 | } else if(dx > 0 && dy < 0){ |
| lucasfontenla | 26:f891ff6beb33 | 788 | x_dir = x_plus; |
| lucasfontenla | 26:f891ff6beb33 | 789 | y_dir = y_minus; |
| lucasfontenla | 26:f891ff6beb33 | 790 | direction = 1; |
| lucasfontenla | 26:f891ff6beb33 | 791 | dy = dy*(-1); |
| lucasfontenla | 26:f891ff6beb33 | 792 | } else if(dx < 0 && dy > 0){ |
| lucasfontenla | 26:f891ff6beb33 | 793 | x_dir = x_minus; |
| lucasfontenla | 26:f891ff6beb33 | 794 | y_dir = y_plus; |
| lucasfontenla | 26:f891ff6beb33 | 795 | direction = -1; |
| lucasfontenla | 26:f891ff6beb33 | 796 | dx = dx*(-1); |
| lucasfontenla | 26:f891ff6beb33 | 797 | } else if(dx < 0 && dy < 0){ |
| lucasfontenla | 26:f891ff6beb33 | 798 | x_dir = x_minus; |
| lucasfontenla | 26:f891ff6beb33 | 799 | y_dir = y_minus; |
| lucasfontenla | 26:f891ff6beb33 | 800 | dx = dx*(-1); |
| lucasfontenla | 26:f891ff6beb33 | 801 | dy = dy*(-1); |
| lucasfontenla | 26:f891ff6beb33 | 802 | direction = -1; |
| lucasfontenla | 26:f891ff6beb33 | 803 | } else if(dy == 0){ |
| lucasfontenla | 26:f891ff6beb33 | 804 | if(dx > 0){ |
| lucasfontenla | 26:f891ff6beb33 | 805 | x_dir = x_plus; |
| lucasfontenla | 26:f891ff6beb33 | 806 | } else if (dx < 0){ |
| lucasfontenla | 26:f891ff6beb33 | 807 | x_dir = x_minus; |
| lucasfontenla | 26:f891ff6beb33 | 808 | dx = dx*(-1); |
| lucasfontenla | 26:f891ff6beb33 | 809 | } |
| lucasfontenla | 26:f891ff6beb33 | 810 | move(ppsMax, x_dir, y_dir, z_dir, dx, 0, 0); |
| lucasfontenla | 26:f891ff6beb33 | 811 | return; |
| lucasfontenla | 26:f891ff6beb33 | 812 | } else if(dx == 0){ |
| lucasfontenla | 26:f891ff6beb33 | 813 | if(dy > 0){ |
| lucasfontenla | 26:f891ff6beb33 | 814 | y_dir = y_plus; |
| lucasfontenla | 26:f891ff6beb33 | 815 | } else if (dy < 0){ |
| lucasfontenla | 26:f891ff6beb33 | 816 | y_dir = y_minus; |
| lucasfontenla | 26:f891ff6beb33 | 817 | dy = dy*(-1); |
| lucasfontenla | 26:f891ff6beb33 | 818 | } |
| lucasfontenla | 26:f891ff6beb33 | 819 | move(ppsMax, x_dir, y_dir, z_dir, 0, dy, 0); |
| lucasfontenla | 26:f891ff6beb33 | 820 | return; |
| lucasfontenla | 26:f891ff6beb33 | 821 | } |
| lucasfontenla | 26:f891ff6beb33 | 822 | |
| lucasfontenla | 26:f891ff6beb33 | 823 | int incI = 2*dy; |
| lucasfontenla | 26:f891ff6beb33 | 824 | int incS = 2*(dy - dx); |
| lucasfontenla | 26:f891ff6beb33 | 825 | |
| lucasfontenla | 26:f891ff6beb33 | 826 | int d = 2*dy - dx; |
| lucasfontenla | 26:f891ff6beb33 | 827 | |
| lucasfontenla | 26:f891ff6beb33 | 828 | |
| lucasfontenla | 26:f891ff6beb33 | 829 | while(x != x2){ |
| lucasfontenla | 26:f891ff6beb33 | 830 | if(d <= 0){ |
| lucasfontenla | 26:f891ff6beb33 | 831 | d = d + incI; |
| lucasfontenla | 26:f891ff6beb33 | 832 | x = x + 1*direction; |
| lucasfontenla | 26:f891ff6beb33 | 833 | move(ppsMax, x_dir, y_dir, z_dir, 1, 0, 0); |
| lucasfontenla | 26:f891ff6beb33 | 834 | } else { |
| lucasfontenla | 26:f891ff6beb33 | 835 | d = d + incS; |
| lucasfontenla | 26:f891ff6beb33 | 836 | x = x + 1*direction; |
| lucasfontenla | 26:f891ff6beb33 | 837 | move(ppsMax, x_dir, y_dir, z_dir, 1, 1, 0); |
| lucasfontenla | 26:f891ff6beb33 | 838 | } |
| lucasfontenla | 26:f891ff6beb33 | 839 | } |
| lucasfontenla | 17:55e6270adab5 | 840 | } |