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@11:0e22a6fc7f87, 2018-05-04 (annotated)

Committer:
lucasfontenla
Date:
Fri May 04 17:48:15 2018 +0000
Revision:
11:0e22a6fc7f87
Parent:
10:cd868ca9997c
Child:
12:801e58a7137c
Zeramento em Y completo

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 10:cd868ca9997c 53 int y_limit_min = 0;
lucasfontenla 10:cd868ca9997c 54 int y_limit_max = 0;
lucasfontenla 9:d771a576e4fc 55
lucasfontenla 5:0cbf491f703d 56 float ppsMax = 600.0;
lucasfontenla 5:0cbf491f703d 57
lucasfontenla 7:27ea14e58788 58 int totalX = 0;
lucasfontenla 7:27ea14e58788 59 int totalY = 0;
lucasfontenla 7:27ea14e58788 60
lucasfontenla 7:27ea14e58788 61 float X = 0;
lucasfontenla 7:27ea14e58788 62 float Y = 0;
lucasfontenla 7:27ea14e58788 63
lucasfontenla 9:d771a576e4fc 64 int endX1Status;
lucasfontenla 9:d771a576e4fc 65 int endX2Status;
lucasfontenla 9:d771a576e4fc 66
lucasfontenla 7:27ea14e58788 67 // machine parameters
lucasfontenla 8:de64672c240f 68 float xPitch = 3.0;
lucasfontenla 8:de64672c240f 69 float yPitch = 3.0;
lucasfontenla 7:27ea14e58788 70
lucasfontenla 7:27ea14e58788 71 float motor_steps_by_rotation = 200.0;
lucasfontenla 7:27ea14e58788 72 float step_mode = 0.5;
lucasfontenla 7:27ea14e58788 73 float steps_by_rotation = motor_steps_by_rotation/step_mode;
lucasfontenla 7:27ea14e58788 74
lucasfontenla 7:27ea14e58788 75 // prototype functions
lucasfontenla 9:d771a576e4fc 76 void setupPins();
lucasfontenla 5:0cbf491f703d 77 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 78 float distance(int steps, float pitch);
lucasfontenla 7:27ea14e58788 79
lucasfontenla 11:0e22a6fc7f87 80 void endX1Int_press(void);
lucasfontenla 11:0e22a6fc7f87 81 void endX1Int_release(void);
lucasfontenla 11:0e22a6fc7f87 82 void endX2Int_press(void);
lucasfontenla 11:0e22a6fc7f87 83 void endX2Int_release(void);
lucasfontenla 11:0e22a6fc7f87 84
lucasfontenla 10:cd868ca9997c 85 void endY1Int_press(void);
lucasfontenla 10:cd868ca9997c 86 void endY1Int_release(void);
lucasfontenla 10:cd868ca9997c 87 void endY2Int_press(void);
lucasfontenla 10:cd868ca9997c 88 void endY2Int_release(void);
lucasfontenla 9:d771a576e4fc 89
lucasfontenla 11:0e22a6fc7f87 90 void zeroY(int pps);
lucasfontenla 11:0e22a6fc7f87 91
lucasfontenla 7:27ea14e58788 92 // MAIN PROGRAM ----------------------------------------------------------------
lucasfontenla 7:27ea14e58788 93
lucasfontenla 7:27ea14e58788 94 int contador = 0;
lucasfontenla 1:b2e395e50a45 95
lucasfontenla 1:b2e395e50a45 96 int main(){
lucasfontenla 4:48717b42eab2 97 printf("Starting...\r\n");
lucasfontenla 5:0cbf491f703d 98
lucasfontenla 10:cd868ca9997c 99 endY1.fall(&endY1Int_press);
lucasfontenla 10:cd868ca9997c 100 endY1.rise(&endY1Int_release);
lucasfontenla 9:d771a576e4fc 101
lucasfontenla 10:cd868ca9997c 102 endY2.fall(&endY2Int_press);
lucasfontenla 10:cd868ca9997c 103 endY2.rise(&endY2Int_release);
lucasfontenla 9:d771a576e4fc 104
lucasfontenla 5:0cbf491f703d 105 if(modeStatus){
lucasfontenla 5:0cbf491f703d 106 //Código de JOG
lucasfontenla 7:27ea14e58788 107 printf("JOG Selected\n");
lucasfontenla 5:0cbf491f703d 108 while(1){
lucasfontenla 5:0cbf491f703d 109 valX = joyX;
lucasfontenla 5:0cbf491f703d 110 valY = joyY;
lucasfontenla 5:0cbf491f703d 111
lucasfontenla 5:0cbf491f703d 112 valZUp = zUp;
lucasfontenla 5:0cbf491f703d 113 valZDwn = zDwn;
lucasfontenla 5:0cbf491f703d 114
lucasfontenla 5:0cbf491f703d 115 if(valX > 0.7){
lucasfontenla 9:d771a576e4fc 116 if(x_dir != x_minus){
lucasfontenla 9:d771a576e4fc 117 x_dir = x_minus;
lucasfontenla 5:0cbf491f703d 118 }
lucasfontenla 7:27ea14e58788 119 activeX = 1;
lucasfontenla 7:27ea14e58788 120 totalX-=1;
lucasfontenla 5:0cbf491f703d 121 }
lucasfontenla 5:0cbf491f703d 122 else if(valX < 0.3){
lucasfontenla 9:d771a576e4fc 123 if(x_dir != x_plus){
lucasfontenla 9:d771a576e4fc 124 x_dir = x_plus;
lucasfontenla 5:0cbf491f703d 125 }
lucasfontenla 7:27ea14e58788 126 activeX = 1;
lucasfontenla 7:27ea14e58788 127 totalX+=1;
lucasfontenla 5:0cbf491f703d 128 }
lucasfontenla 5:0cbf491f703d 129 else{
lucasfontenla 5:0cbf491f703d 130 activeX = 0;
lucasfontenla 4:48717b42eab2 131 }
lucasfontenla 5:0cbf491f703d 132
lucasfontenla 5:0cbf491f703d 133 //----------------------------------------------------------------------
lucasfontenla 5:0cbf491f703d 134
lucasfontenla 5:0cbf491f703d 135 if(valY > 0.7){
lucasfontenla 9:d771a576e4fc 136 if(y_dir != y_plus){
lucasfontenla 9:d771a576e4fc 137 y_dir = y_plus;
lucasfontenla 5:0cbf491f703d 138 }
lucasfontenla 7:27ea14e58788 139 activeY = 1;
lucasfontenla 7:27ea14e58788 140 totalY+=1;
lucasfontenla 5:0cbf491f703d 141 }
lucasfontenla 5:0cbf491f703d 142 else if(valY < 0.3){
lucasfontenla 9:d771a576e4fc 143 if(y_dir != y_minus){
lucasfontenla 9:d771a576e4fc 144 y_dir = y_minus;
lucasfontenla 5:0cbf491f703d 145 }
lucasfontenla 7:27ea14e58788 146 activeY = 1;
lucasfontenla 7:27ea14e58788 147 totalY-=1;
lucasfontenla 5:0cbf491f703d 148 }
lucasfontenla 5:0cbf491f703d 149 else{
lucasfontenla 5:0cbf491f703d 150 activeY = 0;
lucasfontenla 4:48717b42eab2 151 }
lucasfontenla 5:0cbf491f703d 152
lucasfontenla 5:0cbf491f703d 153 //----------------------------------------------------------------------
lucasfontenla 5:0cbf491f703d 154
lucasfontenla 5:0cbf491f703d 155 if(!valZUp && valZDwn){
lucasfontenla 9:d771a576e4fc 156 if(z_dir != z_minus){
lucasfontenla 9:d771a576e4fc 157 z_dir = z_minus;
lucasfontenla 7:27ea14e58788 158 //totalZ-=1;
lucasfontenla 5:0cbf491f703d 159 }
lucasfontenla 5:0cbf491f703d 160 activeZ = 1;
lucasfontenla 5:0cbf491f703d 161 }
lucasfontenla 5:0cbf491f703d 162 else if(!valZDwn && valZUp){
lucasfontenla 9:d771a576e4fc 163 if(z_dir != z_plus){
lucasfontenla 9:d771a576e4fc 164 z_dir = z_plus;
lucasfontenla 7:27ea14e58788 165 //totalZ+=1;
lucasfontenla 5:0cbf491f703d 166 }
lucasfontenla 5:0cbf491f703d 167 activeZ = 1;
lucasfontenla 5:0cbf491f703d 168 }
lucasfontenla 5:0cbf491f703d 169 else {
lucasfontenla 5:0cbf491f703d 170 activeZ = 0;
lucasfontenla 5:0cbf491f703d 171 }
lucasfontenla 5:0cbf491f703d 172
lucasfontenla 5:0cbf491f703d 173 //----------------------------------------------------------------------
lucasfontenla 5:0cbf491f703d 174
lucasfontenla 7:27ea14e58788 175 move(ppsMax, x_dir, y_dir, z_dir, activeX, activeY, activeZ);
lucasfontenla 7:27ea14e58788 176 X = distance(totalX, xPitch);
lucasfontenla 7:27ea14e58788 177 Y = distance(totalY, yPitch);
lucasfontenla 7:27ea14e58788 178
lucasfontenla 7:27ea14e58788 179 if(contador > 10000){
lucasfontenla 7:27ea14e58788 180 X = distance(totalX, xPitch);
lucasfontenla 7:27ea14e58788 181 Y = distance(totalY, yPitch);
lucasfontenla 7:27ea14e58788 182 printf("X: %.2f Y: %.2f\r\n", X, Y);
lucasfontenla 7:27ea14e58788 183 contador = 0;
lucasfontenla 7:27ea14e58788 184 }
lucasfontenla 7:27ea14e58788 185
lucasfontenla 7:27ea14e58788 186 contador+=1;
lucasfontenla 4:48717b42eab2 187 }
lucasfontenla 5:0cbf491f703d 188 }
lucasfontenla 5:0cbf491f703d 189
lucasfontenla 5:0cbf491f703d 190 else {
lucasfontenla 11:0e22a6fc7f87 191 zeroY(ppsMax);
lucasfontenla 9:d771a576e4fc 192
lucasfontenla 11:0e22a6fc7f87 193 X = distance(totalX, xPitch);
lucasfontenla 11:0e22a6fc7f87 194 Y = distance(totalY, yPitch);
lucasfontenla 9:d771a576e4fc 195
lucasfontenla 11:0e22a6fc7f87 196 printf("X: %.2f Y: %.2f\r\n", X, Y);
lucasfontenla 9:d771a576e4fc 197 }
lucasfontenla 9:d771a576e4fc 198 }
lucasfontenla 9:d771a576e4fc 199
lucasfontenla 9:d771a576e4fc 200 // FUNCTIONS ----------------------------------------------------------------
lucasfontenla 5:0cbf491f703d 201
lucasfontenla 10:cd868ca9997c 202 void endY1Int_press(void){
lucasfontenla 9:d771a576e4fc 203 //if(debounce.read_ms() > 50)
lucasfontenla 10:cd868ca9997c 204 printf("Y1 - press\n\r");
lucasfontenla 10:cd868ca9997c 205 y_limit_min = 1;
lucasfontenla 9:d771a576e4fc 206 //debounce.reset();
lucasfontenla 9:d771a576e4fc 207 }
lucasfontenla 9:d771a576e4fc 208
lucasfontenla 10:cd868ca9997c 209 void endY1Int_release(void){
lucasfontenla 9:d771a576e4fc 210 //if(debounce.read_ms() > 50)
lucasfontenla 10:cd868ca9997c 211 printf("Y1 - release\n\r");
lucasfontenla 10:cd868ca9997c 212 y_limit_min = 0;
lucasfontenla 9:d771a576e4fc 213 //debounce.reset();
lucasfontenla 9:d771a576e4fc 214 }
lucasfontenla 9:d771a576e4fc 215
lucasfontenla 10:cd868ca9997c 216 void endY2Int_press(void){
lucasfontenla 9:d771a576e4fc 217 //if(debounce.read_ms() > 50)
lucasfontenla 10:cd868ca9997c 218 printf("Y2 - press\n\r");
lucasfontenla 10:cd868ca9997c 219 y_limit_max = 1;
lucasfontenla 9:d771a576e4fc 220 //debounce.reset();
lucasfontenla 9:d771a576e4fc 221 }
lucasfontenla 9:d771a576e4fc 222
lucasfontenla 10:cd868ca9997c 223 void endY2Int_release(void){
lucasfontenla 9:d771a576e4fc 224 //if(debounce.read_ms() > 50)
lucasfontenla 10:cd868ca9997c 225 printf("Y2 - release\n\r");
lucasfontenla 10:cd868ca9997c 226 y_limit_max = 0;
lucasfontenla 9:d771a576e4fc 227 //debounce.reset();
lucasfontenla 9:d771a576e4fc 228 }
lucasfontenla 7:27ea14e58788 229
lucasfontenla 5:0cbf491f703d 230 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 231 float time = 1.0/pps/2.0;
lucasfontenla 5:0cbf491f703d 232
lucasfontenla 5:0cbf491f703d 233 dirX = x_dir;
lucasfontenla 5:0cbf491f703d 234 dirY = y_dir;
lucasfontenla 5:0cbf491f703d 235 dirZ = z_dir;
lucasfontenla 5:0cbf491f703d 236
lucasfontenla 11:0e22a6fc7f87 237 if((y_dir == y_minus) && y_limit_min){
lucasfontenla 11:0e22a6fc7f87 238 y_step = 0;
lucasfontenla 11:0e22a6fc7f87 239 }
lucasfontenla 11:0e22a6fc7f87 240 if((y_dir == y_plus) && y_limit_max){
lucasfontenla 11:0e22a6fc7f87 241 y_step = 0;
lucasfontenla 11:0e22a6fc7f87 242 }
lucasfontenla 11:0e22a6fc7f87 243
lucasfontenla 5:0cbf491f703d 244 int max_val;
lucasfontenla 5:0cbf491f703d 245
lucasfontenla 6:af32c68ff4cf 246 // maior número de passos -------
lucasfontenla 5:0cbf491f703d 247 if(x_step > y_step){
lucasfontenla 5:0cbf491f703d 248 if(x_step > z_step){
lucasfontenla 5:0cbf491f703d 249 max_val = x_step;
lucasfontenla 4:48717b42eab2 250 }
lucasfontenla 4:48717b42eab2 251 else {
lucasfontenla 5:0cbf491f703d 252 max_val = z_step;
lucasfontenla 4:48717b42eab2 253 }
lucasfontenla 5:0cbf491f703d 254 } else if(y_step > z_step){
lucasfontenla 5:0cbf491f703d 255 max_val = y_step;
lucasfontenla 5:0cbf491f703d 256 }
lucasfontenla 5:0cbf491f703d 257 else {
lucasfontenla 5:0cbf491f703d 258 max_val = z_step;
lucasfontenla 5:0cbf491f703d 259 }
lucasfontenla 5:0cbf491f703d 260
lucasfontenla 5:0cbf491f703d 261 for(int i = 0; i < max_val; i++){
lucasfontenla 6:af32c68ff4cf 262 if(i >= x_step){
lucasfontenla 5:0cbf491f703d 263 stepX = 0;
lucasfontenla 5:0cbf491f703d 264 }
lucasfontenla 5:0cbf491f703d 265 else { stepX = 1; }
lucasfontenla 4:48717b42eab2 266
lucasfontenla 6:af32c68ff4cf 267 if(i >= y_step){
lucasfontenla 5:0cbf491f703d 268 stepY = 0;
lucasfontenla 5:0cbf491f703d 269 }
lucasfontenla 11:0e22a6fc7f87 270 else {
lucasfontenla 11:0e22a6fc7f87 271 stepY = 1;}
lucasfontenla 5:0cbf491f703d 272
lucasfontenla 6:af32c68ff4cf 273 if(i >= z_step){
lucasfontenla 5:0cbf491f703d 274 stepZ = 0;
lucasfontenla 5:0cbf491f703d 275 }
lucasfontenla 5:0cbf491f703d 276 else { stepZ = 1; }
lucasfontenla 5:0cbf491f703d 277
lucasfontenla 5:0cbf491f703d 278 wait(time);
lucasfontenla 4:48717b42eab2 279 stepX = 0;
lucasfontenla 4:48717b42eab2 280 stepY = 0;
lucasfontenla 4:48717b42eab2 281 stepZ = 0;
lucasfontenla 5:0cbf491f703d 282 wait(time);
lucasfontenla 4:48717b42eab2 283 }
lucasfontenla 5:0cbf491f703d 284 }
lucasfontenla 7:27ea14e58788 285
lucasfontenla 11:0e22a6fc7f87 286 void zeroY(int pps){
lucasfontenla 11:0e22a6fc7f87 287 float time = 1.0/pps/2.0;
lucasfontenla 11:0e22a6fc7f87 288
lucasfontenla 11:0e22a6fc7f87 289 while(!y_limit_min){
lucasfontenla 11:0e22a6fc7f87 290 if(dirY != y_minus){
lucasfontenla 11:0e22a6fc7f87 291 dirY = y_minus;
lucasfontenla 11:0e22a6fc7f87 292 }
lucasfontenla 11:0e22a6fc7f87 293 stepY = 1;
lucasfontenla 11:0e22a6fc7f87 294 wait(time);
lucasfontenla 11:0e22a6fc7f87 295 stepY = 0;
lucasfontenla 11:0e22a6fc7f87 296 wait(time);
lucasfontenla 11:0e22a6fc7f87 297 }
lucasfontenla 11:0e22a6fc7f87 298
lucasfontenla 11:0e22a6fc7f87 299 printf("Y zero\n\r");
lucasfontenla 11:0e22a6fc7f87 300
lucasfontenla 11:0e22a6fc7f87 301 totalY = 0.0;
lucasfontenla 11:0e22a6fc7f87 302 }
lucasfontenla 11:0e22a6fc7f87 303
lucasfontenla 7:27ea14e58788 304 float distance(int steps, float pitch){
lucasfontenla 7:27ea14e58788 305 float delta_S = (((float)steps)/steps_by_rotation)*pitch;
lucasfontenla 7:27ea14e58788 306 return delta_S;
lucasfontenla 7:27ea14e58788 307 }