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@9:d771a576e4fc, 2018-05-04 (annotated)

Committer:
lucasfontenla
Date:
Fri May 04 15:57:10 2018 +0000
Revision:
9:d771a576e4fc
Parent:
8:de64672c240f
Child:
10:cd868ca9997c
Fim de curso em Y funcionando.

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 9:d771a576e4fc 20 InterruptIn endX1(D5);
lucasfontenla 9:d771a576e4fc 21 InterruptIn endX2(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 9:d771a576e4fc 53 int x_limit_min = 0;
lucasfontenla 9:d771a576e4fc 54 int x_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 9:d771a576e4fc 80 void endX1Int_press(void);
lucasfontenla 9:d771a576e4fc 81 void endX1Int_release(void);
lucasfontenla 9:d771a576e4fc 82
lucasfontenla 9:d771a576e4fc 83 void endX2Int_press(void);
lucasfontenla 9:d771a576e4fc 84 void endX2Int_release(void);
lucasfontenla 9:d771a576e4fc 85
lucasfontenla 7:27ea14e58788 86 // MAIN PROGRAM ----------------------------------------------------------------
lucasfontenla 7:27ea14e58788 87
lucasfontenla 7:27ea14e58788 88 int contador = 0;
lucasfontenla 1:b2e395e50a45 89
lucasfontenla 1:b2e395e50a45 90 int main(){
lucasfontenla 4:48717b42eab2 91 printf("Starting...\r\n");
lucasfontenla 5:0cbf491f703d 92
lucasfontenla 9:d771a576e4fc 93 endX1.fall(&endX1Int_press);
lucasfontenla 9:d771a576e4fc 94 endX1.rise(&endX1Int_release);
lucasfontenla 9:d771a576e4fc 95
lucasfontenla 9:d771a576e4fc 96 endX2.fall(&endX2Int_press);
lucasfontenla 9:d771a576e4fc 97 endX2.rise(&endX2Int_release);
lucasfontenla 9:d771a576e4fc 98
lucasfontenla 5:0cbf491f703d 99 if(modeStatus){
lucasfontenla 5:0cbf491f703d 100 //Código de JOG
lucasfontenla 7:27ea14e58788 101 printf("JOG Selected\n");
lucasfontenla 5:0cbf491f703d 102 while(1){
lucasfontenla 5:0cbf491f703d 103 valX = joyX;
lucasfontenla 5:0cbf491f703d 104 valY = joyY;
lucasfontenla 5:0cbf491f703d 105
lucasfontenla 5:0cbf491f703d 106 valZUp = zUp;
lucasfontenla 5:0cbf491f703d 107 valZDwn = zDwn;
lucasfontenla 5:0cbf491f703d 108
lucasfontenla 5:0cbf491f703d 109 if(valX > 0.7){
lucasfontenla 9:d771a576e4fc 110 if(x_dir != x_minus){
lucasfontenla 9:d771a576e4fc 111 x_dir = x_minus;
lucasfontenla 5:0cbf491f703d 112 }
lucasfontenla 7:27ea14e58788 113 activeX = 1;
lucasfontenla 7:27ea14e58788 114 totalX-=1;
lucasfontenla 5:0cbf491f703d 115 }
lucasfontenla 5:0cbf491f703d 116 else if(valX < 0.3){
lucasfontenla 9:d771a576e4fc 117 if(x_dir != x_plus){
lucasfontenla 9:d771a576e4fc 118 x_dir = x_plus;
lucasfontenla 5:0cbf491f703d 119 }
lucasfontenla 7:27ea14e58788 120 activeX = 1;
lucasfontenla 7:27ea14e58788 121 totalX+=1;
lucasfontenla 5:0cbf491f703d 122 }
lucasfontenla 5:0cbf491f703d 123 else{
lucasfontenla 5:0cbf491f703d 124 activeX = 0;
lucasfontenla 4:48717b42eab2 125 }
lucasfontenla 5:0cbf491f703d 126
lucasfontenla 5:0cbf491f703d 127 //----------------------------------------------------------------------
lucasfontenla 5:0cbf491f703d 128
lucasfontenla 5:0cbf491f703d 129 if(valY > 0.7){
lucasfontenla 9:d771a576e4fc 130 if(y_dir != y_plus){
lucasfontenla 9:d771a576e4fc 131 y_dir = y_plus;
lucasfontenla 5:0cbf491f703d 132 }
lucasfontenla 7:27ea14e58788 133 activeY = 1;
lucasfontenla 7:27ea14e58788 134 totalY+=1;
lucasfontenla 5:0cbf491f703d 135 }
lucasfontenla 5:0cbf491f703d 136 else if(valY < 0.3){
lucasfontenla 9:d771a576e4fc 137 if(y_dir != y_minus){
lucasfontenla 9:d771a576e4fc 138 y_dir = y_minus;
lucasfontenla 5:0cbf491f703d 139 }
lucasfontenla 7:27ea14e58788 140 activeY = 1;
lucasfontenla 7:27ea14e58788 141 totalY-=1;
lucasfontenla 5:0cbf491f703d 142 }
lucasfontenla 5:0cbf491f703d 143 else{
lucasfontenla 5:0cbf491f703d 144 activeY = 0;
lucasfontenla 4:48717b42eab2 145 }
lucasfontenla 5:0cbf491f703d 146
lucasfontenla 5:0cbf491f703d 147 //----------------------------------------------------------------------
lucasfontenla 5:0cbf491f703d 148
lucasfontenla 5:0cbf491f703d 149 if(!valZUp && valZDwn){
lucasfontenla 9:d771a576e4fc 150 if(z_dir != z_minus){
lucasfontenla 9:d771a576e4fc 151 z_dir = z_minus;
lucasfontenla 7:27ea14e58788 152 //totalZ-=1;
lucasfontenla 5:0cbf491f703d 153 }
lucasfontenla 5:0cbf491f703d 154 activeZ = 1;
lucasfontenla 5:0cbf491f703d 155 }
lucasfontenla 5:0cbf491f703d 156 else if(!valZDwn && valZUp){
lucasfontenla 9:d771a576e4fc 157 if(z_dir != z_plus){
lucasfontenla 9:d771a576e4fc 158 z_dir = z_plus;
lucasfontenla 7:27ea14e58788 159 //totalZ+=1;
lucasfontenla 5:0cbf491f703d 160 }
lucasfontenla 5:0cbf491f703d 161 activeZ = 1;
lucasfontenla 5:0cbf491f703d 162 }
lucasfontenla 5:0cbf491f703d 163 else {
lucasfontenla 5:0cbf491f703d 164 activeZ = 0;
lucasfontenla 5:0cbf491f703d 165 }
lucasfontenla 5:0cbf491f703d 166
lucasfontenla 5:0cbf491f703d 167 //----------------------------------------------------------------------
lucasfontenla 5:0cbf491f703d 168
lucasfontenla 7:27ea14e58788 169 move(ppsMax, x_dir, y_dir, z_dir, activeX, activeY, activeZ);
lucasfontenla 7:27ea14e58788 170 X = distance(totalX, xPitch);
lucasfontenla 7:27ea14e58788 171 Y = distance(totalY, yPitch);
lucasfontenla 7:27ea14e58788 172
lucasfontenla 7:27ea14e58788 173 if(contador > 10000){
lucasfontenla 7:27ea14e58788 174 X = distance(totalX, xPitch);
lucasfontenla 7:27ea14e58788 175 Y = distance(totalY, yPitch);
lucasfontenla 7:27ea14e58788 176 printf("X: %.2f Y: %.2f\r\n", X, Y);
lucasfontenla 7:27ea14e58788 177 contador = 0;
lucasfontenla 7:27ea14e58788 178 }
lucasfontenla 7:27ea14e58788 179
lucasfontenla 7:27ea14e58788 180 contador+=1;
lucasfontenla 4:48717b42eab2 181 }
lucasfontenla 5:0cbf491f703d 182 }
lucasfontenla 5:0cbf491f703d 183
lucasfontenla 5:0cbf491f703d 184 else {
lucasfontenla 9:d771a576e4fc 185 printf("X\n\r");
lucasfontenla 9:d771a576e4fc 186 move(ppsMax, x_plus, y_plus, z_plus, 1600, 0, 0);
lucasfontenla 9:d771a576e4fc 187 move(ppsMax, x_minus, y_plus, z_plus, 1600, 0, 0);
lucasfontenla 9:d771a576e4fc 188
lucasfontenla 9:d771a576e4fc 189 wait(2);
lucasfontenla 9:d771a576e4fc 190
lucasfontenla 9:d771a576e4fc 191 printf("Y\n\r");
lucasfontenla 9:d771a576e4fc 192 move(ppsMax, x_plus, y_plus, z_plus, 0, 1600, 0);
lucasfontenla 9:d771a576e4fc 193 move(ppsMax, x_minus, y_minus, z_plus, 0, 1600, 0);
lucasfontenla 9:d771a576e4fc 194
lucasfontenla 9:d771a576e4fc 195 wait(2);
lucasfontenla 9:d771a576e4fc 196
lucasfontenla 9:d771a576e4fc 197 printf("Z\n\r");
lucasfontenla 9:d771a576e4fc 198 move(ppsMax, x_plus, y_plus, z_plus, 0, 0, 600);
lucasfontenla 9:d771a576e4fc 199 move(ppsMax, x_minus, y_minus, z_minus, 0, 0, 600);
lucasfontenla 9:d771a576e4fc 200
lucasfontenla 9:d771a576e4fc 201 printf("Finished");
lucasfontenla 9:d771a576e4fc 202 }
lucasfontenla 9:d771a576e4fc 203 }
lucasfontenla 9:d771a576e4fc 204
lucasfontenla 9:d771a576e4fc 205 // FUNCTIONS ----------------------------------------------------------------
lucasfontenla 9:d771a576e4fc 206 void setupPins(){
lucasfontenla 9:d771a576e4fc 207 //endX1.mode(PullUp);
lucasfontenla 9:d771a576e4fc 208 //endX2.mode(PullUp);
lucasfontenla 5:0cbf491f703d 209 }
lucasfontenla 5:0cbf491f703d 210
lucasfontenla 9:d771a576e4fc 211 void endX1Int_press(void){
lucasfontenla 9:d771a576e4fc 212 //if(debounce.read_ms() > 50)
lucasfontenla 9:d771a576e4fc 213 printf("X1 - press\n\r");
lucasfontenla 9:d771a576e4fc 214 x_limit_min = 1;
lucasfontenla 9:d771a576e4fc 215 //debounce.reset();
lucasfontenla 9:d771a576e4fc 216 }
lucasfontenla 9:d771a576e4fc 217
lucasfontenla 9:d771a576e4fc 218 void endX1Int_release(void){
lucasfontenla 9:d771a576e4fc 219 //if(debounce.read_ms() > 50)
lucasfontenla 9:d771a576e4fc 220 printf("X1 - release\n\r");
lucasfontenla 9:d771a576e4fc 221 x_limit_min = 0;
lucasfontenla 9:d771a576e4fc 222 //debounce.reset();
lucasfontenla 9:d771a576e4fc 223 }
lucasfontenla 9:d771a576e4fc 224
lucasfontenla 9:d771a576e4fc 225 void endX2Int_press(void){
lucasfontenla 9:d771a576e4fc 226 //if(debounce.read_ms() > 50)
lucasfontenla 9:d771a576e4fc 227 printf("X2 - press\n\r");
lucasfontenla 9:d771a576e4fc 228 x_limit_max = 1;
lucasfontenla 9:d771a576e4fc 229 //debounce.reset();
lucasfontenla 9:d771a576e4fc 230 }
lucasfontenla 9:d771a576e4fc 231
lucasfontenla 9:d771a576e4fc 232 void endX2Int_release(void){
lucasfontenla 9:d771a576e4fc 233 //if(debounce.read_ms() > 50)
lucasfontenla 9:d771a576e4fc 234 printf("X2 - release\n\r");
lucasfontenla 9:d771a576e4fc 235 x_limit_max = 0;
lucasfontenla 9:d771a576e4fc 236 //debounce.reset();
lucasfontenla 9:d771a576e4fc 237 }
lucasfontenla 7:27ea14e58788 238
lucasfontenla 5:0cbf491f703d 239 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 240 float time = 1.0/pps/2.0;
lucasfontenla 5:0cbf491f703d 241
lucasfontenla 5:0cbf491f703d 242 dirX = x_dir;
lucasfontenla 5:0cbf491f703d 243 dirY = y_dir;
lucasfontenla 5:0cbf491f703d 244 dirZ = z_dir;
lucasfontenla 5:0cbf491f703d 245
lucasfontenla 5:0cbf491f703d 246 int max_val;
lucasfontenla 5:0cbf491f703d 247
lucasfontenla 6:af32c68ff4cf 248 // maior número de passos -------
lucasfontenla 5:0cbf491f703d 249 if(x_step > y_step){
lucasfontenla 5:0cbf491f703d 250 if(x_step > z_step){
lucasfontenla 5:0cbf491f703d 251 max_val = x_step;
lucasfontenla 4:48717b42eab2 252 }
lucasfontenla 4:48717b42eab2 253 else {
lucasfontenla 5:0cbf491f703d 254 max_val = z_step;
lucasfontenla 4:48717b42eab2 255 }
lucasfontenla 5:0cbf491f703d 256 } else if(y_step > z_step){
lucasfontenla 5:0cbf491f703d 257 max_val = y_step;
lucasfontenla 5:0cbf491f703d 258 }
lucasfontenla 5:0cbf491f703d 259 else {
lucasfontenla 5:0cbf491f703d 260 max_val = z_step;
lucasfontenla 5:0cbf491f703d 261 }
lucasfontenla 5:0cbf491f703d 262
lucasfontenla 5:0cbf491f703d 263 for(int i = 0; i < max_val; i++){
lucasfontenla 6:af32c68ff4cf 264 if(i >= x_step){
lucasfontenla 5:0cbf491f703d 265 stepX = 0;
lucasfontenla 5:0cbf491f703d 266 }
lucasfontenla 5:0cbf491f703d 267 else { stepX = 1; }
lucasfontenla 4:48717b42eab2 268
lucasfontenla 6:af32c68ff4cf 269 if(i >= y_step){
lucasfontenla 5:0cbf491f703d 270 stepY = 0;
lucasfontenla 5:0cbf491f703d 271 }
lucasfontenla 5:0cbf491f703d 272 else { stepY = 1; }
lucasfontenla 5:0cbf491f703d 273
lucasfontenla 6:af32c68ff4cf 274 if(i >= z_step){
lucasfontenla 5:0cbf491f703d 275 stepZ = 0;
lucasfontenla 5:0cbf491f703d 276 }
lucasfontenla 5:0cbf491f703d 277 else { stepZ = 1; }
lucasfontenla 5:0cbf491f703d 278
lucasfontenla 5:0cbf491f703d 279 wait(time);
lucasfontenla 4:48717b42eab2 280 stepX = 0;
lucasfontenla 4:48717b42eab2 281 stepY = 0;
lucasfontenla 4:48717b42eab2 282 stepZ = 0;
lucasfontenla 5:0cbf491f703d 283 wait(time);
lucasfontenla 4:48717b42eab2 284 }
lucasfontenla 5:0cbf491f703d 285 }
lucasfontenla 7:27ea14e58788 286
lucasfontenla 7:27ea14e58788 287 float distance(int steps, float pitch){
lucasfontenla 7:27ea14e58788 288 float delta_S = (((float)steps)/steps_by_rotation)*pitch;
lucasfontenla 7:27ea14e58788 289 return delta_S;
lucasfontenla 7:27ea14e58788 290 }