50_135_507_38_Insper / Mbed OS Teste_2Motores_Joystick_copy

2 Motores + Joystick

Dependencies:   X_NUCLEO_IHM01A1 TextLCD

Fork of HelloWorld_IHM01A1_2Motors_mbedOS by ST

main.cpp@44:eecdc0b60f14, 2018-05-16 (annotated)

Committer:
leogrotti
Date:
Wed May 16 15:40:31 2018 +0000
Revision:
44:eecdc0b60f14
Parent:
43:d08a3f6b65b5
Child:
45:8fab21ab1251
salva 3 pontos e vai automatico; velo nova; contagem passos certa

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Davidroid 0:e6a49a092e2a 1 /**
Davidroid 0:e6a49a092e2a 2 ******************************************************************************
Davidroid 0:e6a49a092e2a 3 * @file main.cpp
Davidroid 17:aae1446c67f4 4 * @author Davide Aliprandi, STMicroelectronics
Davidroid 0:e6a49a092e2a 5 * @version V1.0.0
Davidroid 0:e6a49a092e2a 6 * @date October 14th, 2015
Davidroid 17:aae1446c67f4 7 * @brief mbed test application for the STMicroelectronics X-NUCLEO-IHM01A1
Davidroid 3:02d9ec4f88b2 8 * Motor Control Expansion Board: control of 2 motors.
Davidroid 0:e6a49a092e2a 9 ******************************************************************************
Davidroid 0:e6a49a092e2a 10 * @attention
Davidroid 0:e6a49a092e2a 11 *
Davidroid 0:e6a49a092e2a 12 * <h2><center>&copy; COPYRIGHT(c) 2015 STMicroelectronics</center></h2>
Davidroid 0:e6a49a092e2a 13 *
Davidroid 0:e6a49a092e2a 14 * Redistribution and use in source and binary forms, with or without modification,
Davidroid 0:e6a49a092e2a 15 * are permitted provided that the following conditions are met:
Davidroid 0:e6a49a092e2a 16 * 1. Redistributions of source code must retain the above copyright notice,
Davidroid 0:e6a49a092e2a 17 * this list of conditions and the following disclaimer.
Davidroid 0:e6a49a092e2a 18 * 2. Redistributions in binary form must reproduce the above copyright notice,
Davidroid 0:e6a49a092e2a 19 * this list of conditions and the following disclaimer in the documentation
Davidroid 0:e6a49a092e2a 20 * and/or other materials provided with the distribution.
Davidroid 0:e6a49a092e2a 21 * 3. Neither the name of STMicroelectronics nor the names of its contributors
Davidroid 0:e6a49a092e2a 22 * may be used to endorse or promote products derived from this software
Davidroid 0:e6a49a092e2a 23 * without specific prior written permission.
Davidroid 0:e6a49a092e2a 24 *
Davidroid 0:e6a49a092e2a 25 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
Davidroid 0:e6a49a092e2a 26 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
Davidroid 0:e6a49a092e2a 27 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
Davidroid 0:e6a49a092e2a 28 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
Davidroid 0:e6a49a092e2a 29 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
Davidroid 0:e6a49a092e2a 30 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
Davidroid 0:e6a49a092e2a 31 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
Davidroid 0:e6a49a092e2a 32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
Davidroid 0:e6a49a092e2a 33 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
Davidroid 0:e6a49a092e2a 34 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
Davidroid 0:e6a49a092e2a 35 *
Davidroid 0:e6a49a092e2a 36 ******************************************************************************
Davidroid 0:e6a49a092e2a 37 */
Davidroid 0:e6a49a092e2a 38
Davidroid 0:e6a49a092e2a 39
Davidroid 0:e6a49a092e2a 40 /* Includes ------------------------------------------------------------------*/
Davidroid 0:e6a49a092e2a 41
Davidroid 0:e6a49a092e2a 42 /* mbed specific header files. */
Davidroid 0:e6a49a092e2a 43 #include "mbed.h"
Davidroid 0:e6a49a092e2a 44
Davidroid 0:e6a49a092e2a 45 /* Helper header files. */
Davidroid 0:e6a49a092e2a 46 #include "DevSPI.h"
Davidroid 0:e6a49a092e2a 47
Davidroid 0:e6a49a092e2a 48 /* Component specific header files. */
Davidroid 29:526970c1d998 49 #include "L6474.h"
Davidroid 0:e6a49a092e2a 50
Davidroid 0:e6a49a092e2a 51
Davidroid 0:e6a49a092e2a 52 /* Definitions ---------------------------------------------------------------*/
Davidroid 0:e6a49a092e2a 53
Davidroid 24:8cb3c4ad055f 54 /* Number of steps. */
Davidroid 9:a9e51320aee4 55 #define STEPS 3200
Davidroid 0:e6a49a092e2a 56
Davidroid 24:8cb3c4ad055f 57 /* Delay in milliseconds. */
leogrotti 42:a04bff02f231 58 #define DELAY_1 20
leogrotti 43:d08a3f6b65b5 59 #define DELAY_2 200
leogrotti 43:d08a3f6b65b5 60 #define DELAY_3 2000
Davidroid 24:8cb3c4ad055f 61
Davidroid 24:8cb3c4ad055f 62 /* Speed in pps (Pulses Per Second).
Davidroid 24:8cb3c4ad055f 63 In Full Step mode: 1 pps = 1 step/s).
Davidroid 24:8cb3c4ad055f 64 In 1/N Step Mode: N pps = 1 step/s). */
leogrotti 34:098efd69c86d 65 #define SPEED_1 1300
leogrotti 35:a05bef8dd995 66 #define SPEED_2 1300
Davidroid 24:8cb3c4ad055f 67
Davidroid 0:e6a49a092e2a 68
Brunoporto2702 32:481674c0f90d 69
Davidroid 0:e6a49a092e2a 70 /* Variables -----------------------------------------------------------------*/
leogrotti 42:a04bff02f231 71 //InterruptIn sensorX1(PC_9);
leogrotti 42:a04bff02f231 72 //InterruptIn sensorY1(PA_14);
leogrotti 40:0b517b49f70d 73
leogrotti 42:a04bff02f231 74
Brunoporto2702 32:481674c0f90d 75
Brunoporto2702 32:481674c0f90d 76 Serial pc(USBTX, USBRX);
Brunoporto2702 32:481674c0f90d 77
leogrotti 42:a04bff02f231 78 AnalogIn eixo_X(A2);
leogrotti 42:a04bff02f231 79 AnalogIn eixo_Y(A3);
leogrotti 42:a04bff02f231 80 AnalogIn eixo_ZU(A0);
leogrotti 42:a04bff02f231 81 AnalogIn eixo_ZD(A1);
Brunoporto2702 32:481674c0f90d 82
leogrotti 43:d08a3f6b65b5 83 DigitalIn salva_pos (PC_1);
leogrotti 43:d08a3f6b65b5 84 DigitalIn vai_cami (PC_0);
leogrotti 43:d08a3f6b65b5 85
leogrotti 43:d08a3f6b65b5 86
Brunoporto2702 32:481674c0f90d 87
Brunoporto2702 32:481674c0f90d 88
Brunoporto2702 32:481674c0f90d 89 float x;
Brunoporto2702 32:481674c0f90d 90 float y;
Brunoporto2702 32:481674c0f90d 91 float w;
Brunoporto2702 32:481674c0f90d 92 float u;
leogrotti 42:a04bff02f231 93 float zu;
leogrotti 42:a04bff02f231 94 float zd;
leogrotti 43:d08a3f6b65b5 95 float a;
leogrotti 43:d08a3f6b65b5 96 float pos;
leogrotti 43:d08a3f6b65b5 97 float sp;
leogrotti 43:d08a3f6b65b5 98 float vc;
Brunoporto2702 32:481674c0f90d 99
leogrotti 42:a04bff02f231 100 int count;
leogrotti 43:d08a3f6b65b5 101 int position1Y ;
leogrotti 43:d08a3f6b65b5 102 int position1X ;
leogrotti 43:d08a3f6b65b5 103 int position1Z ;
leogrotti 43:d08a3f6b65b5 104 int position2Y ;
leogrotti 43:d08a3f6b65b5 105 int position2X ;
leogrotti 43:d08a3f6b65b5 106 int position2Z ;
leogrotti 43:d08a3f6b65b5 107
leogrotti 43:d08a3f6b65b5 108 int position3Y ;
leogrotti 43:d08a3f6b65b5 109 int position3X ;
leogrotti 43:d08a3f6b65b5 110 int position3Z ;
leogrotti 42:a04bff02f231 111
leogrotti 44:eecdc0b60f14 112 int positionY_agora1;
leogrotti 44:eecdc0b60f14 113 int positionX_agora1;
leogrotti 44:eecdc0b60f14 114 int positionZ_agora1;
leogrotti 44:eecdc0b60f14 115 int positionY_agora2;
leogrotti 44:eecdc0b60f14 116 int positionX_agora2;
leogrotti 44:eecdc0b60f14 117 int positionZ_agora2;
leogrotti 44:eecdc0b60f14 118 int positionY_agora3;
leogrotti 44:eecdc0b60f14 119 int positionX_agora3;
leogrotti 44:eecdc0b60f14 120 int positionZ_agora3;
leogrotti 44:eecdc0b60f14 121
leogrotti 44:eecdc0b60f14 122 int dif_posX1;
leogrotti 44:eecdc0b60f14 123 int dif_posY1;
leogrotti 44:eecdc0b60f14 124 int dif_posZ1;
leogrotti 44:eecdc0b60f14 125 int dif_posX2;
leogrotti 44:eecdc0b60f14 126 int dif_posY2;
leogrotti 44:eecdc0b60f14 127 int dif_posZ2;
leogrotti 44:eecdc0b60f14 128 int dif_posX3;
leogrotti 44:eecdc0b60f14 129 int dif_posY3;
leogrotti 44:eecdc0b60f14 130 int dif_posZ3;
leogrotti 44:eecdc0b60f14 131
leogrotti 44:eecdc0b60f14 132
leogrotti 36:0cd4fdbb40af 133
leogrotti 42:a04bff02f231 134 bool flagX1=0;
leogrotti 42:a04bff02f231 135 bool flagY1=0;
leogrotti 35:a05bef8dd995 136
leogrotti 35:a05bef8dd995 137
leogrotti 34:098efd69c86d 138 unsigned int minspeed = 1300;
Brunoporto2702 32:481674c0f90d 139
leogrotti 34:098efd69c86d 140 //int step = 0x08;
Brunoporto2702 32:481674c0f90d 141
leogrotti 35:a05bef8dd995 142 unsigned int speed1;
leogrotti 35:a05bef8dd995 143 unsigned int speed2;
leogrotti 39:7e30bcc989d3 144 unsigned int speed3;
leogrotti 36:0cd4fdbb40af 145
leogrotti 38:c3d77ff8168a 146 //unsigned int pos3;
Brunoporto2702 32:481674c0f90d 147
Davidroid 0:e6a49a092e2a 148
Davidroid 0:e6a49a092e2a 149 /* Motor Control Component. */
Davidroid 3:02d9ec4f88b2 150 L6474 *motor1;
Davidroid 3:02d9ec4f88b2 151 L6474 *motor2;
leogrotti 39:7e30bcc989d3 152 L6474 *motor3;
Davidroid 0:e6a49a092e2a 153
Davidroid 0:e6a49a092e2a 154
Davidroid 0:e6a49a092e2a 155 /* Main ----------------------------------------------------------------------*/
leogrotti 42:a04bff02f231 156 /*
leogrotti 41:7f91e949ca88 157 void released_1()
leogrotti 41:7f91e949ca88 158 {
leogrotti 41:7f91e949ca88 159 motor3->hard_stop();
leogrotti 41:7f91e949ca88 160 printf ("parou \r\n");
leogrotti 41:7f91e949ca88 161
leogrotti 41:7f91e949ca88 162 }
leogrotti 41:7f91e949ca88 163
leogrotti 41:7f91e949ca88 164 void released_2()
leogrotti 41:7f91e949ca88 165 {
leogrotti 41:7f91e949ca88 166 motor3->hard_stop();
leogrotti 41:7f91e949ca88 167 printf ("parou \r\n");
leogrotti 41:7f91e949ca88 168
leogrotti 41:7f91e949ca88 169 }
leogrotti 41:7f91e949ca88 170
leogrotti 42:a04bff02f231 171 void paraX1() {
leogrotti 42:a04bff02f231 172 flagX1 = 1;
leogrotti 42:a04bff02f231 173 motor2->hard_stop();
leogrotti 42:a04bff02f231 174
leogrotti 42:a04bff02f231 175 }
leogrotti 42:a04bff02f231 176
leogrotti 42:a04bff02f231 177 void continuaX1() {
leogrotti 42:a04bff02f231 178 flagX1 = 0;
leogrotti 42:a04bff02f231 179 }
leogrotti 42:a04bff02f231 180
leogrotti 42:a04bff02f231 181
leogrotti 42:a04bff02f231 182 void paraY1() {
leogrotti 42:a04bff02f231 183 flagY1 = 1;
leogrotti 42:a04bff02f231 184 motor1->hard_stop();
leogrotti 42:a04bff02f231 185
leogrotti 42:a04bff02f231 186 }
leogrotti 42:a04bff02f231 187
leogrotti 42:a04bff02f231 188 void continuaY1() {
leogrotti 42:a04bff02f231 189 flagY1 = 0;
leogrotti 42:a04bff02f231 190 }
leogrotti 42:a04bff02f231 191 */
leogrotti 44:eecdc0b60f14 192 int correcao_passo (int passo){
leogrotti 44:eecdc0b60f14 193 int passo_verdade;
leogrotti 44:eecdc0b60f14 194 passo_verdade = passo/1.0892 - 15,56;
leogrotti 44:eecdc0b60f14 195 return (passo_verdade);
leogrotti 44:eecdc0b60f14 196 }
leogrotti 44:eecdc0b60f14 197
Davidroid 0:e6a49a092e2a 198 int main()
Davidroid 0:e6a49a092e2a 199 {
Davidroid 8:cec4c2c03a27 200 /*----- Initialization. -----*/
Davidroid 8:cec4c2c03a27 201
Davidroid 0:e6a49a092e2a 202 /* Initializing SPI bus. */
Davidroid 0:e6a49a092e2a 203 DevSPI dev_spi(D11, D12, D13);
Davidroid 0:e6a49a092e2a 204
Davidroid 9:a9e51320aee4 205 /* Initializing Motor Control Components. */
Davidroid 5:a0268a435bb1 206 motor1 = new L6474(D2, D8, D7, D9, D10, dev_spi);
Davidroid 16:810667a9f31f 207 motor2 = new L6474(D2, D8, D4, D3, D10, dev_spi);
leogrotti 39:7e30bcc989d3 208 motor3 = new L6474(D2, D8, D5, D6, D10, dev_spi);
Davidroid 29:526970c1d998 209 if (motor1->init() != COMPONENT_OK) {
Davidroid 14:fcd452b03d1a 210 exit(EXIT_FAILURE);
Davidroid 28:3f17a4152bcf 211 }
Davidroid 29:526970c1d998 212 if (motor2->init() != COMPONENT_OK) {
Davidroid 14:fcd452b03d1a 213 exit(EXIT_FAILURE);
Davidroid 28:3f17a4152bcf 214 }
leogrotti 39:7e30bcc989d3 215 if (motor3->init() != COMPONENT_OK) {
leogrotti 39:7e30bcc989d3 216 exit(EXIT_FAILURE);
leogrotti 39:7e30bcc989d3 217 }
Davidroid 0:e6a49a092e2a 218
leogrotti 35:a05bef8dd995 219
Davidroid 8:cec4c2c03a27 220
Davidroid 8:cec4c2c03a27 221
leogrotti 39:7e30bcc989d3 222
leogrotti 42:a04bff02f231 223 /* Attaching and enabling interrupt handlers.
leogrotti 42:a04bff02f231 224 motor3->attach_flag_irq(&flag_irq_handler3);
leogrotti 40:0b517b49f70d 225 motor3->enable_flag_irq();
leogrotti 42:a04bff02f231 226 motor1->attach_flag_irq(&flag_irq_handler1);
leogrotti 42:a04bff02f231 227 motor1->enable_flag_irq();
leogrotti 42:a04bff02f231 228 motor2->attach_flag_irq(&flag_irq_handler2);
leogrotti 42:a04bff02f231 229 motor2->enable_flag_irq();
leogrotti 42:a04bff02f231 230 */
leogrotti 41:7f91e949ca88 231
leogrotti 42:a04bff02f231 232 //sensorX1.rise(&paraX1);
leogrotti 42:a04bff02f231 233 // sensorX1.fall(&continuaX1);
leogrotti 39:7e30bcc989d3 234
leogrotti 41:7f91e949ca88 235
leogrotti 42:a04bff02f231 236 //sensorY1.rise(&paraY1);
leogrotti 42:a04bff02f231 237 //sensorY1.fall(&continuaY1);
leogrotti 42:a04bff02f231 238
leogrotti 43:d08a3f6b65b5 239 a = 0;
leogrotti 43:d08a3f6b65b5 240 pos = 0;
leogrotti 43:d08a3f6b65b5 241
leogrotti 43:d08a3f6b65b5 242
leogrotti 42:a04bff02f231 243
Davidroid 28:3f17a4152bcf 244 while(true) {
leogrotti 42:a04bff02f231 245 count = count +1;
leogrotti 42:a04bff02f231 246 // Leitura analógica
Brunoporto2702 32:481674c0f90d 247 u = eixo_X.read();
Brunoporto2702 32:481674c0f90d 248 w = eixo_Y.read();
leogrotti 42:a04bff02f231 249 zu = eixo_ZU.read();
leogrotti 42:a04bff02f231 250 zd = eixo_ZD.read();
leogrotti 43:d08a3f6b65b5 251 sp = salva_pos.read();
leogrotti 43:d08a3f6b65b5 252 vc = vai_cami.read();
leogrotti 43:d08a3f6b65b5 253 if (count == 9999999999999999) {
leogrotti 43:d08a3f6b65b5 254 printf("zu =%f \r\n\n", zu);
leogrotti 43:d08a3f6b65b5 255 printf("zd =%f \r\n\n", zd);
leogrotti 43:d08a3f6b65b5 256 printf("sp =%f \r\n\n", sp);
leogrotti 43:d08a3f6b65b5 257 printf("vc =%f \r\n\n", vc);
leogrotti 43:d08a3f6b65b5 258 count = 0;
leogrotti 43:d08a3f6b65b5 259 }
leogrotti 43:d08a3f6b65b5 260
leogrotti 42:a04bff02f231 261
leogrotti 42:a04bff02f231 262 // Movimentando eixo Z cima caso botão 1 apertado
leogrotti 42:a04bff02f231 263 if (zu >0.7) {
leogrotti 42:a04bff02f231 264 motor3->run(StepperMotor::FWD);
leogrotti 42:a04bff02f231 265 speed3 = motor3->get_speed();
leogrotti 42:a04bff02f231 266 }
leogrotti 34:098efd69c86d 267
leogrotti 42:a04bff02f231 268 // Parando eixo Z caso botão 1 e 2 liberado
leogrotti 42:a04bff02f231 269 if (zu< 0.7 && zd < 0.7){
leogrotti 42:a04bff02f231 270 motor3->hard_stop();
leogrotti 42:a04bff02f231 271 speed3 = 0;
leogrotti 42:a04bff02f231 272 }
leogrotti 42:a04bff02f231 273
leogrotti 42:a04bff02f231 274 // Movimentando eixo Z baixo caso botão 2 apertado
leogrotti 42:a04bff02f231 275 if (zd >0.7) {
leogrotti 42:a04bff02f231 276 motor3->run(StepperMotor::BWD);
leogrotti 42:a04bff02f231 277 speed3 = motor3->get_speed();
leogrotti 42:a04bff02f231 278 }
leogrotti 34:098efd69c86d 279
leogrotti 42:a04bff02f231 280 // Parando eixo Z caso botão 1 e 2 liberado
leogrotti 42:a04bff02f231 281 if (zu< 0.7 && zd < 0.7){
leogrotti 42:a04bff02f231 282 motor3->hard_stop();
leogrotti 42:a04bff02f231 283 speed3 = 0;
leogrotti 42:a04bff02f231 284 }
leogrotti 42:a04bff02f231 285
leogrotti 42:a04bff02f231 286
leogrotti 42:a04bff02f231 287 // Movimentando eixo Y duas direções com joystick
leogrotti 42:a04bff02f231 288 //if (flagY1 == 0 ) {
leogrotti 42:a04bff02f231 289
leogrotti 42:a04bff02f231 290 // Movimentando eixo Y fwd
leogrotti 42:a04bff02f231 291 if (u>0.820) {
leogrotti 42:a04bff02f231 292 motor1->run(StepperMotor::FWD);
leogrotti 42:a04bff02f231 293 speed1 = motor1->get_speed();
leogrotti 42:a04bff02f231 294 }
leogrotti 42:a04bff02f231 295 // Movimentando eixo Y bwd
leogrotti 42:a04bff02f231 296 else{ if(u<0.65) {
leogrotti 42:a04bff02f231 297 motor1->run(StepperMotor::BWD);
leogrotti 43:d08a3f6b65b5 298 speed1 = motor1->get_speed();
Brunoporto2702 32:481674c0f90d 299 }
leogrotti 42:a04bff02f231 300 // parando eixo Y
leogrotti 42:a04bff02f231 301 else
leogrotti 42:a04bff02f231 302 motor1->hard_stop();
leogrotti 35:a05bef8dd995 303 speed1 = 0;
leogrotti 42:a04bff02f231 304 //}
leogrotti 42:a04bff02f231 305 }
leogrotti 42:a04bff02f231 306
leogrotti 42:a04bff02f231 307 // Movimentando eixo X duas direções com joystick
leogrotti 42:a04bff02f231 308 //if (flagX1 == 0) {
leogrotti 43:d08a3f6b65b5 309 // Movimentando eixo X fwd
leogrotti 42:a04bff02f231 310 if (w>0.80) {
leogrotti 34:098efd69c86d 311 motor2->run(StepperMotor::BWD);
leogrotti 42:a04bff02f231 312 speed2 = motor2->get_speed();
leogrotti 43:d08a3f6b65b5 313 wait_ms(DELAY_1);
leogrotti 42:a04bff02f231 314 }
leogrotti 43:d08a3f6b65b5 315 // Movimentando eixo X Bwd
leogrotti 42:a04bff02f231 316 else{ if(w<0.60) {
leogrotti 34:098efd69c86d 317 motor2->run(StepperMotor::FWD);
leogrotti 42:a04bff02f231 318 speed2 = motor2->get_speed();
leogrotti 43:d08a3f6b65b5 319 wait_ms(DELAY_1);
leogrotti 42:a04bff02f231 320 }
leogrotti 42:a04bff02f231 321 // parando eixo X
leogrotti 42:a04bff02f231 322 else
Brunoporto2702 32:481674c0f90d 323 motor2->hard_stop();
leogrotti 43:d08a3f6b65b5 324 speed2 = motor2->get_speed();
leogrotti 43:d08a3f6b65b5 325 wait_ms(DELAY_1);
leogrotti 43:d08a3f6b65b5 326
Brunoporto2702 32:481674c0f90d 327 }
leogrotti 44:eecdc0b60f14 328 /* if (vai_cami){
leogrotti 44:eecdc0b60f14 329 position1Y = motor1->get_position();
leogrotti 44:eecdc0b60f14 330 position1X = motor2->get_position();
leogrotti 44:eecdc0b60f14 331 position1Z = motor3->get_position();
leogrotti 44:eecdc0b60f14 332
leogrotti 44:eecdc0b60f14 333 printf( "pos1Y = %i \n\r", position1Y);
leogrotti 44:eecdc0b60f14 334 printf( "pos1X = %i \n\r", position1X);
leogrotti 44:eecdc0b60f14 335 printf( "pos1Z = %i \n\r", position1Z);
leogrotti 44:eecdc0b60f14 336
leogrotti 44:eecdc0b60f14 337
leogrotti 44:eecdc0b60f14 338 motor2->move(StepperMotor::BWD, 4574);
leogrotti 43:d08a3f6b65b5 339 motor2->wait_while_active();
leogrotti 44:eecdc0b60f14 340
leogrotti 44:eecdc0b60f14 341
leogrotti 44:eecdc0b60f14 342
leogrotti 44:eecdc0b60f14 343 positionY_agora1 = motor1->get_position();
leogrotti 44:eecdc0b60f14 344 positionX_agora1= motor2->get_position();
leogrotti 44:eecdc0b60f14 345 positionZ_agora1 = motor3->get_position();
leogrotti 44:eecdc0b60f14 346 printf( "posY_agora1 = %i \n\r", positionY_agora1);
leogrotti 44:eecdc0b60f14 347 printf( "posX_agora1 = %i \n\r", positionX_agora1);
leogrotti 44:eecdc0b60f14 348 printf( "posZ_agora1 = %i \n\r", positionZ_agora1);
leogrotti 44:eecdc0b60f14 349 a= 1;
leogrotti 43:d08a3f6b65b5 350 }
leogrotti 44:eecdc0b60f14 351 if (salva_pos){
leogrotti 44:eecdc0b60f14 352 position2Y = motor1->get_position();
leogrotti 44:eecdc0b60f14 353 position2X = motor2->get_position();
leogrotti 44:eecdc0b60f14 354 position2Z = motor3->get_position();
leogrotti 44:eecdc0b60f14 355
leogrotti 44:eecdc0b60f14 356 printf( "pos2Y = %i \n\r", position2Y);
leogrotti 44:eecdc0b60f14 357 printf( "pos2X = %i \n\r", position2X);
leogrotti 44:eecdc0b60f14 358 printf( "pos2Z = %i \n\r", position2Z);
leogrotti 44:eecdc0b60f14 359 motor2->move(StepperMotor::FWD, 4574);
leogrotti 43:d08a3f6b65b5 360 motor2->wait_while_active();
leogrotti 44:eecdc0b60f14 361 positionY_agora2 = motor1->get_position();
leogrotti 44:eecdc0b60f14 362 positionX_agora2= motor2->get_position();
leogrotti 44:eecdc0b60f14 363 positionZ_agora2 = motor3->get_position();
leogrotti 44:eecdc0b60f14 364 printf( "posY_agora2 = %i \n\r", positionY_agora2);
leogrotti 44:eecdc0b60f14 365 printf( "posX_agora2 = %i \n\r", positionX_agora2);
leogrotti 44:eecdc0b60f14 366 printf( "posZ_agora2 = %i \n\r", positionZ_agora2);
leogrotti 44:eecdc0b60f14 367
leogrotti 43:d08a3f6b65b5 368 a= 0;
leogrotti 43:d08a3f6b65b5 369 }
leogrotti 44:eecdc0b60f14 370
leogrotti 43:d08a3f6b65b5 371 */
leogrotti 43:d08a3f6b65b5 372
leogrotti 44:eecdc0b60f14 373 if (salva_pos) {
leogrotti 43:d08a3f6b65b5 374 if (pos==0) {
leogrotti 44:eecdc0b60f14 375 position1Y = motor1->get_position();
leogrotti 44:eecdc0b60f14 376 position1X = motor2->get_position();
leogrotti 44:eecdc0b60f14 377 position1Z = motor3->get_position();
leogrotti 43:d08a3f6b65b5 378
leogrotti 43:d08a3f6b65b5 379 printf( "pos1Y = %i \n\r", position1Y);
leogrotti 43:d08a3f6b65b5 380 printf( "pos1X = %i \n\r", position1X);
leogrotti 43:d08a3f6b65b5 381 printf( "pos1Z = %i \n\r", position1Z);
leogrotti 43:d08a3f6b65b5 382
leogrotti 43:d08a3f6b65b5 383 pos = 1;
leogrotti 44:eecdc0b60f14 384 printf( "pos = %f \n\r", pos);
leogrotti 43:d08a3f6b65b5 385 wait_ms(DELAY_2);
leogrotti 43:d08a3f6b65b5 386 }
leogrotti 43:d08a3f6b65b5 387 }
leogrotti 43:d08a3f6b65b5 388 if (salva_pos) {
leogrotti 43:d08a3f6b65b5 389 if (pos==1) {
leogrotti 44:eecdc0b60f14 390 position2Y = motor1->get_position();
leogrotti 44:eecdc0b60f14 391 position2X = motor2->get_position();
leogrotti 44:eecdc0b60f14 392 position2Z = motor3->get_position();
leogrotti 43:d08a3f6b65b5 393
leogrotti 43:d08a3f6b65b5 394 printf( "pos2Y = %i \n\r", position2Y);
leogrotti 43:d08a3f6b65b5 395 printf( "pos2X = %i \n\r", position2X);
leogrotti 43:d08a3f6b65b5 396 printf( "pos2Z = %i \n\r", position2Z);
leogrotti 43:d08a3f6b65b5 397
leogrotti 43:d08a3f6b65b5 398 pos = 2;
leogrotti 44:eecdc0b60f14 399 printf( "pos = %f \n\r", pos);
leogrotti 43:d08a3f6b65b5 400 wait_ms(DELAY_2);
leogrotti 43:d08a3f6b65b5 401 }
leogrotti 43:d08a3f6b65b5 402 }
leogrotti 43:d08a3f6b65b5 403 if (salva_pos) {
leogrotti 43:d08a3f6b65b5 404 if (pos==2) {
leogrotti 44:eecdc0b60f14 405 position3Y = motor1->get_position();
leogrotti 44:eecdc0b60f14 406 position3X = motor2->get_position();
leogrotti 44:eecdc0b60f14 407 position3Z = motor3->get_position();
leogrotti 43:d08a3f6b65b5 408
leogrotti 43:d08a3f6b65b5 409 printf( "pos3Y = %i \n\r", position3Y);
leogrotti 43:d08a3f6b65b5 410 printf( "pos3X = %i \n\r", position3X);
leogrotti 43:d08a3f6b65b5 411 printf( "pos3Z = %i \n\r", position3Z);
leogrotti 43:d08a3f6b65b5 412
leogrotti 43:d08a3f6b65b5 413 pos = 3;
leogrotti 44:eecdc0b60f14 414 printf( "pos = %f \n\r", pos);
leogrotti 43:d08a3f6b65b5 415 wait_ms(DELAY_2);
leogrotti 43:d08a3f6b65b5 416 }
leogrotti 44:eecdc0b60f14 417 }
leogrotti 43:d08a3f6b65b5 418 if (vai_cami) {
leogrotti 44:eecdc0b60f14 419 printf( "pos = %f \n\r", pos);
leogrotti 43:d08a3f6b65b5 420 if (pos ==0){
leogrotti 43:d08a3f6b65b5 421 printf("sem posicoes salvas");
leogrotti 43:d08a3f6b65b5 422 wait_ms(DELAY_2);
leogrotti 43:d08a3f6b65b5 423 }
leogrotti 43:d08a3f6b65b5 424 if (pos == 1) {
leogrotti 44:eecdc0b60f14 425 printf( "pos1Y = %i \n\r", position1Y);
leogrotti 44:eecdc0b60f14 426 printf( "pos1X = %i \n\r", position1X);
leogrotti 44:eecdc0b60f14 427 printf( "pos1Z = %i \n\r", position1Z);
leogrotti 44:eecdc0b60f14 428 positionY_agora1 = motor1->get_position();
leogrotti 44:eecdc0b60f14 429 positionX_agora1= motor2->get_position();
leogrotti 44:eecdc0b60f14 430 positionZ_agora1 = motor3->get_position();
leogrotti 44:eecdc0b60f14 431 printf( "posY_agora1 = %i \n\r", positionY_agora1);
leogrotti 44:eecdc0b60f14 432 printf( "posX_agora1 = %i \n\r", positionX_agora1);
leogrotti 44:eecdc0b60f14 433 printf( "posZ_agora1 = %i \n\r", positionZ_agora1);
leogrotti 44:eecdc0b60f14 434 dif_posX1 = position1X - positionX_agora1;
leogrotti 44:eecdc0b60f14 435 dif_posY1 = position1Y - positionY_agora1;
leogrotti 44:eecdc0b60f14 436 dif_posZ1 = position1Z - positionZ_agora1;
leogrotti 44:eecdc0b60f14 437
leogrotti 44:eecdc0b60f14 438 printf( "difY1 = %i \n\r", correcao_passo(dif_posY1));
leogrotti 44:eecdc0b60f14 439 printf( "difX1 = %i \n\r", correcao_passo(dif_posX1));
leogrotti 44:eecdc0b60f14 440 printf( "difZ1 = %i \n\r", correcao_passo(dif_posZ1));
leogrotti 44:eecdc0b60f14 441 if (dif_posX1 >0){
leogrotti 44:eecdc0b60f14 442 motor2->move(StepperMotor::FWD, correcao_passo(dif_posX1));
leogrotti 44:eecdc0b60f14 443 motor2->wait_while_active();
leogrotti 44:eecdc0b60f14 444 }
leogrotti 44:eecdc0b60f14 445 if (dif_posX1 <0){
leogrotti 44:eecdc0b60f14 446 dif_posX1 = -dif_posX1;
leogrotti 44:eecdc0b60f14 447 motor2->move(StepperMotor::BWD, correcao_passo(dif_posX1));
leogrotti 44:eecdc0b60f14 448 motor2->wait_while_active();
leogrotti 44:eecdc0b60f14 449 }
leogrotti 44:eecdc0b60f14 450 if (dif_posY1 >0){
leogrotti 44:eecdc0b60f14 451 motor1->move(StepperMotor::FWD, correcao_passo(dif_posY1));
leogrotti 43:d08a3f6b65b5 452 motor1->wait_while_active();
leogrotti 44:eecdc0b60f14 453 }
leogrotti 44:eecdc0b60f14 454 if (dif_posY1 <0){
leogrotti 44:eecdc0b60f14 455 dif_posY1 = -dif_posY1;
leogrotti 44:eecdc0b60f14 456 motor1->move(StepperMotor::BWD, correcao_passo(dif_posY1));
leogrotti 44:eecdc0b60f14 457 motor1->wait_while_active();
leogrotti 44:eecdc0b60f14 458 }
leogrotti 44:eecdc0b60f14 459 if (dif_posZ1 >0){
leogrotti 44:eecdc0b60f14 460 motor3->move(StepperMotor::FWD, correcao_passo(dif_posZ1));
leogrotti 43:d08a3f6b65b5 461 motor3->wait_while_active();
leogrotti 43:d08a3f6b65b5 462 }
leogrotti 44:eecdc0b60f14 463 if (dif_posZ1 <0){
leogrotti 44:eecdc0b60f14 464 dif_posZ1 = -dif_posZ1;
leogrotti 44:eecdc0b60f14 465 motor3->move(StepperMotor::BWD, correcao_passo(dif_posZ1));
leogrotti 44:eecdc0b60f14 466 motor3->wait_while_active();
leogrotti 44:eecdc0b60f14 467 }
leogrotti 44:eecdc0b60f14 468
leogrotti 44:eecdc0b60f14 469 positionY_agora1 = motor1->get_position();
leogrotti 44:eecdc0b60f14 470 positionX_agora1= motor2->get_position();
leogrotti 44:eecdc0b60f14 471 positionZ_agora1 = motor3->get_position();
leogrotti 44:eecdc0b60f14 472 printf( "posY_agora1 = %i \n\r", positionY_agora1);
leogrotti 44:eecdc0b60f14 473 printf( "posX_agora1 = %i \n\r", positionX_agora1);
leogrotti 44:eecdc0b60f14 474 printf( "posZ_agora1 = %i \n\r", positionZ_agora1);
leogrotti 44:eecdc0b60f14 475 }
leogrotti 43:d08a3f6b65b5 476 if (pos == 2) {
leogrotti 44:eecdc0b60f14 477 printf( "pos1Y = %i \n\r", position1Y);
leogrotti 44:eecdc0b60f14 478 printf( "pos1X = %i \n\r", position1X);
leogrotti 44:eecdc0b60f14 479 printf( "pos1Z = %i \n\r", position1Z);
leogrotti 44:eecdc0b60f14 480 positionY_agora1 = motor1->get_position();
leogrotti 44:eecdc0b60f14 481 positionX_agora1= motor2->get_position();
leogrotti 44:eecdc0b60f14 482 positionZ_agora1 = motor3->get_position();
leogrotti 44:eecdc0b60f14 483 printf( "posY_agora1 = %i \n\r", positionY_agora1);
leogrotti 44:eecdc0b60f14 484 printf( "posX_agora1 = %i \n\r", positionX_agora1);
leogrotti 44:eecdc0b60f14 485 printf( "posZ_agora1 = %i \n\r", positionZ_agora1);
leogrotti 44:eecdc0b60f14 486 dif_posX1 = position1X - positionX_agora1;
leogrotti 44:eecdc0b60f14 487 dif_posY1 = position1Y - positionY_agora1;
leogrotti 44:eecdc0b60f14 488 dif_posZ1 = position1Z - positionZ_agora1;
leogrotti 44:eecdc0b60f14 489
leogrotti 44:eecdc0b60f14 490 printf( "difY1 = %i \n\r", correcao_passo(dif_posY1));
leogrotti 44:eecdc0b60f14 491 printf( "difX1 = %i \n\r", correcao_passo(dif_posX1));
leogrotti 44:eecdc0b60f14 492 printf( "difZ1 = %i \n\r", correcao_passo(dif_posZ1));
leogrotti 44:eecdc0b60f14 493 if (dif_posX1 >0){
leogrotti 44:eecdc0b60f14 494 motor2->move(StepperMotor::FWD, correcao_passo(dif_posX1));
leogrotti 44:eecdc0b60f14 495 motor2->wait_while_active();
leogrotti 44:eecdc0b60f14 496 }
leogrotti 44:eecdc0b60f14 497 if (dif_posX1 <0){
leogrotti 44:eecdc0b60f14 498 dif_posX1 = -dif_posX1;
leogrotti 44:eecdc0b60f14 499 motor2->move(StepperMotor::BWD, correcao_passo(dif_posX1));
leogrotti 44:eecdc0b60f14 500 motor2->wait_while_active();
leogrotti 44:eecdc0b60f14 501 }
leogrotti 44:eecdc0b60f14 502 if (dif_posY1 >0){
leogrotti 44:eecdc0b60f14 503 motor1->move(StepperMotor::FWD, correcao_passo(dif_posY1));
leogrotti 43:d08a3f6b65b5 504 motor1->wait_while_active();
leogrotti 44:eecdc0b60f14 505 }
leogrotti 44:eecdc0b60f14 506 if (dif_posY1 <0){
leogrotti 44:eecdc0b60f14 507 dif_posY1 = -dif_posY1;
leogrotti 44:eecdc0b60f14 508 motor1->move(StepperMotor::BWD, correcao_passo(dif_posY1));
leogrotti 44:eecdc0b60f14 509 motor1->wait_while_active();
leogrotti 44:eecdc0b60f14 510 }
leogrotti 44:eecdc0b60f14 511 if (dif_posZ1 >0){
leogrotti 44:eecdc0b60f14 512 motor3->move(StepperMotor::FWD, correcao_passo(dif_posZ1));
leogrotti 43:d08a3f6b65b5 513 motor3->wait_while_active();
leogrotti 44:eecdc0b60f14 514 }
leogrotti 44:eecdc0b60f14 515 if (dif_posZ1 <0){
leogrotti 44:eecdc0b60f14 516 dif_posZ1 = -dif_posZ1;
leogrotti 44:eecdc0b60f14 517 motor3->move(StepperMotor::BWD, correcao_passo(dif_posZ1));
leogrotti 44:eecdc0b60f14 518 motor3->wait_while_active();
leogrotti 44:eecdc0b60f14 519 }
leogrotti 44:eecdc0b60f14 520
leogrotti 44:eecdc0b60f14 521 positionY_agora2 = motor1->get_position();
leogrotti 44:eecdc0b60f14 522 positionX_agora2= motor2->get_position();
leogrotti 44:eecdc0b60f14 523 positionZ_agora2 = motor3->get_position();
leogrotti 44:eecdc0b60f14 524 printf( "posY_agora2 = %i \n\r", positionY_agora2);
leogrotti 44:eecdc0b60f14 525 printf( "posX_agora2 = %i \n\r", positionX_agora2);
leogrotti 44:eecdc0b60f14 526 printf( "posZ_agora2 = %i \n\r", positionZ_agora2);
leogrotti 44:eecdc0b60f14 527 wait_ms(DELAY_2);
leogrotti 44:eecdc0b60f14 528 dif_posX2 = position2X - positionX_agora2;
leogrotti 44:eecdc0b60f14 529 dif_posY2 = position2Y - positionY_agora2;
leogrotti 44:eecdc0b60f14 530 dif_posZ2 = position2Z - positionZ_agora2;
leogrotti 44:eecdc0b60f14 531
leogrotti 44:eecdc0b60f14 532 if (dif_posX2 >0){
leogrotti 44:eecdc0b60f14 533 motor2->move(StepperMotor::FWD, correcao_passo(dif_posX2));
leogrotti 44:eecdc0b60f14 534 motor2->wait_while_active();
leogrotti 44:eecdc0b60f14 535 }
leogrotti 44:eecdc0b60f14 536 if (dif_posX2 <0){
leogrotti 44:eecdc0b60f14 537 dif_posX2 = -dif_posX2;
leogrotti 44:eecdc0b60f14 538 motor2->move(StepperMotor::BWD, correcao_passo(dif_posX2));
leogrotti 44:eecdc0b60f14 539 motor2->wait_while_active();
leogrotti 44:eecdc0b60f14 540 }
leogrotti 44:eecdc0b60f14 541 if (dif_posY2 >0){
leogrotti 44:eecdc0b60f14 542 motor1->move(StepperMotor::FWD, correcao_passo(dif_posY2));
leogrotti 43:d08a3f6b65b5 543 motor1->wait_while_active();
leogrotti 44:eecdc0b60f14 544 }
leogrotti 44:eecdc0b60f14 545 if (dif_posY2 <0){
leogrotti 44:eecdc0b60f14 546 dif_posY2 = -dif_posY2;
leogrotti 44:eecdc0b60f14 547 motor1->move(StepperMotor::BWD, correcao_passo(dif_posY2));
leogrotti 44:eecdc0b60f14 548 motor1->wait_while_active();
leogrotti 44:eecdc0b60f14 549 }
leogrotti 44:eecdc0b60f14 550 if (dif_posZ2 >0){
leogrotti 44:eecdc0b60f14 551 motor3->move(StepperMotor::FWD, correcao_passo(dif_posZ2));
leogrotti 43:d08a3f6b65b5 552 motor3->wait_while_active();
leogrotti 43:d08a3f6b65b5 553 }
leogrotti 44:eecdc0b60f14 554 if (dif_posZ2 <0){
leogrotti 44:eecdc0b60f14 555 dif_posZ2 = -dif_posZ2;
leogrotti 44:eecdc0b60f14 556 motor3->move(StepperMotor::BWD, correcao_passo(dif_posZ2));
leogrotti 43:d08a3f6b65b5 557 motor3->wait_while_active();
leogrotti 44:eecdc0b60f14 558 }
leogrotti 44:eecdc0b60f14 559 positionY_agora2 = motor1->get_position();
leogrotti 44:eecdc0b60f14 560 positionX_agora2= motor2->get_position();
leogrotti 44:eecdc0b60f14 561 positionZ_agora2 = motor3->get_position();
leogrotti 44:eecdc0b60f14 562 printf( "posY_agora2 = %i \n\r", positionY_agora2);
leogrotti 44:eecdc0b60f14 563 printf( "posX_agora2 = %i \n\r", positionX_agora2);
leogrotti 44:eecdc0b60f14 564 printf( "posZ_agora2 = %i \n\r", positionZ_agora2);
leogrotti 44:eecdc0b60f14 565 }
leogrotti 44:eecdc0b60f14 566
leogrotti 44:eecdc0b60f14 567 if (pos == 3) {
leogrotti 44:eecdc0b60f14 568 printf( "pos1Y = %i \n\r", position1Y);
leogrotti 44:eecdc0b60f14 569 printf( "pos1X = %i \n\r", position1X);
leogrotti 44:eecdc0b60f14 570 printf( "pos1Z = %i \n\r", position1Z);
leogrotti 44:eecdc0b60f14 571 positionY_agora1 = motor1->get_position();
leogrotti 44:eecdc0b60f14 572 positionX_agora1= motor2->get_position();
leogrotti 44:eecdc0b60f14 573 positionZ_agora1 = motor3->get_position();
leogrotti 44:eecdc0b60f14 574 printf( "posY_agora1 = %i \n\r", positionY_agora1);
leogrotti 44:eecdc0b60f14 575 printf( "posX_agora1 = %i \n\r", positionX_agora1);
leogrotti 44:eecdc0b60f14 576 printf( "posZ_agora1 = %i \n\r", positionZ_agora1);
leogrotti 44:eecdc0b60f14 577 dif_posX1 = position1X - positionX_agora1;
leogrotti 44:eecdc0b60f14 578 dif_posY1 = position1Y - positionY_agora1;
leogrotti 44:eecdc0b60f14 579 dif_posZ1 = position1Z - positionZ_agora1;
leogrotti 44:eecdc0b60f14 580
leogrotti 44:eecdc0b60f14 581 printf( "difY1 = %i \n\r", correcao_passo(dif_posY1));
leogrotti 44:eecdc0b60f14 582 printf( "difX1 = %i \n\r", correcao_passo(dif_posX1));
leogrotti 44:eecdc0b60f14 583 printf( "difZ1 = %i \n\r", correcao_passo(dif_posZ1));
leogrotti 44:eecdc0b60f14 584 if (dif_posX1 >0){
leogrotti 44:eecdc0b60f14 585 motor2->move(StepperMotor::FWD, correcao_passo(dif_posX1));
leogrotti 44:eecdc0b60f14 586 motor2->wait_while_active();
leogrotti 44:eecdc0b60f14 587 }
leogrotti 44:eecdc0b60f14 588 if (dif_posX1 <0){
leogrotti 44:eecdc0b60f14 589 dif_posX1 = -dif_posX1;
leogrotti 44:eecdc0b60f14 590 motor2->move(StepperMotor::BWD, correcao_passo(dif_posX1));
leogrotti 44:eecdc0b60f14 591 motor2->wait_while_active();
leogrotti 44:eecdc0b60f14 592 }
leogrotti 44:eecdc0b60f14 593 if (dif_posY1 >0){
leogrotti 44:eecdc0b60f14 594 motor1->move(StepperMotor::FWD, correcao_passo(dif_posY1));
leogrotti 43:d08a3f6b65b5 595 motor1->wait_while_active();
leogrotti 44:eecdc0b60f14 596 }
leogrotti 44:eecdc0b60f14 597 if (dif_posY1 <0){
leogrotti 44:eecdc0b60f14 598 dif_posY1 = -dif_posY1;
leogrotti 44:eecdc0b60f14 599 motor1->move(StepperMotor::BWD, correcao_passo(dif_posY1));
leogrotti 44:eecdc0b60f14 600 motor1->wait_while_active();
leogrotti 44:eecdc0b60f14 601 }
leogrotti 44:eecdc0b60f14 602 if (dif_posZ1 >0){
leogrotti 44:eecdc0b60f14 603 motor3->move(StepperMotor::FWD, correcao_passo(dif_posZ1));
leogrotti 43:d08a3f6b65b5 604 motor3->wait_while_active();
leogrotti 44:eecdc0b60f14 605 }
leogrotti 44:eecdc0b60f14 606 if (dif_posZ1 <0){
leogrotti 44:eecdc0b60f14 607 dif_posZ1 = -dif_posZ1;
leogrotti 44:eecdc0b60f14 608 motor3->move(StepperMotor::BWD, correcao_passo(dif_posZ1));
leogrotti 43:d08a3f6b65b5 609 motor3->wait_while_active();
leogrotti 43:d08a3f6b65b5 610 }
leogrotti 44:eecdc0b60f14 611
leogrotti 44:eecdc0b60f14 612 positionY_agora2 = motor1->get_position();
leogrotti 44:eecdc0b60f14 613 positionX_agora2= motor2->get_position();
leogrotti 44:eecdc0b60f14 614 positionZ_agora2 = motor3->get_position();
leogrotti 44:eecdc0b60f14 615 printf( "posY_agora2 = %i \n\r", positionY_agora2);
leogrotti 44:eecdc0b60f14 616 printf( "posX_agora2 = %i \n\r", positionX_agora2);
leogrotti 44:eecdc0b60f14 617 printf( "posZ_agora2 = %i \n\r", positionZ_agora2);
leogrotti 44:eecdc0b60f14 618 wait_ms(DELAY_2);
leogrotti 44:eecdc0b60f14 619 dif_posX2 = position2X - positionX_agora2;
leogrotti 44:eecdc0b60f14 620 dif_posY2 = position2Y - positionY_agora2;
leogrotti 44:eecdc0b60f14 621 dif_posZ2 = position2Z - positionZ_agora2;
leogrotti 44:eecdc0b60f14 622
leogrotti 44:eecdc0b60f14 623 if (dif_posX2 >0){
leogrotti 44:eecdc0b60f14 624 motor2->move(StepperMotor::FWD, correcao_passo(dif_posX2));
leogrotti 44:eecdc0b60f14 625 motor2->wait_while_active();
leogrotti 43:d08a3f6b65b5 626 }
leogrotti 44:eecdc0b60f14 627 if (dif_posX2 <0){
leogrotti 44:eecdc0b60f14 628 dif_posX2 = -dif_posX2;
leogrotti 44:eecdc0b60f14 629 motor2->move(StepperMotor::BWD, correcao_passo(dif_posX2));
leogrotti 44:eecdc0b60f14 630 motor2->wait_while_active();
leogrotti 44:eecdc0b60f14 631 }
leogrotti 44:eecdc0b60f14 632 if (dif_posY2 >0){
leogrotti 44:eecdc0b60f14 633 motor1->move(StepperMotor::FWD, correcao_passo(dif_posY2));
leogrotti 44:eecdc0b60f14 634 motor1->wait_while_active();
leogrotti 44:eecdc0b60f14 635 }
leogrotti 44:eecdc0b60f14 636 if (dif_posY2 <0){
leogrotti 44:eecdc0b60f14 637 dif_posY2 = -dif_posY2;
leogrotti 44:eecdc0b60f14 638 motor1->move(StepperMotor::BWD, correcao_passo(dif_posY2));
leogrotti 44:eecdc0b60f14 639 motor1->wait_while_active();
leogrotti 44:eecdc0b60f14 640 }
leogrotti 44:eecdc0b60f14 641 if (dif_posZ2 >0){
leogrotti 44:eecdc0b60f14 642 motor3->move(StepperMotor::FWD, correcao_passo(dif_posZ2));
leogrotti 44:eecdc0b60f14 643 motor3->wait_while_active();
leogrotti 44:eecdc0b60f14 644 }
leogrotti 44:eecdc0b60f14 645 if (dif_posZ2 <0){
leogrotti 44:eecdc0b60f14 646 dif_posZ2 = -dif_posZ2;
leogrotti 44:eecdc0b60f14 647 motor3->move(StepperMotor::BWD, correcao_passo(dif_posZ2));
leogrotti 44:eecdc0b60f14 648 motor3->wait_while_active();
leogrotti 44:eecdc0b60f14 649 }
leogrotti 44:eecdc0b60f14 650 positionY_agora2 = motor1->get_position();
leogrotti 44:eecdc0b60f14 651 positionX_agora2= motor2->get_position();
leogrotti 44:eecdc0b60f14 652 positionZ_agora2 = motor3->get_position();
leogrotti 44:eecdc0b60f14 653 printf( "posY_agora2 = %i \n\r", positionY_agora2);
leogrotti 44:eecdc0b60f14 654 printf( "posX_agora2 = %i \n\r", positionX_agora2);
leogrotti 44:eecdc0b60f14 655 printf( "posZ_agora2 = %i \n\r", positionZ_agora2);
leogrotti 44:eecdc0b60f14 656
leogrotti 44:eecdc0b60f14 657
leogrotti 44:eecdc0b60f14 658 positionY_agora3 = motor1->get_position();
leogrotti 44:eecdc0b60f14 659 positionX_agora3= motor2->get_position();
leogrotti 44:eecdc0b60f14 660 positionZ_agora3 = motor3->get_position();
leogrotti 44:eecdc0b60f14 661 printf( "posY_agora3 = %i \n\r", positionY_agora3);
leogrotti 44:eecdc0b60f14 662 printf( "posX_agora3 = %i \n\r", positionX_agora3);
leogrotti 44:eecdc0b60f14 663 printf( "posZ_agora3 = %i \n\r", positionZ_agora3);
leogrotti 44:eecdc0b60f14 664 wait_ms(DELAY_2);
leogrotti 44:eecdc0b60f14 665 dif_posX3 = position3X - positionX_agora3;
leogrotti 44:eecdc0b60f14 666 dif_posY3 = position3Y - positionY_agora3;
leogrotti 44:eecdc0b60f14 667 dif_posZ3 = position3Z - positionZ_agora3;
leogrotti 44:eecdc0b60f14 668
leogrotti 44:eecdc0b60f14 669 if (dif_posX3 >0){
leogrotti 44:eecdc0b60f14 670 motor2->move(StepperMotor::FWD, correcao_passo(dif_posX3));
leogrotti 44:eecdc0b60f14 671 motor2->wait_while_active();
leogrotti 44:eecdc0b60f14 672 }
leogrotti 44:eecdc0b60f14 673 if (dif_posX3 <0){
leogrotti 44:eecdc0b60f14 674 dif_posX3 = -dif_posX3;
leogrotti 44:eecdc0b60f14 675 motor2->move(StepperMotor::BWD, correcao_passo(dif_posX3));
leogrotti 44:eecdc0b60f14 676 motor2->wait_while_active();
leogrotti 44:eecdc0b60f14 677 }
leogrotti 44:eecdc0b60f14 678 if (dif_posY3 >0){
leogrotti 44:eecdc0b60f14 679 motor1->move(StepperMotor::FWD, correcao_passo(dif_posY3));
leogrotti 44:eecdc0b60f14 680 motor1->wait_while_active();
leogrotti 44:eecdc0b60f14 681 }
leogrotti 44:eecdc0b60f14 682 if (dif_posY3 <0){
leogrotti 44:eecdc0b60f14 683 dif_posY3 = -dif_posY3;
leogrotti 44:eecdc0b60f14 684 motor1->move(StepperMotor::BWD, correcao_passo(dif_posY3));
leogrotti 44:eecdc0b60f14 685 motor1->wait_while_active();
leogrotti 44:eecdc0b60f14 686 }
leogrotti 44:eecdc0b60f14 687 if (dif_posZ3 >0){
leogrotti 44:eecdc0b60f14 688 motor3->move(StepperMotor::FWD, correcao_passo(dif_posZ3));
leogrotti 44:eecdc0b60f14 689 motor3->wait_while_active();
leogrotti 44:eecdc0b60f14 690 }
leogrotti 44:eecdc0b60f14 691 if (dif_posZ3 <0){
leogrotti 44:eecdc0b60f14 692 dif_posZ3 = -dif_posZ3;
leogrotti 44:eecdc0b60f14 693 motor3->move(StepperMotor::BWD, correcao_passo(dif_posZ3));
leogrotti 44:eecdc0b60f14 694 motor3->wait_while_active();
leogrotti 44:eecdc0b60f14 695 }
leogrotti 44:eecdc0b60f14 696 positionY_agora3 = motor1->get_position();
leogrotti 44:eecdc0b60f14 697 positionX_agora3= motor2->get_position();
leogrotti 44:eecdc0b60f14 698 positionZ_agora3 = motor3->get_position();
leogrotti 44:eecdc0b60f14 699 printf( "posY_agora3 = %i \n\r", positionY_agora3);
leogrotti 44:eecdc0b60f14 700 printf( "posX_agora3 = %i \n\r", positionX_agora3);
leogrotti 44:eecdc0b60f14 701 printf( "posZ_agora3 = %i \n\r", positionZ_agora3);
leogrotti 44:eecdc0b60f14 702 }
leogrotti 44:eecdc0b60f14 703 }
leogrotti 44:eecdc0b60f14 704 }
leogrotti 43:d08a3f6b65b5 705
leogrotti 43:d08a3f6b65b5 706
leogrotti 43:d08a3f6b65b5 707
Davidroid 0:e6a49a092e2a 708 }
leogrotti 44:eecdc0b60f14 709
leogrotti 44:eecdc0b60f14 710 //}