Grupo T / Mbed OS GRUPOT
Committer:
lucasfontenla
Date:
Fri May 04 17:55:03 2018 +0000
Revision:
12:801e58a7137c
Parent:
11:0e22a6fc7f87
Child:
13:0f385bfe3e0d
Zeramento em X implementado (n?o testado)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lucasfontenla 0:cc562f78f889 1 #include "mbed.h"
lucasfontenla 0:cc562f78f889 2
lucasfontenla 9:d771a576e4fc 3 // Classes ---------------------------------------------------------------------------------
lucasfontenla 9:d771a576e4fc 4 Timer debounce;
lucasfontenla 9:d771a576e4fc 5
lucasfontenla 5:0cbf491f703d 6 // MACHINE SETUP ---------------------------------------------------------------------------
lucasfontenla 5:0cbf491f703d 7
lucasfontenla 5:0cbf491f703d 8 // drivers output signal
lucasfontenla 4:48717b42eab2 9 DigitalOut stepX(D2);
lucasfontenla 4:48717b42eab2 10 DigitalOut dirX(D3);
lucasfontenla 4:48717b42eab2 11
lucasfontenla 4:48717b42eab2 12 DigitalOut stepY(D8);
lucasfontenla 4:48717b42eab2 13 DigitalOut dirY(D9);
lucasfontenla 4:48717b42eab2 14
lucasfontenla 4:48717b42eab2 15 DigitalOut stepZ(D11);
lucasfontenla 4:48717b42eab2 16 DigitalOut dirZ(D12);
lucasfontenla 4:48717b42eab2 17
lucasfontenla 5:0cbf491f703d 18 // hardware input signal
lucasfontenla 5:0cbf491f703d 19 // end-of-stroke sensors
lucasfontenla 10:cd868ca9997c 20 InterruptIn endY1(D5);
lucasfontenla 10:cd868ca9997c 21 InterruptIn endY2(D13);
lucasfontenla 5:0cbf491f703d 22
lucasfontenla 5:0cbf491f703d 23 // IHM for development
lucasfontenla 9:d771a576e4fc 24 AnalogIn joyX(A0);
lucasfontenla 9:d771a576e4fc 25 AnalogIn joyY(A1);
lucasfontenla 4:48717b42eab2 26
lucasfontenla 8:de64672c240f 27 DigitalIn zUp(D7);
lucasfontenla 8:de64672c240f 28 DigitalIn zDwn(D6);
lucasfontenla 4:48717b42eab2 29
lucasfontenla 5:0cbf491f703d 30 // variables definition
lucasfontenla 9:d771a576e4fc 31 int modeStatus = 0;
lucasfontenla 5:0cbf491f703d 32
lucasfontenla 4:48717b42eab2 33 float valX;
lucasfontenla 4:48717b42eab2 34 float valY;
lucasfontenla 4:48717b42eab2 35 int valZUp;
lucasfontenla 4:48717b42eab2 36 int valZDwn;
lucasfontenla 4:48717b42eab2 37
lucasfontenla 4:48717b42eab2 38 int activeX = 1;
lucasfontenla 4:48717b42eab2 39 int activeY = 1;
lucasfontenla 4:48717b42eab2 40 int activeZ = 1;
lucasfontenla 4:48717b42eab2 41
lucasfontenla 5:0cbf491f703d 42 int x_dir;
lucasfontenla 5:0cbf491f703d 43 int y_dir;
lucasfontenla 5:0cbf491f703d 44 int z_dir;
lucasfontenla 5:0cbf491f703d 45
lucasfontenla 9:d771a576e4fc 46 int x_plus = 1;
lucasfontenla 9:d771a576e4fc 47 int x_minus = 0;
lucasfontenla 9:d771a576e4fc 48 int y_plus = 1;
lucasfontenla 9:d771a576e4fc 49 int y_minus = 0;
lucasfontenla 9:d771a576e4fc 50 int z_plus = 1;
lucasfontenla 9:d771a576e4fc 51 int z_minus = 0;
lucasfontenla 9:d771a576e4fc 52
lucasfontenla 12:801e58a7137c 53 int x_limit_min = 0;
lucasfontenla 12:801e58a7137c 54 int x_limit_max = 0;
lucasfontenla 12:801e58a7137c 55
lucasfontenla 10:cd868ca9997c 56 int y_limit_min = 0;
lucasfontenla 10:cd868ca9997c 57 int y_limit_max = 0;
lucasfontenla 9:d771a576e4fc 58
lucasfontenla 5:0cbf491f703d 59 float ppsMax = 600.0;
lucasfontenla 5:0cbf491f703d 60
lucasfontenla 7:27ea14e58788 61 int totalX = 0;
lucasfontenla 7:27ea14e58788 62 int totalY = 0;
lucasfontenla 7:27ea14e58788 63
lucasfontenla 7:27ea14e58788 64 float X = 0;
lucasfontenla 7:27ea14e58788 65 float Y = 0;
lucasfontenla 7:27ea14e58788 66
lucasfontenla 9:d771a576e4fc 67 int endX1Status;
lucasfontenla 9:d771a576e4fc 68 int endX2Status;
lucasfontenla 9:d771a576e4fc 69
lucasfontenla 7:27ea14e58788 70 // machine parameters
lucasfontenla 8:de64672c240f 71 float xPitch = 3.0;
lucasfontenla 8:de64672c240f 72 float yPitch = 3.0;
lucasfontenla 7:27ea14e58788 73
lucasfontenla 7:27ea14e58788 74 float motor_steps_by_rotation = 200.0;
lucasfontenla 7:27ea14e58788 75 float step_mode = 0.5;
lucasfontenla 7:27ea14e58788 76 float steps_by_rotation = motor_steps_by_rotation/step_mode;
lucasfontenla 7:27ea14e58788 77
lucasfontenla 7:27ea14e58788 78 // prototype functions
lucasfontenla 9:d771a576e4fc 79 void setupPins();
lucasfontenla 5:0cbf491f703d 80 void move(int pps, int x_dir, int y_dir, int z_dir, int x_step, int y_step, int z_step);
lucasfontenla 7:27ea14e58788 81 float distance(int steps, float pitch);
lucasfontenla 7:27ea14e58788 82
lucasfontenla 11:0e22a6fc7f87 83 void endX1Int_press(void);
lucasfontenla 11:0e22a6fc7f87 84 void endX1Int_release(void);
lucasfontenla 11:0e22a6fc7f87 85 void endX2Int_press(void);
lucasfontenla 11:0e22a6fc7f87 86 void endX2Int_release(void);
lucasfontenla 11:0e22a6fc7f87 87
lucasfontenla 10:cd868ca9997c 88 void endY1Int_press(void);
lucasfontenla 10:cd868ca9997c 89 void endY1Int_release(void);
lucasfontenla 10:cd868ca9997c 90 void endY2Int_press(void);
lucasfontenla 10:cd868ca9997c 91 void endY2Int_release(void);
lucasfontenla 9:d771a576e4fc 92
lucasfontenla 12:801e58a7137c 93 void zeroX(int pps);
lucasfontenla 11:0e22a6fc7f87 94 void zeroY(int pps);
lucasfontenla 11:0e22a6fc7f87 95
lucasfontenla 7:27ea14e58788 96 // MAIN PROGRAM ----------------------------------------------------------------
lucasfontenla 7:27ea14e58788 97
lucasfontenla 7:27ea14e58788 98 int contador = 0;
lucasfontenla 1:b2e395e50a45 99
lucasfontenla 1:b2e395e50a45 100 int main(){
lucasfontenla 4:48717b42eab2 101 printf("Starting...\r\n");
lucasfontenla 12:801e58a7137c 102
lucasfontenla 12:801e58a7137c 103 endX1.fall(&endX1Int_press);
lucasfontenla 12:801e58a7137c 104 endX1.rise(&endX1Int_release);
lucasfontenla 12:801e58a7137c 105 endX2.fall(&endX2Int_press);
lucasfontenla 12:801e58a7137c 106 endX2.rise(&endX2Int_release);
lucasfontenla 5:0cbf491f703d 107
lucasfontenla 10:cd868ca9997c 108 endY1.fall(&endY1Int_press);
lucasfontenla 10:cd868ca9997c 109 endY1.rise(&endY1Int_release);
lucasfontenla 10:cd868ca9997c 110 endY2.fall(&endY2Int_press);
lucasfontenla 10:cd868ca9997c 111 endY2.rise(&endY2Int_release);
lucasfontenla 9:d771a576e4fc 112
lucasfontenla 5:0cbf491f703d 113 if(modeStatus){
lucasfontenla 5:0cbf491f703d 114 //Código de JOG
lucasfontenla 7:27ea14e58788 115 printf("JOG Selected\n");
lucasfontenla 5:0cbf491f703d 116 while(1){
lucasfontenla 5:0cbf491f703d 117 valX = joyX;
lucasfontenla 5:0cbf491f703d 118 valY = joyY;
lucasfontenla 5:0cbf491f703d 119
lucasfontenla 5:0cbf491f703d 120 valZUp = zUp;
lucasfontenla 5:0cbf491f703d 121 valZDwn = zDwn;
lucasfontenla 5:0cbf491f703d 122
lucasfontenla 5:0cbf491f703d 123 if(valX > 0.7){
lucasfontenla 9:d771a576e4fc 124 if(x_dir != x_minus){
lucasfontenla 9:d771a576e4fc 125 x_dir = x_minus;
lucasfontenla 5:0cbf491f703d 126 }
lucasfontenla 7:27ea14e58788 127 activeX = 1;
lucasfontenla 7:27ea14e58788 128 totalX-=1;
lucasfontenla 5:0cbf491f703d 129 }
lucasfontenla 5:0cbf491f703d 130 else if(valX < 0.3){
lucasfontenla 9:d771a576e4fc 131 if(x_dir != x_plus){
lucasfontenla 9:d771a576e4fc 132 x_dir = x_plus;
lucasfontenla 5:0cbf491f703d 133 }
lucasfontenla 7:27ea14e58788 134 activeX = 1;
lucasfontenla 7:27ea14e58788 135 totalX+=1;
lucasfontenla 5:0cbf491f703d 136 }
lucasfontenla 5:0cbf491f703d 137 else{
lucasfontenla 5:0cbf491f703d 138 activeX = 0;
lucasfontenla 4:48717b42eab2 139 }
lucasfontenla 5:0cbf491f703d 140
lucasfontenla 5:0cbf491f703d 141 //----------------------------------------------------------------------
lucasfontenla 5:0cbf491f703d 142
lucasfontenla 5:0cbf491f703d 143 if(valY > 0.7){
lucasfontenla 9:d771a576e4fc 144 if(y_dir != y_plus){
lucasfontenla 9:d771a576e4fc 145 y_dir = y_plus;
lucasfontenla 5:0cbf491f703d 146 }
lucasfontenla 7:27ea14e58788 147 activeY = 1;
lucasfontenla 7:27ea14e58788 148 totalY+=1;
lucasfontenla 5:0cbf491f703d 149 }
lucasfontenla 5:0cbf491f703d 150 else if(valY < 0.3){
lucasfontenla 9:d771a576e4fc 151 if(y_dir != y_minus){
lucasfontenla 9:d771a576e4fc 152 y_dir = y_minus;
lucasfontenla 5:0cbf491f703d 153 }
lucasfontenla 7:27ea14e58788 154 activeY = 1;
lucasfontenla 7:27ea14e58788 155 totalY-=1;
lucasfontenla 5:0cbf491f703d 156 }
lucasfontenla 5:0cbf491f703d 157 else{
lucasfontenla 5:0cbf491f703d 158 activeY = 0;
lucasfontenla 4:48717b42eab2 159 }
lucasfontenla 5:0cbf491f703d 160
lucasfontenla 5:0cbf491f703d 161 //----------------------------------------------------------------------
lucasfontenla 5:0cbf491f703d 162
lucasfontenla 5:0cbf491f703d 163 if(!valZUp && valZDwn){
lucasfontenla 9:d771a576e4fc 164 if(z_dir != z_minus){
lucasfontenla 9:d771a576e4fc 165 z_dir = z_minus;
lucasfontenla 7:27ea14e58788 166 //totalZ-=1;
lucasfontenla 5:0cbf491f703d 167 }
lucasfontenla 5:0cbf491f703d 168 activeZ = 1;
lucasfontenla 5:0cbf491f703d 169 }
lucasfontenla 5:0cbf491f703d 170 else if(!valZDwn && valZUp){
lucasfontenla 9:d771a576e4fc 171 if(z_dir != z_plus){
lucasfontenla 9:d771a576e4fc 172 z_dir = z_plus;
lucasfontenla 7:27ea14e58788 173 //totalZ+=1;
lucasfontenla 5:0cbf491f703d 174 }
lucasfontenla 5:0cbf491f703d 175 activeZ = 1;
lucasfontenla 5:0cbf491f703d 176 }
lucasfontenla 5:0cbf491f703d 177 else {
lucasfontenla 5:0cbf491f703d 178 activeZ = 0;
lucasfontenla 5:0cbf491f703d 179 }
lucasfontenla 5:0cbf491f703d 180
lucasfontenla 5:0cbf491f703d 181 //----------------------------------------------------------------------
lucasfontenla 5:0cbf491f703d 182
lucasfontenla 7:27ea14e58788 183 move(ppsMax, x_dir, y_dir, z_dir, activeX, activeY, activeZ);
lucasfontenla 7:27ea14e58788 184 X = distance(totalX, xPitch);
lucasfontenla 7:27ea14e58788 185 Y = distance(totalY, yPitch);
lucasfontenla 7:27ea14e58788 186
lucasfontenla 7:27ea14e58788 187 if(contador > 10000){
lucasfontenla 7:27ea14e58788 188 X = distance(totalX, xPitch);
lucasfontenla 7:27ea14e58788 189 Y = distance(totalY, yPitch);
lucasfontenla 7:27ea14e58788 190 printf("X: %.2f Y: %.2f\r\n", X, Y);
lucasfontenla 7:27ea14e58788 191 contador = 0;
lucasfontenla 7:27ea14e58788 192 }
lucasfontenla 7:27ea14e58788 193
lucasfontenla 7:27ea14e58788 194 contador+=1;
lucasfontenla 4:48717b42eab2 195 }
lucasfontenla 5:0cbf491f703d 196 }
lucasfontenla 5:0cbf491f703d 197
lucasfontenla 5:0cbf491f703d 198 else {
lucasfontenla 11:0e22a6fc7f87 199 zeroY(ppsMax);
lucasfontenla 9:d771a576e4fc 200
lucasfontenla 11:0e22a6fc7f87 201 X = distance(totalX, xPitch);
lucasfontenla 11:0e22a6fc7f87 202 Y = distance(totalY, yPitch);
lucasfontenla 9:d771a576e4fc 203
lucasfontenla 11:0e22a6fc7f87 204 printf("X: %.2f Y: %.2f\r\n", X, Y);
lucasfontenla 9:d771a576e4fc 205 }
lucasfontenla 9:d771a576e4fc 206 }
lucasfontenla 9:d771a576e4fc 207
lucasfontenla 9:d771a576e4fc 208 // FUNCTIONS ----------------------------------------------------------------
lucasfontenla 12:801e58a7137c 209 void endX1Int_press(void){
lucasfontenla 12:801e58a7137c 210 printf("X1 - press\n\r");
lucasfontenla 12:801e58a7137c 211 x_limit_min = 1;
lucasfontenla 12:801e58a7137c 212 }
lucasfontenla 12:801e58a7137c 213 void endX1Int_release(void){
lucasfontenla 12:801e58a7137c 214 printf("X1 - release\n\r");
lucasfontenla 12:801e58a7137c 215 x_limit_min = 0;
lucasfontenla 12:801e58a7137c 216 }
lucasfontenla 12:801e58a7137c 217 void endX2Int_press(void){
lucasfontenla 12:801e58a7137c 218 printf("X2 - press\n\r");
lucasfontenla 12:801e58a7137c 219 x_limit_max = 1;
lucasfontenla 12:801e58a7137c 220 }
lucasfontenla 12:801e58a7137c 221 void endX2Int_release(void){
lucasfontenla 12:801e58a7137c 222 printf("X2 - release\n\r");
lucasfontenla 12:801e58a7137c 223 x_limit_max = 0;
lucasfontenla 9:d771a576e4fc 224 }
lucasfontenla 9:d771a576e4fc 225
lucasfontenla 12:801e58a7137c 226 void endY1Int_press(void){
lucasfontenla 12:801e58a7137c 227 printf("Y1 - press\n\r");
lucasfontenla 12:801e58a7137c 228 y_limit_min = 1;
lucasfontenla 12:801e58a7137c 229 }
lucasfontenla 10:cd868ca9997c 230 void endY1Int_release(void){
lucasfontenla 10:cd868ca9997c 231 printf("Y1 - release\n\r");
lucasfontenla 12:801e58a7137c 232 y_limit_min = 0;
lucasfontenla 9:d771a576e4fc 233 }
lucasfontenla 10:cd868ca9997c 234 void endY2Int_press(void){
lucasfontenla 10:cd868ca9997c 235 printf("Y2 - press\n\r");
lucasfontenla 12:801e58a7137c 236 y_limit_max = 1;
lucasfontenla 9:d771a576e4fc 237 }
lucasfontenla 10:cd868ca9997c 238 void endY2Int_release(void){
lucasfontenla 10:cd868ca9997c 239 printf("Y2 - release\n\r");
lucasfontenla 12:801e58a7137c 240 y_limit_max = 0;
lucasfontenla 9:d771a576e4fc 241 }
lucasfontenla 7:27ea14e58788 242
lucasfontenla 5:0cbf491f703d 243 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 244 float time = 1.0/pps/2.0;
lucasfontenla 5:0cbf491f703d 245
lucasfontenla 5:0cbf491f703d 246 dirX = x_dir;
lucasfontenla 5:0cbf491f703d 247 dirY = y_dir;
lucasfontenla 5:0cbf491f703d 248 dirZ = z_dir;
lucasfontenla 5:0cbf491f703d 249
lucasfontenla 12:801e58a7137c 250 if((x_dir == x_minus) && x_limit_min){
lucasfontenla 12:801e58a7137c 251 x_step = 0;
lucasfontenla 12:801e58a7137c 252 }
lucasfontenla 12:801e58a7137c 253 if((x_dir == x_plus) && x_limit_max){
lucasfontenla 12:801e58a7137c 254 x_step = 0;
lucasfontenla 12:801e58a7137c 255 }
lucasfontenla 12:801e58a7137c 256
lucasfontenla 11:0e22a6fc7f87 257 if((y_dir == y_minus) && y_limit_min){
lucasfontenla 11:0e22a6fc7f87 258 y_step = 0;
lucasfontenla 11:0e22a6fc7f87 259 }
lucasfontenla 11:0e22a6fc7f87 260 if((y_dir == y_plus) && y_limit_max){
lucasfontenla 11:0e22a6fc7f87 261 y_step = 0;
lucasfontenla 11:0e22a6fc7f87 262 }
lucasfontenla 11:0e22a6fc7f87 263
lucasfontenla 5:0cbf491f703d 264 int max_val;
lucasfontenla 5:0cbf491f703d 265
lucasfontenla 6:af32c68ff4cf 266 // maior número de passos -------
lucasfontenla 5:0cbf491f703d 267 if(x_step > y_step){
lucasfontenla 5:0cbf491f703d 268 if(x_step > z_step){
lucasfontenla 5:0cbf491f703d 269 max_val = x_step;
lucasfontenla 4:48717b42eab2 270 }
lucasfontenla 4:48717b42eab2 271 else {
lucasfontenla 5:0cbf491f703d 272 max_val = z_step;
lucasfontenla 4:48717b42eab2 273 }
lucasfontenla 5:0cbf491f703d 274 } else if(y_step > z_step){
lucasfontenla 5:0cbf491f703d 275 max_val = y_step;
lucasfontenla 5:0cbf491f703d 276 }
lucasfontenla 5:0cbf491f703d 277 else {
lucasfontenla 5:0cbf491f703d 278 max_val = z_step;
lucasfontenla 5:0cbf491f703d 279 }
lucasfontenla 5:0cbf491f703d 280
lucasfontenla 5:0cbf491f703d 281 for(int i = 0; i < max_val; i++){
lucasfontenla 6:af32c68ff4cf 282 if(i >= x_step){
lucasfontenla 5:0cbf491f703d 283 stepX = 0;
lucasfontenla 5:0cbf491f703d 284 }
lucasfontenla 5:0cbf491f703d 285 else { stepX = 1; }
lucasfontenla 4:48717b42eab2 286
lucasfontenla 6:af32c68ff4cf 287 if(i >= y_step){
lucasfontenla 5:0cbf491f703d 288 stepY = 0;
lucasfontenla 5:0cbf491f703d 289 }
lucasfontenla 11:0e22a6fc7f87 290 else {
lucasfontenla 11:0e22a6fc7f87 291 stepY = 1;}
lucasfontenla 5:0cbf491f703d 292
lucasfontenla 6:af32c68ff4cf 293 if(i >= z_step){
lucasfontenla 5:0cbf491f703d 294 stepZ = 0;
lucasfontenla 5:0cbf491f703d 295 }
lucasfontenla 5:0cbf491f703d 296 else { stepZ = 1; }
lucasfontenla 5:0cbf491f703d 297
lucasfontenla 5:0cbf491f703d 298 wait(time);
lucasfontenla 4:48717b42eab2 299 stepX = 0;
lucasfontenla 4:48717b42eab2 300 stepY = 0;
lucasfontenla 4:48717b42eab2 301 stepZ = 0;
lucasfontenla 5:0cbf491f703d 302 wait(time);
lucasfontenla 4:48717b42eab2 303 }
lucasfontenla 5:0cbf491f703d 304 }
lucasfontenla 7:27ea14e58788 305
lucasfontenla 12:801e58a7137c 306 void zeroX(int pps){
lucasfontenla 12:801e58a7137c 307 float time = 1.0/pps/2.0;
lucasfontenla 12:801e58a7137c 308
lucasfontenla 12:801e58a7137c 309 while(!x_limit_min){
lucasfontenla 12:801e58a7137c 310 if(dirX != x_minus){
lucasfontenla 12:801e58a7137c 311 dirX = x_minus;
lucasfontenla 12:801e58a7137c 312 }
lucasfontenla 12:801e58a7137c 313 stepX = 1;
lucasfontenla 12:801e58a7137c 314 wait(time);
lucasfontenla 12:801e58a7137c 315 stepX = 0;
lucasfontenla 12:801e58a7137c 316 wait(time);
lucasfontenla 12:801e58a7137c 317 }
lucasfontenla 12:801e58a7137c 318
lucasfontenla 12:801e58a7137c 319 printf("X zero\n\r");
lucasfontenla 12:801e58a7137c 320
lucasfontenla 12:801e58a7137c 321 totalX = 0.0;
lucasfontenla 12:801e58a7137c 322 }
lucasfontenla 12:801e58a7137c 323
lucasfontenla 11:0e22a6fc7f87 324 void zeroY(int pps){
lucasfontenla 11:0e22a6fc7f87 325 float time = 1.0/pps/2.0;
lucasfontenla 11:0e22a6fc7f87 326
lucasfontenla 11:0e22a6fc7f87 327 while(!y_limit_min){
lucasfontenla 11:0e22a6fc7f87 328 if(dirY != y_minus){
lucasfontenla 11:0e22a6fc7f87 329 dirY = y_minus;
lucasfontenla 11:0e22a6fc7f87 330 }
lucasfontenla 11:0e22a6fc7f87 331 stepY = 1;
lucasfontenla 11:0e22a6fc7f87 332 wait(time);
lucasfontenla 11:0e22a6fc7f87 333 stepY = 0;
lucasfontenla 11:0e22a6fc7f87 334 wait(time);
lucasfontenla 11:0e22a6fc7f87 335 }
lucasfontenla 11:0e22a6fc7f87 336
lucasfontenla 11:0e22a6fc7f87 337 printf("Y zero\n\r");
lucasfontenla 11:0e22a6fc7f87 338
lucasfontenla 11:0e22a6fc7f87 339 totalY = 0.0;
lucasfontenla 11:0e22a6fc7f87 340 }
lucasfontenla 11:0e22a6fc7f87 341
lucasfontenla 7:27ea14e58788 342 float distance(int steps, float pitch){
lucasfontenla 7:27ea14e58788 343 float delta_S = (((float)steps)/steps_by_rotation)*pitch;
lucasfontenla 7:27ea14e58788 344 return delta_S;
lucasfontenla 7:27ea14e58788 345 }