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@43:d08a3f6b65b5, 2018-05-15 (annotated)

Committer:
leogrotti
Date:
Tue May 15 18:47:13 2018 +0000
Revision:
43:d08a3f6b65b5
Parent:
42:a04bff02f231
Child:
44:eecdc0b60f14
tentativa go_to;

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 36:0cd4fdbb40af 112
leogrotti 42:a04bff02f231 113 bool flagX1=0;
leogrotti 42:a04bff02f231 114 bool flagY1=0;
leogrotti 35:a05bef8dd995 115
leogrotti 35:a05bef8dd995 116
leogrotti 34:098efd69c86d 117 unsigned int minspeed = 1300;
Brunoporto2702 32:481674c0f90d 118
leogrotti 34:098efd69c86d 119 //int step = 0x08;
Brunoporto2702 32:481674c0f90d 120
leogrotti 35:a05bef8dd995 121 unsigned int speed1;
leogrotti 35:a05bef8dd995 122 unsigned int speed2;
leogrotti 39:7e30bcc989d3 123 unsigned int speed3;
leogrotti 36:0cd4fdbb40af 124
leogrotti 38:c3d77ff8168a 125 //unsigned int pos3;
Brunoporto2702 32:481674c0f90d 126
Davidroid 0:e6a49a092e2a 127
Davidroid 0:e6a49a092e2a 128 /* Motor Control Component. */
Davidroid 3:02d9ec4f88b2 129 L6474 *motor1;
Davidroid 3:02d9ec4f88b2 130 L6474 *motor2;
leogrotti 39:7e30bcc989d3 131 L6474 *motor3;
Davidroid 0:e6a49a092e2a 132
Davidroid 0:e6a49a092e2a 133
Davidroid 0:e6a49a092e2a 134 /* Main ----------------------------------------------------------------------*/
leogrotti 42:a04bff02f231 135 /*
leogrotti 41:7f91e949ca88 136 void released_1()
leogrotti 41:7f91e949ca88 137 {
leogrotti 41:7f91e949ca88 138 motor3->hard_stop();
leogrotti 41:7f91e949ca88 139 printf ("parou \r\n");
leogrotti 41:7f91e949ca88 140
leogrotti 41:7f91e949ca88 141 }
leogrotti 41:7f91e949ca88 142
leogrotti 41:7f91e949ca88 143 void released_2()
leogrotti 41:7f91e949ca88 144 {
leogrotti 41:7f91e949ca88 145 motor3->hard_stop();
leogrotti 41:7f91e949ca88 146 printf ("parou \r\n");
leogrotti 41:7f91e949ca88 147
leogrotti 41:7f91e949ca88 148 }
leogrotti 41:7f91e949ca88 149
leogrotti 42:a04bff02f231 150 void paraX1() {
leogrotti 42:a04bff02f231 151 flagX1 = 1;
leogrotti 42:a04bff02f231 152 motor2->hard_stop();
leogrotti 42:a04bff02f231 153
leogrotti 42:a04bff02f231 154 }
leogrotti 42:a04bff02f231 155
leogrotti 42:a04bff02f231 156 void continuaX1() {
leogrotti 42:a04bff02f231 157 flagX1 = 0;
leogrotti 42:a04bff02f231 158 }
leogrotti 42:a04bff02f231 159
leogrotti 42:a04bff02f231 160
leogrotti 42:a04bff02f231 161 void paraY1() {
leogrotti 42:a04bff02f231 162 flagY1 = 1;
leogrotti 42:a04bff02f231 163 motor1->hard_stop();
leogrotti 42:a04bff02f231 164
leogrotti 42:a04bff02f231 165 }
leogrotti 42:a04bff02f231 166
leogrotti 42:a04bff02f231 167 void continuaY1() {
leogrotti 42:a04bff02f231 168 flagY1 = 0;
leogrotti 42:a04bff02f231 169 }
leogrotti 42:a04bff02f231 170 */
leogrotti 42:a04bff02f231 171
Davidroid 0:e6a49a092e2a 172 int main()
Davidroid 0:e6a49a092e2a 173 {
Davidroid 8:cec4c2c03a27 174 /*----- Initialization. -----*/
Davidroid 8:cec4c2c03a27 175
Davidroid 0:e6a49a092e2a 176 /* Initializing SPI bus. */
Davidroid 0:e6a49a092e2a 177 DevSPI dev_spi(D11, D12, D13);
Davidroid 0:e6a49a092e2a 178
Davidroid 9:a9e51320aee4 179 /* Initializing Motor Control Components. */
Davidroid 5:a0268a435bb1 180 motor1 = new L6474(D2, D8, D7, D9, D10, dev_spi);
Davidroid 16:810667a9f31f 181 motor2 = new L6474(D2, D8, D4, D3, D10, dev_spi);
leogrotti 39:7e30bcc989d3 182 motor3 = new L6474(D2, D8, D5, D6, D10, dev_spi);
Davidroid 29:526970c1d998 183 if (motor1->init() != COMPONENT_OK) {
Davidroid 14:fcd452b03d1a 184 exit(EXIT_FAILURE);
Davidroid 28:3f17a4152bcf 185 }
Davidroid 29:526970c1d998 186 if (motor2->init() != COMPONENT_OK) {
Davidroid 14:fcd452b03d1a 187 exit(EXIT_FAILURE);
Davidroid 28:3f17a4152bcf 188 }
leogrotti 39:7e30bcc989d3 189 if (motor3->init() != COMPONENT_OK) {
leogrotti 39:7e30bcc989d3 190 exit(EXIT_FAILURE);
leogrotti 39:7e30bcc989d3 191 }
Davidroid 0:e6a49a092e2a 192
leogrotti 35:a05bef8dd995 193
Davidroid 8:cec4c2c03a27 194
Davidroid 8:cec4c2c03a27 195
leogrotti 39:7e30bcc989d3 196
leogrotti 42:a04bff02f231 197 /* Attaching and enabling interrupt handlers.
leogrotti 42:a04bff02f231 198 motor3->attach_flag_irq(&flag_irq_handler3);
leogrotti 40:0b517b49f70d 199 motor3->enable_flag_irq();
leogrotti 42:a04bff02f231 200 motor1->attach_flag_irq(&flag_irq_handler1);
leogrotti 42:a04bff02f231 201 motor1->enable_flag_irq();
leogrotti 42:a04bff02f231 202 motor2->attach_flag_irq(&flag_irq_handler2);
leogrotti 42:a04bff02f231 203 motor2->enable_flag_irq();
leogrotti 42:a04bff02f231 204 */
leogrotti 41:7f91e949ca88 205
leogrotti 42:a04bff02f231 206 //sensorX1.rise(&paraX1);
leogrotti 42:a04bff02f231 207 // sensorX1.fall(&continuaX1);
leogrotti 39:7e30bcc989d3 208
leogrotti 41:7f91e949ca88 209
leogrotti 42:a04bff02f231 210 //sensorY1.rise(&paraY1);
leogrotti 42:a04bff02f231 211 //sensorY1.fall(&continuaY1);
leogrotti 42:a04bff02f231 212
leogrotti 43:d08a3f6b65b5 213 a = 0;
leogrotti 43:d08a3f6b65b5 214 pos = 0;
leogrotti 43:d08a3f6b65b5 215
leogrotti 43:d08a3f6b65b5 216
leogrotti 42:a04bff02f231 217
Davidroid 28:3f17a4152bcf 218 while(true) {
leogrotti 42:a04bff02f231 219 count = count +1;
leogrotti 42:a04bff02f231 220 // Leitura analógica
Brunoporto2702 32:481674c0f90d 221 u = eixo_X.read();
Brunoporto2702 32:481674c0f90d 222 w = eixo_Y.read();
leogrotti 42:a04bff02f231 223 zu = eixo_ZU.read();
leogrotti 42:a04bff02f231 224 zd = eixo_ZD.read();
leogrotti 43:d08a3f6b65b5 225 sp = salva_pos.read();
leogrotti 43:d08a3f6b65b5 226 vc = vai_cami.read();
leogrotti 43:d08a3f6b65b5 227 if (count == 9999999999999999) {
leogrotti 43:d08a3f6b65b5 228 printf("zu =%f \r\n\n", zu);
leogrotti 43:d08a3f6b65b5 229 printf("zd =%f \r\n\n", zd);
leogrotti 43:d08a3f6b65b5 230 printf("sp =%f \r\n\n", sp);
leogrotti 43:d08a3f6b65b5 231 printf("vc =%f \r\n\n", vc);
leogrotti 43:d08a3f6b65b5 232 count = 0;
leogrotti 43:d08a3f6b65b5 233 }
leogrotti 43:d08a3f6b65b5 234
leogrotti 42:a04bff02f231 235
leogrotti 42:a04bff02f231 236 // Movimentando eixo Z cima caso botão 1 apertado
leogrotti 42:a04bff02f231 237 if (zu >0.7) {
leogrotti 42:a04bff02f231 238 motor3->run(StepperMotor::FWD);
leogrotti 42:a04bff02f231 239 speed3 = motor3->get_speed();
leogrotti 42:a04bff02f231 240 }
leogrotti 34:098efd69c86d 241
leogrotti 42:a04bff02f231 242 // Parando eixo Z caso botão 1 e 2 liberado
leogrotti 42:a04bff02f231 243 if (zu< 0.7 && zd < 0.7){
leogrotti 42:a04bff02f231 244 motor3->hard_stop();
leogrotti 42:a04bff02f231 245 speed3 = 0;
leogrotti 42:a04bff02f231 246 }
leogrotti 42:a04bff02f231 247
leogrotti 42:a04bff02f231 248 // Movimentando eixo Z baixo caso botão 2 apertado
leogrotti 42:a04bff02f231 249 if (zd >0.7) {
leogrotti 42:a04bff02f231 250 motor3->run(StepperMotor::BWD);
leogrotti 42:a04bff02f231 251 speed3 = motor3->get_speed();
leogrotti 42:a04bff02f231 252 }
leogrotti 34:098efd69c86d 253
leogrotti 42:a04bff02f231 254 // Parando eixo Z caso botão 1 e 2 liberado
leogrotti 42:a04bff02f231 255 if (zu< 0.7 && zd < 0.7){
leogrotti 42:a04bff02f231 256 motor3->hard_stop();
leogrotti 42:a04bff02f231 257 speed3 = 0;
leogrotti 42:a04bff02f231 258 }
leogrotti 42:a04bff02f231 259
leogrotti 42:a04bff02f231 260
leogrotti 42:a04bff02f231 261 // Movimentando eixo Y duas direções com joystick
leogrotti 42:a04bff02f231 262 //if (flagY1 == 0 ) {
leogrotti 42:a04bff02f231 263
leogrotti 42:a04bff02f231 264 // Movimentando eixo Y fwd
leogrotti 42:a04bff02f231 265 if (u>0.820) {
leogrotti 42:a04bff02f231 266 motor1->run(StepperMotor::FWD);
leogrotti 42:a04bff02f231 267 speed1 = motor1->get_speed();
leogrotti 42:a04bff02f231 268 }
leogrotti 42:a04bff02f231 269 // Movimentando eixo Y bwd
leogrotti 42:a04bff02f231 270 else{ if(u<0.65) {
leogrotti 42:a04bff02f231 271 motor1->run(StepperMotor::BWD);
leogrotti 43:d08a3f6b65b5 272 speed1 = motor1->get_speed();
Brunoporto2702 32:481674c0f90d 273 }
leogrotti 42:a04bff02f231 274 // parando eixo Y
leogrotti 42:a04bff02f231 275 else
leogrotti 42:a04bff02f231 276 motor1->hard_stop();
leogrotti 35:a05bef8dd995 277 speed1 = 0;
leogrotti 42:a04bff02f231 278 //}
leogrotti 42:a04bff02f231 279 }
leogrotti 42:a04bff02f231 280
leogrotti 42:a04bff02f231 281 // Movimentando eixo X duas direções com joystick
leogrotti 42:a04bff02f231 282 //if (flagX1 == 0) {
leogrotti 43:d08a3f6b65b5 283 // Movimentando eixo X fwd
leogrotti 42:a04bff02f231 284 if (w>0.80) {
leogrotti 34:098efd69c86d 285 motor2->run(StepperMotor::BWD);
leogrotti 42:a04bff02f231 286 speed2 = motor2->get_speed();
leogrotti 43:d08a3f6b65b5 287 wait_ms(DELAY_1);
leogrotti 42:a04bff02f231 288 }
leogrotti 43:d08a3f6b65b5 289 // Movimentando eixo X Bwd
leogrotti 42:a04bff02f231 290 else{ if(w<0.60) {
leogrotti 34:098efd69c86d 291 motor2->run(StepperMotor::FWD);
leogrotti 42:a04bff02f231 292 speed2 = motor2->get_speed();
leogrotti 43:d08a3f6b65b5 293 wait_ms(DELAY_1);
leogrotti 42:a04bff02f231 294 }
leogrotti 42:a04bff02f231 295 // parando eixo X
leogrotti 42:a04bff02f231 296 else
Brunoporto2702 32:481674c0f90d 297 motor2->hard_stop();
leogrotti 43:d08a3f6b65b5 298 speed2 = motor2->get_speed();
leogrotti 43:d08a3f6b65b5 299 wait_ms(DELAY_1);
leogrotti 43:d08a3f6b65b5 300
Brunoporto2702 32:481674c0f90d 301 }
leogrotti 43:d08a3f6b65b5 302 /* if (a==0);{
leogrotti 43:d08a3f6b65b5 303 motor2->move(StepperMotor::BWD, 2000);
leogrotti 43:d08a3f6b65b5 304 motor2->wait_while_active();
leogrotti 43:d08a3f6b65b5 305 a= 1;
leogrotti 43:d08a3f6b65b5 306 }
leogrotti 43:d08a3f6b65b5 307 if (a==1);{
leogrotti 43:d08a3f6b65b5 308 motor2->move(StepperMotor::FWD, 2000);
leogrotti 43:d08a3f6b65b5 309 motor2->wait_while_active();
leogrotti 43:d08a3f6b65b5 310 a= 0;
leogrotti 43:d08a3f6b65b5 311 }
leogrotti 43:d08a3f6b65b5 312 */
leogrotti 43:d08a3f6b65b5 313
leogrotti 43:d08a3f6b65b5 314 if (salva_pos) {
leogrotti 43:d08a3f6b65b5 315 if (pos==0) {
leogrotti 43:d08a3f6b65b5 316 int position1Y = motor1->get_position();
leogrotti 43:d08a3f6b65b5 317 int position1X = motor2->get_position();
leogrotti 43:d08a3f6b65b5 318 int position1Z = motor3->get_position();
leogrotti 43:d08a3f6b65b5 319
leogrotti 43:d08a3f6b65b5 320 printf( "pos1Y = %i \n\r", position1Y);
leogrotti 43:d08a3f6b65b5 321 printf( "pos1X = %i \n\r", position1X);
leogrotti 43:d08a3f6b65b5 322 printf( "pos1Z = %i \n\r", position1Z);
leogrotti 43:d08a3f6b65b5 323
leogrotti 43:d08a3f6b65b5 324 pos = 1;
leogrotti 43:d08a3f6b65b5 325 wait_ms(DELAY_2);
leogrotti 43:d08a3f6b65b5 326 }
leogrotti 43:d08a3f6b65b5 327 }
leogrotti 43:d08a3f6b65b5 328
leogrotti 43:d08a3f6b65b5 329
leogrotti 43:d08a3f6b65b5 330 if (salva_pos) {
leogrotti 43:d08a3f6b65b5 331 if (pos==1) {
leogrotti 43:d08a3f6b65b5 332 int position2Y = motor1->get_position();
leogrotti 43:d08a3f6b65b5 333 int position2X = motor2->get_position();
leogrotti 43:d08a3f6b65b5 334 int position2Z = motor3->get_position();
leogrotti 43:d08a3f6b65b5 335
leogrotti 43:d08a3f6b65b5 336 printf( "pos2Y = %i \n\r", position2Y);
leogrotti 43:d08a3f6b65b5 337 printf( "pos2X = %i \n\r", position2X);
leogrotti 43:d08a3f6b65b5 338 printf( "pos2Z = %i \n\r", position2Z);
leogrotti 43:d08a3f6b65b5 339
leogrotti 43:d08a3f6b65b5 340 pos = 2;
leogrotti 43:d08a3f6b65b5 341 wait_ms(DELAY_2);
leogrotti 43:d08a3f6b65b5 342 }
leogrotti 43:d08a3f6b65b5 343 }
leogrotti 43:d08a3f6b65b5 344
leogrotti 43:d08a3f6b65b5 345 if (salva_pos) {
leogrotti 43:d08a3f6b65b5 346 if (pos==2) {
leogrotti 43:d08a3f6b65b5 347 int position3Y = motor1->get_position();
leogrotti 43:d08a3f6b65b5 348 int position3X = motor2->get_position();
leogrotti 43:d08a3f6b65b5 349 int position3Z = motor3->get_position();
leogrotti 43:d08a3f6b65b5 350
leogrotti 43:d08a3f6b65b5 351 printf( "pos3Y = %i \n\r", position3Y);
leogrotti 43:d08a3f6b65b5 352 printf( "pos3X = %i \n\r", position3X);
leogrotti 43:d08a3f6b65b5 353 printf( "pos3Z = %i \n\r", position3Z);
leogrotti 43:d08a3f6b65b5 354
leogrotti 43:d08a3f6b65b5 355 pos = 3;
leogrotti 43:d08a3f6b65b5 356 wait_ms(DELAY_2);
leogrotti 43:d08a3f6b65b5 357 }
leogrotti 43:d08a3f6b65b5 358 }
leogrotti 43:d08a3f6b65b5 359
leogrotti 43:d08a3f6b65b5 360 if (vai_cami) {
leogrotti 43:d08a3f6b65b5 361 if (pos ==0){
leogrotti 43:d08a3f6b65b5 362 printf("sem posicoes salvas");
leogrotti 43:d08a3f6b65b5 363 wait_ms(DELAY_2);
leogrotti 43:d08a3f6b65b5 364 }
leogrotti 43:d08a3f6b65b5 365 if (pos == 1) {
leogrotti 43:d08a3f6b65b5 366 motor1->go_to(position1Y);
leogrotti 43:d08a3f6b65b5 367 motor2->go_to(position1X);
leogrotti 43:d08a3f6b65b5 368 motor3->go_to(position1Z);
leogrotti 43:d08a3f6b65b5 369 motor1->wait_while_active();
leogrotti 43:d08a3f6b65b5 370 motor2->wait_while_active();
leogrotti 43:d08a3f6b65b5 371 motor3->wait_while_active();
leogrotti 43:d08a3f6b65b5 372 }
leogrotti 43:d08a3f6b65b5 373 if (pos == 2) {
leogrotti 43:d08a3f6b65b5 374 motor1->go_to(position1Y);
leogrotti 43:d08a3f6b65b5 375 motor2->go_to(position1X);
leogrotti 43:d08a3f6b65b5 376 motor3->go_to(position1Z);
leogrotti 43:d08a3f6b65b5 377 motor1->wait_while_active();
leogrotti 43:d08a3f6b65b5 378 motor2->wait_while_active();
leogrotti 43:d08a3f6b65b5 379 motor3->wait_while_active();
leogrotti 43:d08a3f6b65b5 380 wait_ms(DELAY_3);
leogrotti 43:d08a3f6b65b5 381 motor1->go_to(position2Y);
leogrotti 43:d08a3f6b65b5 382 motor2->go_to(position2X);
leogrotti 43:d08a3f6b65b5 383 motor3->go_to(position2Z);
leogrotti 43:d08a3f6b65b5 384 motor1->wait_while_active();
leogrotti 43:d08a3f6b65b5 385 motor2->wait_while_active();
leogrotti 43:d08a3f6b65b5 386 motor3->wait_while_active();
leogrotti 43:d08a3f6b65b5 387 }
leogrotti 43:d08a3f6b65b5 388 if (pos == 3) {
leogrotti 43:d08a3f6b65b5 389 motor1->go_to(position1Y);
leogrotti 43:d08a3f6b65b5 390 motor2->go_to(position1X);
leogrotti 43:d08a3f6b65b5 391 motor3->go_to(position1Z);
leogrotti 43:d08a3f6b65b5 392 motor1->wait_while_active();
leogrotti 43:d08a3f6b65b5 393 motor2->wait_while_active();
leogrotti 43:d08a3f6b65b5 394 motor3->wait_while_active();
leogrotti 43:d08a3f6b65b5 395 wait_ms(DELAY_3);
leogrotti 43:d08a3f6b65b5 396 motor1->go_to(position2Y);
leogrotti 43:d08a3f6b65b5 397 motor2->go_to(position2X);
leogrotti 43:d08a3f6b65b5 398 motor3->go_to(position2Z);
leogrotti 43:d08a3f6b65b5 399 motor1->wait_while_active();
leogrotti 43:d08a3f6b65b5 400 motor2->wait_while_active();
leogrotti 43:d08a3f6b65b5 401 motor3->wait_while_active();
leogrotti 43:d08a3f6b65b5 402 wait_ms(DELAY_3);
leogrotti 43:d08a3f6b65b5 403 motor1->go_to(position3Y);
leogrotti 43:d08a3f6b65b5 404 motor2->go_to(position3X);
leogrotti 43:d08a3f6b65b5 405 motor3->go_to(position3Z);
leogrotti 43:d08a3f6b65b5 406 motor1->wait_while_active();
leogrotti 43:d08a3f6b65b5 407 motor2->wait_while_active();
leogrotti 43:d08a3f6b65b5 408 motor3->wait_while_active();
leogrotti 43:d08a3f6b65b5 409 }
leogrotti 43:d08a3f6b65b5 410 }
leogrotti 43:d08a3f6b65b5 411
leogrotti 43:d08a3f6b65b5 412
leogrotti 43:d08a3f6b65b5 413
Davidroid 0:e6a49a092e2a 414 }
Brunoporto2702 32:481674c0f90d 415 }