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@12:801e58a7137c, 2018-05-04 (annotated)
- 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?
| User | Revision | Line number | New 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 | } |