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@15:4604a7ee9c77, 2018-05-07 (annotated)

Committer:
lucasfontenla
Date:
Mon May 07 19:54:12 2018 +0000
Revision:
15:4604a7ee9c77
Parent:
14:7cc41420a12c
Child:
16:3adab04d42a6
Portas ajustadas e fim de curso alterado para conex?es paralelas.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lucasfontenla 0:cc562f78f889 1 #include "mbed.h"
lucasfontenla 0:cc562f78f889 2
lucasfontenla 5:0cbf491f703d 3 // MACHINE SETUP ---------------------------------------------------------------------------
lucasfontenla 5:0cbf491f703d 4
lucasfontenla 5:0cbf491f703d 5 // drivers output signal
lucasfontenla 14:7cc41420a12c 6 DigitalOut enable(D2);
lucasfontenla 14:7cc41420a12c 7
lucasfontenla 4:48717b42eab2 8 DigitalOut dirX(D3);
lucasfontenla 15:4604a7ee9c77 9 DigitalOut stepX(D4);
lucasfontenla 4:48717b42eab2 10
lucasfontenla 15:4604a7ee9c77 11 DigitalOut dirY(D5);
lucasfontenla 15:4604a7ee9c77 12 DigitalOut stepY(D6);
lucasfontenla 4:48717b42eab2 13
lucasfontenla 15:4604a7ee9c77 14 DigitalOut dirZ(D7);
lucasfontenla 15:4604a7ee9c77 15 DigitalOut stepZ(D8);
lucasfontenla 4:48717b42eab2 16
lucasfontenla 5:0cbf491f703d 17 // hardware input signal
lucasfontenla 5:0cbf491f703d 18 // end-of-stroke sensors
lucasfontenla 15:4604a7ee9c77 19 InterruptIn endX(D9);
lucasfontenla 15:4604a7ee9c77 20 InterruptIn endY(D10);
lucasfontenla 15:4604a7ee9c77 21 InterruptIn endZ(D11);
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 15:4604a7ee9c77 27 DigitalIn zUp(D12);
lucasfontenla 15:4604a7ee9c77 28 DigitalIn zDwn(D13);
lucasfontenla 4:48717b42eab2 29
lucasfontenla 5:0cbf491f703d 30 // variables definition
lucasfontenla 15:4604a7ee9c77 31 int modeStatus = 1;
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 13:0f385bfe3e0d 46 int x_plus = 0;
lucasfontenla 13:0f385bfe3e0d 47 int x_minus = 1;
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 15:4604a7ee9c77 53 int x_limit = 0;
lucasfontenla 15:4604a7ee9c77 54 int x_block = 0;
lucasfontenla 12:801e58a7137c 55
lucasfontenla 15:4604a7ee9c77 56 int y_limit = 0;
lucasfontenla 15:4604a7ee9c77 57 int y_block = 0;
lucasfontenla 15:4604a7ee9c77 58
lucasfontenla 15:4604a7ee9c77 59 int z_limit = 0;
lucasfontenla 15:4604a7ee9c77 60 int z_block = 0;
lucasfontenla 9:d771a576e4fc 61
lucasfontenla 5:0cbf491f703d 62 float ppsMax = 600.0;
lucasfontenla 5:0cbf491f703d 63
lucasfontenla 7:27ea14e58788 64 int totalX = 0;
lucasfontenla 7:27ea14e58788 65 int totalY = 0;
lucasfontenla 15:4604a7ee9c77 66 int totalZ = 0;
lucasfontenla 7:27ea14e58788 67
lucasfontenla 7:27ea14e58788 68 float X = 0;
lucasfontenla 7:27ea14e58788 69 float Y = 0;
lucasfontenla 15:4604a7ee9c77 70 float Z = 0;
lucasfontenla 7:27ea14e58788 71
lucasfontenla 9:d771a576e4fc 72 int endX1Status;
lucasfontenla 9:d771a576e4fc 73 int endX2Status;
lucasfontenla 9:d771a576e4fc 74
lucasfontenla 7:27ea14e58788 75 // machine parameters
lucasfontenla 8:de64672c240f 76 float xPitch = 3.0;
lucasfontenla 8:de64672c240f 77 float yPitch = 3.0;
lucasfontenla 15:4604a7ee9c77 78 float zPitch = 3.0;
lucasfontenla 7:27ea14e58788 79
lucasfontenla 7:27ea14e58788 80 float motor_steps_by_rotation = 200.0;
lucasfontenla 7:27ea14e58788 81 float step_mode = 0.5;
lucasfontenla 7:27ea14e58788 82 float steps_by_rotation = motor_steps_by_rotation/step_mode;
lucasfontenla 7:27ea14e58788 83
lucasfontenla 7:27ea14e58788 84 // prototype functions
lucasfontenla 9:d771a576e4fc 85 void setupPins();
lucasfontenla 5:0cbf491f703d 86 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 87 float distance(int steps, float pitch);
lucasfontenla 7:27ea14e58788 88
lucasfontenla 15:4604a7ee9c77 89 void endX_press(void);
lucasfontenla 15:4604a7ee9c77 90 void endX_release(void);
lucasfontenla 11:0e22a6fc7f87 91
lucasfontenla 15:4604a7ee9c77 92 void endY_press(void);
lucasfontenla 15:4604a7ee9c77 93 void endY_release(void);
lucasfontenla 15:4604a7ee9c77 94
lucasfontenla 15:4604a7ee9c77 95 void endZ_press(void);
lucasfontenla 15:4604a7ee9c77 96 void endZ_release(void);
lucasfontenla 9:d771a576e4fc 97
lucasfontenla 12:801e58a7137c 98 void zeroX(int pps);
lucasfontenla 11:0e22a6fc7f87 99 void zeroY(int pps);
lucasfontenla 11:0e22a6fc7f87 100
lucasfontenla 7:27ea14e58788 101 // MAIN PROGRAM ----------------------------------------------------------------
lucasfontenla 7:27ea14e58788 102
lucasfontenla 7:27ea14e58788 103 int contador = 0;
lucasfontenla 1:b2e395e50a45 104
lucasfontenla 1:b2e395e50a45 105 int main(){
lucasfontenla 4:48717b42eab2 106 printf("Starting...\r\n");
lucasfontenla 14:7cc41420a12c 107
lucasfontenla 14:7cc41420a12c 108 enable = 0;
lucasfontenla 12:801e58a7137c 109
lucasfontenla 15:4604a7ee9c77 110 endX.fall(&endX_press);
lucasfontenla 15:4604a7ee9c77 111 endX.rise(&endX_release);
lucasfontenla 5:0cbf491f703d 112
lucasfontenla 15:4604a7ee9c77 113 endY.fall(&endY_press);
lucasfontenla 15:4604a7ee9c77 114 endY.rise(&endY_release);
lucasfontenla 15:4604a7ee9c77 115
lucasfontenla 15:4604a7ee9c77 116 endZ.fall(&endZ_press);
lucasfontenla 15:4604a7ee9c77 117 endZ.rise(&endZ_release);
lucasfontenla 9:d771a576e4fc 118
lucasfontenla 5:0cbf491f703d 119 if(modeStatus){
lucasfontenla 5:0cbf491f703d 120 //Código de JOG
lucasfontenla 7:27ea14e58788 121 printf("JOG Selected\n");
lucasfontenla 5:0cbf491f703d 122 while(1){
lucasfontenla 5:0cbf491f703d 123 valX = joyX;
lucasfontenla 5:0cbf491f703d 124 valY = joyY;
lucasfontenla 5:0cbf491f703d 125
lucasfontenla 5:0cbf491f703d 126 valZUp = zUp;
lucasfontenla 5:0cbf491f703d 127 valZDwn = zDwn;
lucasfontenla 5:0cbf491f703d 128
lucasfontenla 5:0cbf491f703d 129 if(valX > 0.7){
lucasfontenla 14:7cc41420a12c 130 if(x_dir != x_plus){
lucasfontenla 14:7cc41420a12c 131 x_dir = x_plus;
lucasfontenla 5:0cbf491f703d 132 }
lucasfontenla 7:27ea14e58788 133 activeX = 1;
lucasfontenla 15:4604a7ee9c77 134 totalX+=1;
lucasfontenla 5:0cbf491f703d 135 }
lucasfontenla 5:0cbf491f703d 136 else if(valX < 0.3){
lucasfontenla 14:7cc41420a12c 137 if(x_dir != x_minus){
lucasfontenla 14:7cc41420a12c 138 x_dir = x_minus;
lucasfontenla 5:0cbf491f703d 139 }
lucasfontenla 7:27ea14e58788 140 activeX = 1;
lucasfontenla 15:4604a7ee9c77 141 totalX-=1;
lucasfontenla 5:0cbf491f703d 142 }
lucasfontenla 5:0cbf491f703d 143 else{
lucasfontenla 5:0cbf491f703d 144 activeX = 0;
lucasfontenla 4:48717b42eab2 145 }
lucasfontenla 5:0cbf491f703d 146
lucasfontenla 5:0cbf491f703d 147 //----------------------------------------------------------------------
lucasfontenla 5:0cbf491f703d 148
lucasfontenla 5:0cbf491f703d 149 if(valY > 0.7){
lucasfontenla 9:d771a576e4fc 150 if(y_dir != y_plus){
lucasfontenla 9:d771a576e4fc 151 y_dir = y_plus;
lucasfontenla 5:0cbf491f703d 152 }
lucasfontenla 7:27ea14e58788 153 activeY = 1;
lucasfontenla 7:27ea14e58788 154 totalY+=1;
lucasfontenla 5:0cbf491f703d 155 }
lucasfontenla 5:0cbf491f703d 156 else if(valY < 0.3){
lucasfontenla 9:d771a576e4fc 157 if(y_dir != y_minus){
lucasfontenla 9:d771a576e4fc 158 y_dir = y_minus;
lucasfontenla 5:0cbf491f703d 159 }
lucasfontenla 7:27ea14e58788 160 activeY = 1;
lucasfontenla 7:27ea14e58788 161 totalY-=1;
lucasfontenla 5:0cbf491f703d 162 }
lucasfontenla 5:0cbf491f703d 163 else{
lucasfontenla 5:0cbf491f703d 164 activeY = 0;
lucasfontenla 4:48717b42eab2 165 }
lucasfontenla 5:0cbf491f703d 166
lucasfontenla 5:0cbf491f703d 167 //----------------------------------------------------------------------
lucasfontenla 5:0cbf491f703d 168
lucasfontenla 5:0cbf491f703d 169 if(!valZUp && valZDwn){
lucasfontenla 9:d771a576e4fc 170 if(z_dir != z_minus){
lucasfontenla 15:4604a7ee9c77 171 z_dir = z_minus;
lucasfontenla 5:0cbf491f703d 172 }
lucasfontenla 15:4604a7ee9c77 173 activeZ = 1;
lucasfontenla 15:4604a7ee9c77 174 totalZ-=1;
lucasfontenla 5:0cbf491f703d 175 }
lucasfontenla 5:0cbf491f703d 176 else if(!valZDwn && valZUp){
lucasfontenla 9:d771a576e4fc 177 if(z_dir != z_plus){
lucasfontenla 15:4604a7ee9c77 178 z_dir = z_plus;
lucasfontenla 5:0cbf491f703d 179 }
lucasfontenla 15:4604a7ee9c77 180 activeZ = 1;
lucasfontenla 15:4604a7ee9c77 181 totalZ+=1;
lucasfontenla 5:0cbf491f703d 182 }
lucasfontenla 5:0cbf491f703d 183 else {
lucasfontenla 5:0cbf491f703d 184 activeZ = 0;
lucasfontenla 5:0cbf491f703d 185 }
lucasfontenla 5:0cbf491f703d 186
lucasfontenla 5:0cbf491f703d 187 //----------------------------------------------------------------------
lucasfontenla 5:0cbf491f703d 188
lucasfontenla 15:4604a7ee9c77 189 //move(ppsMax, x_dir, y_dir, z_dir, activeX, activeY, activeZ);
lucasfontenla 7:27ea14e58788 190 X = distance(totalX, xPitch);
lucasfontenla 7:27ea14e58788 191 Y = distance(totalY, yPitch);
lucasfontenla 15:4604a7ee9c77 192 Z = distance(totalZ, zPitch);
lucasfontenla 7:27ea14e58788 193
lucasfontenla 7:27ea14e58788 194 if(contador > 10000){
lucasfontenla 15:4604a7ee9c77 195 printf("X: %.2f Y: %.2f Z: %.2f\r\n", X, Y, Z);
lucasfontenla 7:27ea14e58788 196 contador = 0;
lucasfontenla 7:27ea14e58788 197 }
lucasfontenla 7:27ea14e58788 198
lucasfontenla 7:27ea14e58788 199 contador+=1;
lucasfontenla 4:48717b42eab2 200 }
lucasfontenla 5:0cbf491f703d 201 }
lucasfontenla 5:0cbf491f703d 202
lucasfontenla 5:0cbf491f703d 203 else {
lucasfontenla 9:d771a576e4fc 204
lucasfontenla 9:d771a576e4fc 205 }
lucasfontenla 9:d771a576e4fc 206 }
lucasfontenla 9:d771a576e4fc 207
lucasfontenla 9:d771a576e4fc 208 // FUNCTIONS ----------------------------------------------------------------
lucasfontenla 15:4604a7ee9c77 209 void endX_press(void){
lucasfontenla 15:4604a7ee9c77 210 printf("X - press\n\r");
lucasfontenla 15:4604a7ee9c77 211 if(dirX == x_plus){
lucasfontenla 15:4604a7ee9c77 212 x_limit = x_plus;
lucasfontenla 15:4604a7ee9c77 213 x_block = 1;
lucasfontenla 15:4604a7ee9c77 214 }
lucasfontenla 15:4604a7ee9c77 215 else {
lucasfontenla 15:4604a7ee9c77 216 x_limit = x_minus;
lucasfontenla 15:4604a7ee9c77 217 x_block = 1;
lucasfontenla 15:4604a7ee9c77 218 }
lucasfontenla 12:801e58a7137c 219 }
lucasfontenla 15:4604a7ee9c77 220 void endX_release(void){
lucasfontenla 15:4604a7ee9c77 221 printf("X - release\n\r");
lucasfontenla 15:4604a7ee9c77 222 x_block = 0;
lucasfontenla 12:801e58a7137c 223 }
lucasfontenla 15:4604a7ee9c77 224 void endY_press(void){
lucasfontenla 15:4604a7ee9c77 225 printf("Y - press\n\r");
lucasfontenla 15:4604a7ee9c77 226 y_limit = 1;
lucasfontenla 15:4604a7ee9c77 227 }
lucasfontenla 15:4604a7ee9c77 228 void endY_release(void){
lucasfontenla 15:4604a7ee9c77 229 printf("Y - release\n\r");
lucasfontenla 15:4604a7ee9c77 230 y_limit = 0;
lucasfontenla 9:d771a576e4fc 231 }
lucasfontenla 9:d771a576e4fc 232
lucasfontenla 15:4604a7ee9c77 233 void endZ_press(void){
lucasfontenla 15:4604a7ee9c77 234 printf("Z - press\n\r");
lucasfontenla 15:4604a7ee9c77 235 z_limit = 1;
lucasfontenla 9:d771a576e4fc 236 }
lucasfontenla 15:4604a7ee9c77 237 void endZ_release(void){
lucasfontenla 15:4604a7ee9c77 238 printf("Z - release\n\r");
lucasfontenla 15:4604a7ee9c77 239 z_limit = 0;
lucasfontenla 9:d771a576e4fc 240 }
lucasfontenla 7:27ea14e58788 241
lucasfontenla 5:0cbf491f703d 242 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 243 float time = 1.0/pps/2.0;
lucasfontenla 5:0cbf491f703d 244
lucasfontenla 5:0cbf491f703d 245 dirX = x_dir;
lucasfontenla 5:0cbf491f703d 246 dirY = y_dir;
lucasfontenla 5:0cbf491f703d 247 dirZ = z_dir;
lucasfontenla 5:0cbf491f703d 248
lucasfontenla 15:4604a7ee9c77 249 if((x_dir == x_limit) && x_block){
lucasfontenla 12:801e58a7137c 250 x_step = 0;
lucasfontenla 12:801e58a7137c 251 }
lucasfontenla 12:801e58a7137c 252
lucasfontenla 15:4604a7ee9c77 253 if((y_dir == y_limit) && y_block){
lucasfontenla 11:0e22a6fc7f87 254 y_step = 0;
lucasfontenla 11:0e22a6fc7f87 255 }
lucasfontenla 15:4604a7ee9c77 256 if((z_dir == z_limit) && z_block){
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 5:0cbf491f703d 300 }
lucasfontenla 15:4604a7ee9c77 301 /*
lucasfontenla 12:801e58a7137c 302 void zeroX(int pps){
lucasfontenla 12:801e58a7137c 303 float time = 1.0/pps/2.0;
lucasfontenla 12:801e58a7137c 304
lucasfontenla 12:801e58a7137c 305 while(!x_limit_min){
lucasfontenla 12:801e58a7137c 306 if(dirX != x_minus){
lucasfontenla 12:801e58a7137c 307 dirX = x_minus;
lucasfontenla 12:801e58a7137c 308 }
lucasfontenla 12:801e58a7137c 309 stepX = 1;
lucasfontenla 12:801e58a7137c 310 wait(time);
lucasfontenla 12:801e58a7137c 311 stepX = 0;
lucasfontenla 12:801e58a7137c 312 wait(time);
lucasfontenla 12:801e58a7137c 313 }
lucasfontenla 12:801e58a7137c 314
lucasfontenla 12:801e58a7137c 315 printf("X zero\n\r");
lucasfontenla 12:801e58a7137c 316
lucasfontenla 12:801e58a7137c 317 totalX = 0.0;
lucasfontenla 12:801e58a7137c 318 }
lucasfontenla 12:801e58a7137c 319
lucasfontenla 11:0e22a6fc7f87 320 void zeroY(int pps){
lucasfontenla 11:0e22a6fc7f87 321 float time = 1.0/pps/2.0;
lucasfontenla 11:0e22a6fc7f87 322
lucasfontenla 11:0e22a6fc7f87 323 while(!y_limit_min){
lucasfontenla 11:0e22a6fc7f87 324 if(dirY != y_minus){
lucasfontenla 11:0e22a6fc7f87 325 dirY = y_minus;
lucasfontenla 11:0e22a6fc7f87 326 }
lucasfontenla 11:0e22a6fc7f87 327 stepY = 1;
lucasfontenla 11:0e22a6fc7f87 328 wait(time);
lucasfontenla 11:0e22a6fc7f87 329 stepY = 0;
lucasfontenla 11:0e22a6fc7f87 330 wait(time);
lucasfontenla 11:0e22a6fc7f87 331 }
lucasfontenla 11:0e22a6fc7f87 332
lucasfontenla 11:0e22a6fc7f87 333 printf("Y zero\n\r");
lucasfontenla 11:0e22a6fc7f87 334
lucasfontenla 11:0e22a6fc7f87 335 totalY = 0.0;
lucasfontenla 15:4604a7ee9c77 336 }*/
lucasfontenla 11:0e22a6fc7f87 337
lucasfontenla 7:27ea14e58788 338 float distance(int steps, float pitch){
lucasfontenla 7:27ea14e58788 339 float delta_S = (((float)steps)/steps_by_rotation)*pitch;
lucasfontenla 7:27ea14e58788 340 return delta_S;
lucasfontenla 7:27ea14e58788 341 }