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@61:b7ef7d06c57b, 2018-05-29 (annotated)

Committer:
digo1234
Date:
Tue May 29 19:08:55 2018 +0000
Revision:
61:b7ef7d06c57b
Parent:
60:9f4d34edef70 
tudo good menos calibra??o + deletado codigo antigo;

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
digo1234 57:270b6c82d071 6 * @date October 14th, 1025
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 *
digo1234 57:270b6c82d071 12 * <h2><center>&copy; COPYRIGHT(c) 1025 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
leogrotti 46:63b15a56cbe5 51 #include <list>
leogrotti 46:63b15a56cbe5 52
leogrotti 53:7c56b06e694e 53 /* includes para os lcds */
leogrotti 53:7c56b06e694e 54 #include "TextLCD.h"
leogrotti 53:7c56b06e694e 55 /* definindo apertado e solto para lcd */
digo1234 56:5aedf3b4e184 56 #define apertado 1
digo1234 56:5aedf3b4e184 57 #define solto 0
leogrotti 53:7c56b06e694e 58
Davidroid 0:e6a49a092e2a 59
Davidroid 0:e6a49a092e2a 60 /* Definitions ---------------------------------------------------------------*/
Davidroid 0:e6a49a092e2a 61
Davidroid 24:8cb3c4ad055f 62 /* Number of steps. */
digo1234 57:270b6c82d071 63 #define STEPS 3101
Davidroid 0:e6a49a092e2a 64
Davidroid 24:8cb3c4ad055f 65 /* Delay in milliseconds. */
leogrotti 42:a04bff02f231 66 #define DELAY_1 20
digo1234 57:270b6c82d071 67 #define DELAY_2 101
digo1234 57:270b6c82d071 68 #define DELAY_3 1010
Davidroid 24:8cb3c4ad055f 69
Davidroid 24:8cb3c4ad055f 70 /* Speed in pps (Pulses Per Second).
Davidroid 24:8cb3c4ad055f 71 In Full Step mode: 1 pps = 1 step/s).
Davidroid 24:8cb3c4ad055f 72 In 1/N Step Mode: N pps = 1 step/s). */
leogrotti 34:098efd69c86d 73 #define SPEED_1 1300
leogrotti 35:a05bef8dd995 74 #define SPEED_2 1300
Davidroid 24:8cb3c4ad055f 75
Davidroid 0:e6a49a092e2a 76
Brunoporto2702 32:481674c0f90d 77
Davidroid 0:e6a49a092e2a 78 /* Variables -----------------------------------------------------------------*/
leogrotti 40:0b517b49f70d 79
leogrotti 50:58a2df31077a 80 DigitalIn fim_curso_X (PD_2);
leogrotti 50:58a2df31077a 81 DigitalIn fim_curso_Y (PC_11);
leogrotti 53:7c56b06e694e 82 DigitalIn fim_curso_Z (PC_8);
leogrotti 42:a04bff02f231 83
digo1234 59:8d34a4bfc2b7 84 DigitalOut solenoide (PB_15);
Brunoporto2702 32:481674c0f90d 85
Brunoporto2702 32:481674c0f90d 86 Serial pc(USBTX, USBRX);
Brunoporto2702 32:481674c0f90d 87
leogrotti 53:7c56b06e694e 88
leogrotti 53:7c56b06e694e 89 /* LCD */
leogrotti 53:7c56b06e694e 90 I2C i2c_lcd(D14,D15); // sda, scl
leogrotti 53:7c56b06e694e 91
leogrotti 53:7c56b06e694e 92 TextLCD_I2C lcd(&i2c_lcd, 0x7E, TextLCD::LCD20x4); // I2C exp: I2C bus, PCF8574 Slaveaddress, LCD Type
leogrotti 53:7c56b06e694e 93
leogrotti 53:7c56b06e694e 94 int menu=0;
digo1234 60:9f4d34edef70 95 int menu_memoria=0;
digo1234 60:9f4d34edef70 96 //float xmenu=1.0, ymenu=2.0, zmenu=3.0;
leogrotti 53:7c56b06e694e 97 int inicial= 1;
leogrotti 53:7c56b06e694e 98 int pisca =0;
leogrotti 53:7c56b06e694e 99 float i=0.0;
leogrotti 53:7c56b06e694e 100
leogrotti 53:7c56b06e694e 101 /* FIM LCD */
leogrotti 53:7c56b06e694e 102
leogrotti 42:a04bff02f231 103 AnalogIn eixo_X(A2);
leogrotti 42:a04bff02f231 104 AnalogIn eixo_Y(A3);
leogrotti 42:a04bff02f231 105 AnalogIn eixo_ZU(A0);
leogrotti 42:a04bff02f231 106 AnalogIn eixo_ZD(A1);
Brunoporto2702 32:481674c0f90d 107
leogrotti 54:175e4f3682e8 108 DigitalIn botao1 (PC_1); //1
leogrotti 54:175e4f3682e8 109 DigitalIn botao2 (PC_0); //2
leogrotti 54:175e4f3682e8 110 DigitalIn botao3 (PC_10); //3
leogrotti 54:175e4f3682e8 111 DigitalIn botao4 (PC_12); //4
digo1234 58:c600f21ab6f9 112 DigitalIn emergencia(PC_4);// butão de emergencia
leogrotti 43:d08a3f6b65b5 113
Brunoporto2702 32:481674c0f90d 114 float x;
Brunoporto2702 32:481674c0f90d 115 float y;
Brunoporto2702 32:481674c0f90d 116 float w;
Brunoporto2702 32:481674c0f90d 117 float u;
leogrotti 42:a04bff02f231 118 float zu;
leogrotti 42:a04bff02f231 119 float zd;
leogrotti 43:d08a3f6b65b5 120 float a;
leogrotti 43:d08a3f6b65b5 121 float pos;
leogrotti 43:d08a3f6b65b5 122 float sp;
leogrotti 43:d08a3f6b65b5 123 float vc;
Brunoporto2702 32:481674c0f90d 124
leogrotti 50:58a2df31077a 125 int Dir_X;
leogrotti 50:58a2df31077a 126 int Dir_Y;
leogrotti 50:58a2df31077a 127 int Dir_Z;
leogrotti 51:3f0e4e641054 128 int segu_fim_curso;
leogrotti 43:d08a3f6b65b5 129
leogrotti 50:58a2df31077a 130 int count;
leogrotti 44:eecdc0b60f14 131
leogrotti 44:eecdc0b60f14 132 int dif_posX1;
leogrotti 44:eecdc0b60f14 133 int dif_posY1;
leogrotti 44:eecdc0b60f14 134 int dif_posZ1;
leogrotti 44:eecdc0b60f14 135 int dif_posX2;
leogrotti 44:eecdc0b60f14 136 int dif_posY2;
leogrotti 44:eecdc0b60f14 137 int dif_posZ2;
leogrotti 44:eecdc0b60f14 138 int dif_posX3;
leogrotti 44:eecdc0b60f14 139 int dif_posY3;
leogrotti 44:eecdc0b60f14 140 int dif_posZ3;
leogrotti 44:eecdc0b60f14 141
digo1234 59:8d34a4bfc2b7 142 int listaX[20];
digo1234 59:8d34a4bfc2b7 143 int listaY[20];
digo1234 59:8d34a4bfc2b7 144 int listaZ[20];
digo1234 60:9f4d34edef70 145 int listaCola[20];
leogrotti 46:63b15a56cbe5 146
leogrotti 46:63b15a56cbe5 147 int index = 0;
leogrotti 46:63b15a56cbe5 148
leogrotti 42:a04bff02f231 149 bool flagX1=0;
leogrotti 42:a04bff02f231 150 bool flagY1=0;
leogrotti 35:a05bef8dd995 151
digo1234 56:5aedf3b4e184 152 int safe = 0;
digo1234 58:c600f21ab6f9 153 int safe_emergencia = 0;
digo1234 60:9f4d34edef70 154 int safe_menu = 0;
digo1234 60:9f4d34edef70 155 int safe_eixoY = 0;
digo1234 56:5aedf3b4e184 156
leogrotti 34:098efd69c86d 157 unsigned int minspeed = 1300;
Brunoporto2702 32:481674c0f90d 158
leogrotti 34:098efd69c86d 159 //int step = 0x08;
Brunoporto2702 32:481674c0f90d 160
leogrotti 35:a05bef8dd995 161 unsigned int speed1;
leogrotti 35:a05bef8dd995 162 unsigned int speed2;
leogrotti 39:7e30bcc989d3 163 unsigned int speed3;
leogrotti 36:0cd4fdbb40af 164
leogrotti 38:c3d77ff8168a 165 //unsigned int pos3;
Brunoporto2702 32:481674c0f90d 166
Davidroid 0:e6a49a092e2a 167
Davidroid 0:e6a49a092e2a 168 /* Motor Control Component. */
Davidroid 3:02d9ec4f88b2 169 L6474 *motor1;
Davidroid 3:02d9ec4f88b2 170 L6474 *motor2;
leogrotti 39:7e30bcc989d3 171 L6474 *motor3;
Davidroid 0:e6a49a092e2a 172
Davidroid 0:e6a49a092e2a 173
Davidroid 0:e6a49a092e2a 174 /* Main ----------------------------------------------------------------------*/
leogrotti 55:b2ad60cc21a1 175 // função de correção de passos
leogrotti 44:eecdc0b60f14 176 int correcao_passo (int passo){
leogrotti 44:eecdc0b60f14 177 int passo_verdade;
leogrotti 45:8fab21ab1251 178 if (passo != 0){
leogrotti 44:eecdc0b60f14 179 passo_verdade = passo/1.0892 - 15,56;
leogrotti 45:8fab21ab1251 180 }
leogrotti 47:b5dc79d3fb35 181 else {
leogrotti 47:b5dc79d3fb35 182 passo_verdade = 0;
leogrotti 47:b5dc79d3fb35 183 }
leogrotti 55:b2ad60cc21a1 184 return (passo_verdade);
leogrotti 44:eecdc0b60f14 185 }
leogrotti 55:b2ad60cc21a1 186 // função salva posição
leogrotti 53:7c56b06e694e 187 void salva_posicao(){
leogrotti 53:7c56b06e694e 188 listaX[index] = motor2->get_position();
leogrotti 53:7c56b06e694e 189 listaY[index] = motor1->get_position();
leogrotti 53:7c56b06e694e 190 listaZ[index] = motor3->get_position();
leogrotti 53:7c56b06e694e 191 for (int j=0; j<=index; j++) {
digo1234 60:9f4d34edef70 192 printf("posX%d %d: \r\n", j+1, listaX[j]);
digo1234 60:9f4d34edef70 193 printf("posY%d %d: \r\n", j+1, listaY[j]);
digo1234 60:9f4d34edef70 194 printf("posZ%d %d: \r\n", j+1, listaZ[j]);
digo1234 60:9f4d34edef70 195 printf("Cola? :entre %d e %d %d \r\n", j+1,j, listaCola[j]);
leogrotti 53:7c56b06e694e 196 }
leogrotti 53:7c56b06e694e 197 index = index+1;
leogrotti 53:7c56b06e694e 198 wait_ms(DELAY_2);
leogrotti 53:7c56b06e694e 199 }
leogrotti 55:b2ad60cc21a1 200 // função percorre caminho
digo1234 60:9f4d34edef70 201 void vai_caminho_desejado (int listapontosX[20], int listapontosY[20], int listapontosZ[20], int listapontosCola[20], int index_local) {
leogrotti 46:63b15a56cbe5 202 int PYatual = motor1->get_position();
leogrotti 46:63b15a56cbe5 203 int PXatual = motor2->get_position();
leogrotti 46:63b15a56cbe5 204 int PZatual = motor3->get_position();
leogrotti 44:eecdc0b60f14 205
leogrotti 46:63b15a56cbe5 206 int calcdeltaX = correcao_passo(listapontosX[0] - PXatual);
leogrotti 46:63b15a56cbe5 207 int calcdeltaY = correcao_passo(listapontosY[0] - PYatual);
leogrotti 46:63b15a56cbe5 208 int calcdeltaZ = correcao_passo(listapontosZ[0] - PZatual);
leogrotti 46:63b15a56cbe5 209
digo1234 59:8d34a4bfc2b7 210 if (calcdeltaZ > 0) {
digo1234 59:8d34a4bfc2b7 211 motor3->move(StepperMotor::FWD, calcdeltaZ);
digo1234 59:8d34a4bfc2b7 212 motor3->wait_while_active();
digo1234 59:8d34a4bfc2b7 213 }
digo1234 59:8d34a4bfc2b7 214 else {
digo1234 59:8d34a4bfc2b7 215 motor3->move(StepperMotor::BWD, -calcdeltaZ);
digo1234 59:8d34a4bfc2b7 216 motor3->wait_while_active();
digo1234 59:8d34a4bfc2b7 217 }
digo1234 59:8d34a4bfc2b7 218
leogrotti 50:58a2df31077a 219 if (calcdeltaX >= 0 and calcdeltaY >= 0 ) {
leogrotti 46:63b15a56cbe5 220 motor2->move(StepperMotor::FWD, calcdeltaX);
leogrotti 47:b5dc79d3fb35 221 motor1->move(StepperMotor::FWD, calcdeltaY);
leogrotti 46:63b15a56cbe5 222 motor2->wait_while_active();
leogrotti 47:b5dc79d3fb35 223 motor1->wait_while_active();
leogrotti 46:63b15a56cbe5 224 }
leogrotti 50:58a2df31077a 225 if (calcdeltaX >= 0 and calcdeltaY < 0 ) {
leogrotti 47:b5dc79d3fb35 226 motor2->move(StepperMotor::FWD, calcdeltaX);
leogrotti 47:b5dc79d3fb35 227 motor1->move(StepperMotor::BWD, -calcdeltaY);
leogrotti 47:b5dc79d3fb35 228 motor2->wait_while_active();
leogrotti 46:63b15a56cbe5 229 motor1->wait_while_active();
leogrotti 46:63b15a56cbe5 230 }
leogrotti 50:58a2df31077a 231 if (calcdeltaX < 0 and calcdeltaY >= 0 ) {
leogrotti 47:b5dc79d3fb35 232 motor2->move(StepperMotor::BWD, -calcdeltaX);
leogrotti 47:b5dc79d3fb35 233 motor1->move(StepperMotor::FWD, calcdeltaY);
leogrotti 47:b5dc79d3fb35 234 motor2->wait_while_active();
leogrotti 47:b5dc79d3fb35 235 motor1->wait_while_active();
leogrotti 47:b5dc79d3fb35 236 }
leogrotti 47:b5dc79d3fb35 237 if (calcdeltaX < 0 and calcdeltaY < 0 ) {
leogrotti 47:b5dc79d3fb35 238 motor2->move(StepperMotor::BWD, -calcdeltaX);
leogrotti 47:b5dc79d3fb35 239 motor1->move(StepperMotor::BWD, -calcdeltaY);
leogrotti 47:b5dc79d3fb35 240 motor2->wait_while_active();
leogrotti 47:b5dc79d3fb35 241 motor1->wait_while_active();
leogrotti 47:b5dc79d3fb35 242 }
leogrotti 47:b5dc79d3fb35 243
digo1234 59:8d34a4bfc2b7 244
leogrotti 50:58a2df31077a 245
leogrotti 46:63b15a56cbe5 246 printf("O for vai rodar: %d \r\n", index_local-1);
leogrotti 46:63b15a56cbe5 247 for (int i = 1; i< index_local ; i++) {
digo1234 60:9f4d34edef70 248 if (i==index_local-1){
digo1234 59:8d34a4bfc2b7 249 solenoide.write(0);
digo1234 59:8d34a4bfc2b7 250 }
digo1234 60:9f4d34edef70 251
digo1234 60:9f4d34edef70 252 if (listapontosCola[i] == 0){
digo1234 60:9f4d34edef70 253 printf("solenoide desliga %i \r\n",listapontosCola[i]);
digo1234 60:9f4d34edef70 254 solenoide.write(1);
digo1234 60:9f4d34edef70 255 }
digo1234 60:9f4d34edef70 256
digo1234 60:9f4d34edef70 257 else {
digo1234 60:9f4d34edef70 258 printf("solenoide liga %i \r\n",listapontosCola[i]);
digo1234 60:9f4d34edef70 259 solenoide.write(0);
digo1234 60:9f4d34edef70 260 }
leogrotti 46:63b15a56cbe5 261
leogrotti 46:63b15a56cbe5 262 calcdeltaX = correcao_passo(listapontosX[i] - listapontosX[i-1]);
leogrotti 46:63b15a56cbe5 263 calcdeltaY = correcao_passo(listapontosY[i] - listapontosY[i-1]);
leogrotti 46:63b15a56cbe5 264 calcdeltaZ = correcao_passo(listapontosZ[i] - listapontosZ[i-1]);
leogrotti 46:63b15a56cbe5 265
digo1234 59:8d34a4bfc2b7 266 if (calcdeltaZ > 0) {
digo1234 59:8d34a4bfc2b7 267 motor3->move(StepperMotor::FWD, calcdeltaZ);
digo1234 59:8d34a4bfc2b7 268 motor3->wait_while_active();
digo1234 59:8d34a4bfc2b7 269 }
digo1234 59:8d34a4bfc2b7 270 else {
digo1234 59:8d34a4bfc2b7 271 motor3->move(StepperMotor::BWD, -calcdeltaZ);
digo1234 59:8d34a4bfc2b7 272 motor3->wait_while_active();
digo1234 59:8d34a4bfc2b7 273 }
digo1234 59:8d34a4bfc2b7 274
leogrotti 50:58a2df31077a 275 if (calcdeltaX >= 0 and calcdeltaY >= 0) {
leogrotti 46:63b15a56cbe5 276 motor2->move(StepperMotor::FWD, calcdeltaX);
leogrotti 48:76ccfa408a33 277 motor1->move(StepperMotor::FWD, calcdeltaY);
leogrotti 46:63b15a56cbe5 278 motor2->wait_while_active();
leogrotti 48:76ccfa408a33 279 motor1->wait_while_active();
leogrotti 46:63b15a56cbe5 280 }
leogrotti 50:58a2df31077a 281 if (calcdeltaX >= 0 and calcdeltaY < 0) {
leogrotti 48:76ccfa408a33 282 motor2->move(StepperMotor::FWD, calcdeltaX);
leogrotti 48:76ccfa408a33 283 motor1->move(StepperMotor::BWD, -calcdeltaY);
leogrotti 48:76ccfa408a33 284 motor2->wait_while_active();
leogrotti 46:63b15a56cbe5 285 motor1->wait_while_active();
leogrotti 46:63b15a56cbe5 286 }
leogrotti 50:58a2df31077a 287 if (calcdeltaX < 0 and calcdeltaY >= 0) {
leogrotti 48:76ccfa408a33 288 motor2->move(StepperMotor::BWD, -calcdeltaX);
leogrotti 48:76ccfa408a33 289 motor1->move(StepperMotor::FWD, calcdeltaY);
leogrotti 48:76ccfa408a33 290 motor2->wait_while_active();
leogrotti 48:76ccfa408a33 291 motor1->wait_while_active();
leogrotti 48:76ccfa408a33 292 }
leogrotti 48:76ccfa408a33 293 if (calcdeltaX < 0 and calcdeltaY < 0) {
leogrotti 48:76ccfa408a33 294 motor2->move(StepperMotor::BWD, -calcdeltaX);
leogrotti 48:76ccfa408a33 295 motor1->move(StepperMotor::BWD, -calcdeltaY);
leogrotti 48:76ccfa408a33 296 motor2->wait_while_active();
leogrotti 48:76ccfa408a33 297 motor1->wait_while_active();
leogrotti 48:76ccfa408a33 298 }
leogrotti 46:63b15a56cbe5 299 }
leogrotti 46:63b15a56cbe5 300 }
leogrotti 46:63b15a56cbe5 301
leogrotti 46:63b15a56cbe5 302 int main()
Davidroid 0:e6a49a092e2a 303 {
Davidroid 8:cec4c2c03a27 304 /*----- Initialization. -----*/
Davidroid 8:cec4c2c03a27 305
Davidroid 0:e6a49a092e2a 306 /* Initializing SPI bus. */
Davidroid 0:e6a49a092e2a 307 DevSPI dev_spi(D11, D12, D13);
Davidroid 0:e6a49a092e2a 308
Davidroid 9:a9e51320aee4 309 /* Initializing Motor Control Components. */
Davidroid 5:a0268a435bb1 310 motor1 = new L6474(D2, D8, D7, D9, D10, dev_spi);
Davidroid 16:810667a9f31f 311 motor2 = new L6474(D2, D8, D4, D3, D10, dev_spi);
leogrotti 39:7e30bcc989d3 312 motor3 = new L6474(D2, D8, D5, D6, D10, dev_spi);
Davidroid 29:526970c1d998 313 if (motor1->init() != COMPONENT_OK) {
Davidroid 14:fcd452b03d1a 314 exit(EXIT_FAILURE);
Davidroid 28:3f17a4152bcf 315 }
Davidroid 29:526970c1d998 316 if (motor2->init() != COMPONENT_OK) {
Davidroid 14:fcd452b03d1a 317 exit(EXIT_FAILURE);
Davidroid 28:3f17a4152bcf 318 }
leogrotti 39:7e30bcc989d3 319 if (motor3->init() != COMPONENT_OK) {
leogrotti 39:7e30bcc989d3 320 exit(EXIT_FAILURE);
leogrotti 39:7e30bcc989d3 321 }
Davidroid 0:e6a49a092e2a 322
leogrotti 55:b2ad60cc21a1 323 //ligando lcd
leogrotti 53:7c56b06e694e 324 lcd.setBacklight(TextLCD::LightOn);
leogrotti 53:7c56b06e694e 325 lcd.setCursor(TextLCD::CurOff_BlkOff);
leogrotti 55:b2ad60cc21a1 326 //abrindo menu inicial lcd
leogrotti 53:7c56b06e694e 327 lcd.cls();
leogrotti 53:7c56b06e694e 328 lcd.setAddress(3,0);
leogrotti 53:7c56b06e694e 329 lcd.printf("MENU PRINCIPAL");
leogrotti 53:7c56b06e694e 330 lcd.setAddress(0,2);
leogrotti 53:7c56b06e694e 331 lcd.printf("1-SALVAR PONTOS");
leogrotti 53:7c56b06e694e 332 lcd.setAddress(0,3);
leogrotti 53:7c56b06e694e 333 lcd.printf("2-INICIAR PROGRAMA");
digo1234 60:9f4d34edef70 334 printf("programa rodando \r\n");
Davidroid 28:3f17a4152bcf 335 while(true) {
digo1234 58:c600f21ab6f9 336 if (!botao1 && !botao2 && !botao3 && !botao4 && !emergencia){
digo1234 56:5aedf3b4e184 337 safe = 0;
digo1234 56:5aedf3b4e184 338 }
digo1234 60:9f4d34edef70 339 solenoide.write(1);
leogrotti 42:a04bff02f231 340 count = count +1;
digo1234 60:9f4d34edef70 341 //printf("programa rodando \r\n");
leogrotti 42:a04bff02f231 342 // Leitura analógica
Brunoporto2702 32:481674c0f90d 343 u = eixo_X.read();
Brunoporto2702 32:481674c0f90d 344 w = eixo_Y.read();
leogrotti 42:a04bff02f231 345 zu = eixo_ZU.read();
leogrotti 42:a04bff02f231 346 zd = eixo_ZD.read();
leogrotti 54:175e4f3682e8 347 sp = botao1.read();
leogrotti 54:175e4f3682e8 348 vc = botao2.read();
leogrotti 52:7f80fc02710e 349 if (count == 50) {
digo1234 60:9f4d34edef70 350 //printf("programa rodando \r\n");
digo1234 60:9f4d34edef70 351 printf("u =%f \r\n\n", u);
leogrotti 52:7f80fc02710e 352 wait_ms(DELAY_1);
leogrotti 52:7f80fc02710e 353
leogrotti 43:d08a3f6b65b5 354 count = 0;
leogrotti 43:d08a3f6b65b5 355 }
leogrotti 43:d08a3f6b65b5 356
leogrotti 42:a04bff02f231 357
leogrotti 42:a04bff02f231 358 // Movimentando eixo Z cima caso botão 1 apertado
digo1234 60:9f4d34edef70 359 if (zu >0.7 && !fim_curso_Z ) {
leogrotti 42:a04bff02f231 360 motor3->run(StepperMotor::FWD);
leogrotti 53:7c56b06e694e 361 segu_fim_curso = 1;
leogrotti 53:7c56b06e694e 362 Dir_Z = 1;
leogrotti 53:7c56b06e694e 363 wait_ms(DELAY_1);
leogrotti 53:7c56b06e694e 364 }
leogrotti 53:7c56b06e694e 365
leogrotti 53:7c56b06e694e 366 // Movimentando eixo Z baixo caso botão 2 apertado
digo1234 60:9f4d34edef70 367 else if (zd >0.7 && !fim_curso_Z ) {
leogrotti 53:7c56b06e694e 368 motor3->run(StepperMotor::BWD);
leogrotti 53:7c56b06e694e 369 segu_fim_curso = 1;
leogrotti 53:7c56b06e694e 370 Dir_Z = 0;
leogrotti 50:58a2df31077a 371 wait_ms(DELAY_1);
leogrotti 42:a04bff02f231 372 }
leogrotti 34:098efd69c86d 373
leogrotti 42:a04bff02f231 374 // Parando eixo Z caso botão 1 e 2 liberado
leogrotti 53:7c56b06e694e 375 else /*(zu< 0.7 && zd < 0.7 )*/{
leogrotti 42:a04bff02f231 376 motor3->hard_stop();
leogrotti 52:7f80fc02710e 377 wait_ms(DELAY_1);
leogrotti 53:7c56b06e694e 378 if (fim_curso_Z && segu_fim_curso==1) {
leogrotti 53:7c56b06e694e 379 printf("Fim de curso Z apertada \r\n");
leogrotti 53:7c56b06e694e 380 if (Dir_Z == 1){
leogrotti 53:7c56b06e694e 381 motor3->move(StepperMotor::BWD, 1000);
leogrotti 53:7c56b06e694e 382 motor3->wait_while_active();
leogrotti 53:7c56b06e694e 383 segu_fim_curso = 0;
leogrotti 53:7c56b06e694e 384 wait_ms(DELAY_1);
leogrotti 53:7c56b06e694e 385 }
leogrotti 53:7c56b06e694e 386 if (Dir_Z == 0){
leogrotti 53:7c56b06e694e 387 motor3->move(StepperMotor::FWD, 1000);
leogrotti 53:7c56b06e694e 388 motor3->wait_while_active();
leogrotti 53:7c56b06e694e 389 segu_fim_curso = 0;
leogrotti 53:7c56b06e694e 390 wait_ms(DELAY_1);
leogrotti 53:7c56b06e694e 391 }
leogrotti 53:7c56b06e694e 392 }
leogrotti 42:a04bff02f231 393 }
leogrotti 42:a04bff02f231 394
leogrotti 52:7f80fc02710e 395 // Movimentando eixo Y duas direções com joystick
leogrotti 42:a04bff02f231 396 // Movimentando eixo Y fwd
digo1234 60:9f4d34edef70 397 if (u>0.820 && !fim_curso_Y && !botao1 && !botao2 && zu<0.7 && zd<0.7) {
digo1234 60:9f4d34edef70 398 printf("motor y indo para frente");
leogrotti 42:a04bff02f231 399 motor1->run(StepperMotor::FWD);
leogrotti 50:58a2df31077a 400 Dir_Y = 1;
leogrotti 51:3f0e4e641054 401 segu_fim_curso = 1;
leogrotti 52:7f80fc02710e 402 wait_ms(DELAY_1);
leogrotti 42:a04bff02f231 403 }
leogrotti 42:a04bff02f231 404 // Movimentando eixo Y bwd
digo1234 60:9f4d34edef70 405 else if(u<0.65 && !fim_curso_Y && !botao1 && !botao2 && zu<0.7 && zd<0.7) {
digo1234 60:9f4d34edef70 406 printf("motor y indo para tras");
leogrotti 42:a04bff02f231 407 motor1->run(StepperMotor::BWD);
leogrotti 50:58a2df31077a 408 Dir_Y = 0;
leogrotti 51:3f0e4e641054 409 segu_fim_curso = 1;
leogrotti 52:7f80fc02710e 410 wait_ms(DELAY_1);
Brunoporto2702 32:481674c0f90d 411 }
leogrotti 50:58a2df31077a 412
leogrotti 42:a04bff02f231 413 // parando eixo Y
leogrotti 50:58a2df31077a 414 else {
leogrotti 42:a04bff02f231 415 motor1->hard_stop();
leogrotti 50:58a2df31077a 416 wait_ms(DELAY_1);
digo1234 60:9f4d34edef70 417 if (fim_curso_Y && segu_fim_curso==1 ) {
leogrotti 51:3f0e4e641054 418 printf("Fim de curso Y apertada \r\n");
leogrotti 50:58a2df31077a 419 if (Dir_Y == 1){
leogrotti 50:58a2df31077a 420 motor1->move(StepperMotor::BWD, 1000);
leogrotti 50:58a2df31077a 421 motor1->wait_while_active();
leogrotti 51:3f0e4e641054 422 segu_fim_curso = 0;
leogrotti 52:7f80fc02710e 423 wait_ms(DELAY_1);
leogrotti 50:58a2df31077a 424 }
leogrotti 50:58a2df31077a 425 if (Dir_Y == 0){
leogrotti 50:58a2df31077a 426 motor1->move(StepperMotor::FWD, 1000);
leogrotti 50:58a2df31077a 427 motor1->wait_while_active();
leogrotti 51:3f0e4e641054 428 segu_fim_curso = 0;
leogrotti 52:7f80fc02710e 429 wait_ms(DELAY_1);
leogrotti 50:58a2df31077a 430 }
leogrotti 50:58a2df31077a 431 }
leogrotti 42:a04bff02f231 432 }
leogrotti 42:a04bff02f231 433
leogrotti 42:a04bff02f231 434 // Movimentando eixo X duas direções com joystick
leogrotti 43:d08a3f6b65b5 435 // Movimentando eixo X fwd
leogrotti 50:58a2df31077a 436 if (w>0.80 && !fim_curso_X) {
leogrotti 34:098efd69c86d 437 motor2->run(StepperMotor::BWD);
leogrotti 50:58a2df31077a 438 Dir_X = 1;
leogrotti 51:3f0e4e641054 439 segu_fim_curso = 1;
leogrotti 52:7f80fc02710e 440 wait_ms(DELAY_1);
leogrotti 50:58a2df31077a 441 }
leogrotti 50:58a2df31077a 442
leogrotti 43:d08a3f6b65b5 443 // Movimentando eixo X Bwd
leogrotti 50:58a2df31077a 444 else if(w<0.60 && !fim_curso_X) {
leogrotti 50:58a2df31077a 445 motor2->run(StepperMotor::FWD);
leogrotti 50:58a2df31077a 446 Dir_X = 0;
leogrotti 51:3f0e4e641054 447 segu_fim_curso = 1;
leogrotti 52:7f80fc02710e 448 wait_ms(DELAY_1);
leogrotti 42:a04bff02f231 449 }
leogrotti 42:a04bff02f231 450 // parando eixo X
leogrotti 50:58a2df31077a 451 else{
Brunoporto2702 32:481674c0f90d 452 motor2->hard_stop();
leogrotti 50:58a2df31077a 453 wait_ms(DELAY_1);
leogrotti 51:3f0e4e641054 454 if (fim_curso_X && segu_fim_curso==1) {
leogrotti 51:3f0e4e641054 455 printf("Fim de curso X apertada \r\n");
leogrotti 50:58a2df31077a 456 if (Dir_X == 1){
leogrotti 50:58a2df31077a 457 motor2->move(StepperMotor::FWD, 1000);
leogrotti 50:58a2df31077a 458 motor2->wait_while_active();
leogrotti 51:3f0e4e641054 459 segu_fim_curso = 0;
leogrotti 52:7f80fc02710e 460 wait_ms(DELAY_1);
leogrotti 50:58a2df31077a 461 }
leogrotti 50:58a2df31077a 462 if (Dir_X == 0){
leogrotti 50:58a2df31077a 463 motor2->move(StepperMotor::BWD, 1000);
leogrotti 50:58a2df31077a 464 motor2->wait_while_active();
leogrotti 51:3f0e4e641054 465 segu_fim_curso = 0;
leogrotti 52:7f80fc02710e 466 wait_ms(DELAY_1);
leogrotti 50:58a2df31077a 467 }
leogrotti 50:58a2df31077a 468 }
Brunoporto2702 32:481674c0f90d 469 }
digo1234 58:c600f21ab6f9 470
digo1234 58:c600f21ab6f9 471 // botão de emergencia apertado
digo1234 58:c600f21ab6f9 472 if(emergencia && safe_emergencia==0){
digo1234 58:c600f21ab6f9 473 lcd.cls();
digo1234 58:c600f21ab6f9 474 lcd.setAddress(0,1);
digo1234 58:c600f21ab6f9 475 lcd.printf("BOTAO DE EMERGENCIA");
digo1234 58:c600f21ab6f9 476 lcd.setAddress(4,2);
digo1234 58:c600f21ab6f9 477 lcd.printf("PRESSIONADO");
digo1234 58:c600f21ab6f9 478 safe_emergencia=1;
digo1234 58:c600f21ab6f9 479 }
digo1234 58:c600f21ab6f9 480 if(!emergencia && safe_emergencia==1){
digo1234 58:c600f21ab6f9 481 lcd.cls();
digo1234 58:c600f21ab6f9 482 lcd.setAddress(0,0);
digo1234 58:c600f21ab6f9 483 lcd.printf("PONTOS PERDIDOS");
digo1234 58:c600f21ab6f9 484 lcd.setAddress(0,1);
digo1234 58:c600f21ab6f9 485 lcd.printf("MAQUINA REINICIANDO");
digo1234 58:c600f21ab6f9 486 wait(2);
digo1234 58:c600f21ab6f9 487 safe_emergencia=0;
digo1234 58:c600f21ab6f9 488 NVIC_SystemReset();
digo1234 58:c600f21ab6f9 489 }
digo1234 58:c600f21ab6f9 490
digo1234 58:c600f21ab6f9 491
digo1234 60:9f4d34edef70 492 // Menus
digo1234 60:9f4d34edef70 493
digo1234 60:9f4d34edef70 494 if (menu == 0 && safe_menu == 0 ){
digo1234 60:9f4d34edef70 495 lcd.cls();
digo1234 60:9f4d34edef70 496 lcd.setAddress(3,0);
digo1234 60:9f4d34edef70 497 lcd.printf("MENU PRINCIPAL");
digo1234 60:9f4d34edef70 498 lcd.setAddress(0,2);
digo1234 60:9f4d34edef70 499 lcd.printf("1-SALVAR NOVOS PONTOS");
digo1234 60:9f4d34edef70 500 lcd.setAddress(0,3);
digo1234 60:9f4d34edef70 501 lcd.printf("2-INICIAR PROGRAMA");
digo1234 60:9f4d34edef70 502 safe_menu = 1;
digo1234 60:9f4d34edef70 503 }
digo1234 60:9f4d34edef70 504
digo1234 60:9f4d34edef70 505 if (menu == 1 && safe_menu == 0 ) {
digo1234 60:9f4d34edef70 506 lcd.cls();
digo1234 60:9f4d34edef70 507 lcd.setCursor(TextLCD::CurOff_BlkOff);
digo1234 60:9f4d34edef70 508 lcd.setAddress(0,0);
digo1234 60:9f4d34edef70 509 lcd.printf("1-SALVAR PONTO %i",index+1);
digo1234 60:9f4d34edef70 510 lcd.setAddress(0,1);
digo1234 60:9f4d34edef70 511 lcd.printf("2-VOLTAR");
digo1234 60:9f4d34edef70 512 lcd.setAddress(0,2);
digo1234 60:9f4d34edef70 513 lcd.printf("3-CANCELAR ");
digo1234 60:9f4d34edef70 514 lcd.setAddress(0,3);
digo1234 60:9f4d34edef70 515 lcd.printf("4-CONCLUIR ");
digo1234 60:9f4d34edef70 516 safe_menu = 1;
digo1234 60:9f4d34edef70 517 }
digo1234 60:9f4d34edef70 518
digo1234 60:9f4d34edef70 519 if (menu == 2 && safe_menu == 0 ) {
digo1234 60:9f4d34edef70 520 lcd.cls();
digo1234 60:9f4d34edef70 521 lcd.setCursor(TextLCD::CurOff_BlkOff);
digo1234 60:9f4d34edef70 522 lcd.setAddress(0,0);
digo1234 60:9f4d34edef70 523 lcd.printf("QUER APLICAR COLA ");
digo1234 60:9f4d34edef70 524 lcd.setAddress(0,1);
digo1234 60:9f4d34edef70 525 lcd.printf("ENTRE PONTO %i E %i",index,index-1);
digo1234 60:9f4d34edef70 526 lcd.setAddress(0,3);
digo1234 60:9f4d34edef70 527 lcd.printf("1-SIM ");
digo1234 60:9f4d34edef70 528 lcd.setAddress(8,3);
digo1234 60:9f4d34edef70 529 lcd.printf("2-NAO");
digo1234 60:9f4d34edef70 530 safe_menu = 1;
digo1234 60:9f4d34edef70 531 }
digo1234 60:9f4d34edef70 532
digo1234 60:9f4d34edef70 533 if (menu == 3 && safe_menu == 0 ) {
digo1234 60:9f4d34edef70 534 lcd.cls();
digo1234 60:9f4d34edef70 535 lcd.setAddress(2,0);
digo1234 60:9f4d34edef70 536 lcd.printf("TEM CERTEZA QUE");
digo1234 60:9f4d34edef70 537 lcd.setAddress(2,1);
digo1234 60:9f4d34edef70 538 lcd.printf("DESEJA CANCELAR?");
digo1234 60:9f4d34edef70 539 lcd.setAddress(1,2);
digo1234 60:9f4d34edef70 540 lcd.printf("PONTOS IRAO APAGAR");
digo1234 60:9f4d34edef70 541 lcd.setAddress(1,4);
digo1234 60:9f4d34edef70 542 lcd.printf("1-SIM");
digo1234 60:9f4d34edef70 543 lcd.setAddress(10,4);
digo1234 60:9f4d34edef70 544 lcd.printf("2-NAO");
digo1234 60:9f4d34edef70 545 safe_menu = 1;
digo1234 60:9f4d34edef70 546 }
digo1234 60:9f4d34edef70 547
digo1234 60:9f4d34edef70 548 if (menu == 4 && safe_menu == 0 ){
digo1234 60:9f4d34edef70 549 lcd.cls();
digo1234 60:9f4d34edef70 550 lcd.setAddress(0,0);
digo1234 60:9f4d34edef70 551 lcd.printf("PONTOS SALVOS");
digo1234 60:9f4d34edef70 552 lcd.setAddress(0,2);
digo1234 60:9f4d34edef70 553 lcd.printf("1-INICIAR PROGRAMA");
digo1234 60:9f4d34edef70 554 lcd.setAddress(0,3);
digo1234 60:9f4d34edef70 555 lcd.printf("2-VOLTAR A SALVAR");
digo1234 60:9f4d34edef70 556 safe_menu = 1;
digo1234 60:9f4d34edef70 557 }
digo1234 60:9f4d34edef70 558
digo1234 60:9f4d34edef70 559 if (menu == 5 && safe_menu == 0 ){
digo1234 60:9f4d34edef70 560 lcd.cls();
digo1234 60:9f4d34edef70 561 lcd.setAddress(0,0);
digo1234 60:9f4d34edef70 562 lcd.printf("SALVE SEU PONTO");
digo1234 60:9f4d34edef70 563 lcd.setAddress(0,1);
digo1234 60:9f4d34edef70 564 lcd.printf("DE RETIRADA");
digo1234 60:9f4d34edef70 565 lcd.setAddress(0,4);
digo1234 60:9f4d34edef70 566 lcd.printf("1-SALVAR PONTO");
digo1234 60:9f4d34edef70 567 safe_menu = 1;
digo1234 60:9f4d34edef70 568 }
digo1234 60:9f4d34edef70 569
digo1234 60:9f4d34edef70 570 if (menu ==6 && safe_menu == 0) {
digo1234 60:9f4d34edef70 571 lcd.cls();
digo1234 60:9f4d34edef70 572 lcd.setAddress(0,1);
digo1234 60:9f4d34edef70 573 lcd.printf("NAO HA PONTOS SALVOS");
digo1234 60:9f4d34edef70 574 wait(2);
digo1234 60:9f4d34edef70 575 menu=0;
digo1234 60:9f4d34edef70 576 }
digo1234 60:9f4d34edef70 577
digo1234 60:9f4d34edef70 578 if (menu == 7 && safe_menu == 0) {
digo1234 60:9f4d34edef70 579 lcd.setAddress(0,1);
digo1234 60:9f4d34edef70 580 lcd.printf("AGUARDE...");
digo1234 60:9f4d34edef70 581 safe_menu =1;
digo1234 60:9f4d34edef70 582 }
digo1234 60:9f4d34edef70 583
digo1234 60:9f4d34edef70 584
digo1234 60:9f4d34edef70 585 // botao1
digo1234 60:9f4d34edef70 586
digo1234 60:9f4d34edef70 587 if (botao1 && menu == 0 && safe == 0){
digo1234 60:9f4d34edef70 588 for (int i=0; i<21;i++){
digo1234 60:9f4d34edef70 589 listaX[i] = 0;
digo1234 60:9f4d34edef70 590 listaY[i] = 0;
digo1234 60:9f4d34edef70 591 listaZ[i] = 0;
digo1234 60:9f4d34edef70 592 }
digo1234 60:9f4d34edef70 593 index = 0;
digo1234 60:9f4d34edef70 594 menu = 0;
digo1234 60:9f4d34edef70 595 menu = 1;
digo1234 60:9f4d34edef70 596 safe =1;
digo1234 60:9f4d34edef70 597 safe_menu = 0;
digo1234 60:9f4d34edef70 598 }
digo1234 60:9f4d34edef70 599
digo1234 60:9f4d34edef70 600 if (botao1 && menu == 1 && safe == 0) {
digo1234 60:9f4d34edef70 601 if (index<1){
digo1234 60:9f4d34edef70 602 listaCola[index-1]=0;
digo1234 60:9f4d34edef70 603 salva_posicao();
digo1234 60:9f4d34edef70 604 }
digo1234 60:9f4d34edef70 605 else {
digo1234 60:9f4d34edef70 606 menu = 2;
digo1234 60:9f4d34edef70 607 salva_posicao();
digo1234 60:9f4d34edef70 608 }
digo1234 60:9f4d34edef70 609 safe = 1;
digo1234 60:9f4d34edef70 610 safe_menu = 0;
digo1234 60:9f4d34edef70 611 }
digo1234 60:9f4d34edef70 612
digo1234 60:9f4d34edef70 613 if (botao1 && menu == 2 && safe == 0) {
digo1234 60:9f4d34edef70 614 listaCola[index-1]=1;
digo1234 60:9f4d34edef70 615 for (int a=0; a<=index; a++){
digo1234 60:9f4d34edef70 616 printf("cola : %i \r\n", listaCola[a]);
digo1234 60:9f4d34edef70 617 }
digo1234 60:9f4d34edef70 618 menu = 1;
digo1234 60:9f4d34edef70 619 safe = 1;
digo1234 60:9f4d34edef70 620 safe_menu = 0;
digo1234 60:9f4d34edef70 621 }
digo1234 60:9f4d34edef70 622
digo1234 60:9f4d34edef70 623 if (botao1 && menu == 3 && safe == 0) {
digo1234 60:9f4d34edef70 624 for (int i=0; i<21;i++){
digo1234 60:9f4d34edef70 625 listaX[i] = 0;
digo1234 60:9f4d34edef70 626 listaY[i] = 0;
digo1234 60:9f4d34edef70 627 listaZ[i] = 0;
digo1234 60:9f4d34edef70 628 }
digo1234 60:9f4d34edef70 629 index = 0;
digo1234 60:9f4d34edef70 630 lcd.cls();
digo1234 60:9f4d34edef70 631 lcd.setAddress(0,1);
digo1234 60:9f4d34edef70 632 lcd.printf("OPERACAO CANCELADA!");
digo1234 60:9f4d34edef70 633 lcd.setAddress(0,2);
digo1234 60:9f4d34edef70 634 lcd.printf("PONTOS APAGADOS!");
digo1234 60:9f4d34edef70 635 wait(2);
digo1234 60:9f4d34edef70 636 menu = 0;
digo1234 60:9f4d34edef70 637 safe = 1;
digo1234 60:9f4d34edef70 638 safe_menu = 0;
digo1234 60:9f4d34edef70 639 }
digo1234 60:9f4d34edef70 640
digo1234 60:9f4d34edef70 641 if (botao1 && menu == 4 && safe == 0) {
digo1234 60:9f4d34edef70 642
digo1234 60:9f4d34edef70 643 menu = 5;
digo1234 60:9f4d34edef70 644 safe = 1;
digo1234 60:9f4d34edef70 645 safe_menu = 0;
digo1234 60:9f4d34edef70 646 }
digo1234 60:9f4d34edef70 647
digo1234 60:9f4d34edef70 648 if (botao1 && menu == 5 && safe == 0) {
digo1234 60:9f4d34edef70 649 salva_posicao();
digo1234 60:9f4d34edef70 650 listaCola[index]=0;
digo1234 60:9f4d34edef70 651 lcd.cls();
digo1234 60:9f4d34edef70 652 lcd.setAddress(0,1);
digo1234 60:9f4d34edef70 653 lcd.printf("AGUARDE...");
digo1234 60:9f4d34edef70 654 vai_caminho_desejado(listaX, listaY, listaZ, listaCola, index);
digo1234 60:9f4d34edef70 655 menu = 0;
digo1234 60:9f4d34edef70 656 safe = 1;
digo1234 60:9f4d34edef70 657 safe_menu = 0;
digo1234 60:9f4d34edef70 658 }
digo1234 60:9f4d34edef70 659
digo1234 60:9f4d34edef70 660 // botao2
digo1234 60:9f4d34edef70 661
digo1234 60:9f4d34edef70 662 if (botao2 && menu == 0 && safe == 0) {
digo1234 60:9f4d34edef70 663 if (listaX[1]==0&& listaY[1]==0 && listaZ[1]==0) {
digo1234 60:9f4d34edef70 664 menu = 6;
digo1234 60:9f4d34edef70 665 }
digo1234 60:9f4d34edef70 666 else {
digo1234 60:9f4d34edef70 667 vai_caminho_desejado(listaX, listaY, listaZ, listaCola, index);
digo1234 60:9f4d34edef70 668 menu = 0;
digo1234 60:9f4d34edef70 669 }
digo1234 60:9f4d34edef70 670 safe = 1;
digo1234 60:9f4d34edef70 671 safe_menu = 0;
digo1234 60:9f4d34edef70 672 }
digo1234 60:9f4d34edef70 673
digo1234 60:9f4d34edef70 674 if (botao2 && menu == 1 && safe == 0) {
digo1234 60:9f4d34edef70 675 listaX[index] = 0;
digo1234 60:9f4d34edef70 676 listaY[index] = 0;
digo1234 60:9f4d34edef70 677 listaZ[index] = 0;
digo1234 60:9f4d34edef70 678 listaCola[index]=0;
digo1234 60:9f4d34edef70 679 index = index - 1;
digo1234 60:9f4d34edef70 680 menu = 1;
digo1234 60:9f4d34edef70 681 if (index<0){
digo1234 60:9f4d34edef70 682 index=0;
digo1234 60:9f4d34edef70 683 menu=0;
digo1234 60:9f4d34edef70 684 }
digo1234 60:9f4d34edef70 685 safe = 1;
digo1234 60:9f4d34edef70 686 safe_menu = 0;
digo1234 60:9f4d34edef70 687 }
digo1234 60:9f4d34edef70 688
digo1234 60:9f4d34edef70 689 if (botao2 && menu == 2 && safe == 0) {
digo1234 60:9f4d34edef70 690 listaCola[index-1]=0;
digo1234 60:9f4d34edef70 691 for (int a=0; a<=index; a++){
digo1234 60:9f4d34edef70 692 printf("cola : %i \r\n", listaCola[a]);
digo1234 60:9f4d34edef70 693 }
digo1234 60:9f4d34edef70 694 menu=1;
digo1234 60:9f4d34edef70 695 safe = 1;
digo1234 60:9f4d34edef70 696 safe_menu = 0;
digo1234 60:9f4d34edef70 697 }
digo1234 60:9f4d34edef70 698
digo1234 60:9f4d34edef70 699 if (botao2 && menu == 3 && safe == 0) {
digo1234 60:9f4d34edef70 700 menu = 1;
digo1234 60:9f4d34edef70 701 safe = 1;
digo1234 60:9f4d34edef70 702 safe_menu = 0;
digo1234 60:9f4d34edef70 703 }
digo1234 60:9f4d34edef70 704
digo1234 60:9f4d34edef70 705 if (botao2 && menu == 4 && safe == 0) {
digo1234 60:9f4d34edef70 706 menu = 1;
digo1234 60:9f4d34edef70 707 safe = 1;
digo1234 60:9f4d34edef70 708 safe_menu = 0;
digo1234 60:9f4d34edef70 709 }
digo1234 60:9f4d34edef70 710
digo1234 60:9f4d34edef70 711 // botao3
digo1234 60:9f4d34edef70 712
digo1234 60:9f4d34edef70 713 if (botao3 && menu == 1 && safe == 0) {
digo1234 60:9f4d34edef70 714 menu = 3;
digo1234 60:9f4d34edef70 715 safe = 1;
digo1234 60:9f4d34edef70 716 safe_menu = 0;
digo1234 60:9f4d34edef70 717 }
digo1234 60:9f4d34edef70 718
digo1234 60:9f4d34edef70 719 // botao4
digo1234 60:9f4d34edef70 720
digo1234 60:9f4d34edef70 721 if (botao4 && menu == 1 && safe ==0) {
digo1234 60:9f4d34edef70 722 menu = 4;
digo1234 60:9f4d34edef70 723 safe = 1;
digo1234 60:9f4d34edef70 724 safe_menu = 0;
digo1234 60:9f4d34edef70 725 }
digo1234 60:9f4d34edef70 726
digo1234 60:9f4d34edef70 727
digo1234 60:9f4d34edef70 728
leogrotti 50:58a2df31077a 729 }
leogrotti 44:eecdc0b60f14 730
digo1234 60:9f4d34edef70 731 }