File content as of revision 61:b7ef7d06c57b:

/**
 ******************************************************************************
 * @file    main.cpp
 * @author  Davide Aliprandi, STMicroelectronics
 * @version V1.0.0
 * @date    October 14th, 1025
 * @brief   mbed test application for the STMicroelectronics X-NUCLEO-IHM01A1
 *          Motor Control Expansion Board: control of 2 motors.
 ******************************************************************************
 * @attention
 *
 * <h2><center>&copy; COPYRIGHT(c) 1025 STMicroelectronics</center></h2>
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *   1. Redistributions of source code must retain the above copyright notice,
 *      this list of conditions and the following disclaimer.
 *   2. Redistributions in binary form must reproduce the above copyright notice,
 *      this list of conditions and the following disclaimer in the documentation
 *      and/or other materials provided with the distribution.
 *   3. Neither the name of STMicroelectronics nor the names of its contributors
 *      may be used to endorse or promote products derived from this software
 *      without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 ******************************************************************************
 */


/* Includes ------------------------------------------------------------------*/

/* mbed specific header files. */
#include "mbed.h"

/* Helper header files. */
#include "DevSPI.h"

/* Component specific header files. */
#include "L6474.h"

#include <list>

/* includes para os lcds */
#include "TextLCD.h"
/* definindo apertado e solto para lcd */
#define apertado 1
#define solto 0


/* Definitions ---------------------------------------------------------------*/

/* Number of steps. */
#define STEPS 3101

/* Delay in milliseconds. */
#define DELAY_1 20
#define DELAY_2 101
#define DELAY_3 1010

/* Speed in pps (Pulses Per Second).
   In Full Step mode: 1 pps = 1 step/s).
   In 1/N Step Mode:  N pps = 1 step/s). */
#define SPEED_1 1300
#define SPEED_2 1300



/* Variables -----------------------------------------------------------------*/

DigitalIn fim_curso_X (PD_2);
DigitalIn fim_curso_Y (PC_11);
DigitalIn fim_curso_Z (PC_8);

DigitalOut solenoide (PB_15);

Serial pc(USBTX, USBRX);


/* LCD */
I2C i2c_lcd(D14,D15); // sda, scl

TextLCD_I2C lcd(&i2c_lcd, 0x7E, TextLCD::LCD20x4);                  // I2C exp: I2C bus, PCF8574 Slaveaddress, LCD Type

int menu=0;
int menu_memoria=0;
//float xmenu=1.0, ymenu=2.0, zmenu=3.0;
int inicial= 1;
int pisca =0;
float i=0.0;

/* FIM LCD */

AnalogIn eixo_X(A2);
AnalogIn eixo_Y(A3);
AnalogIn eixo_ZU(A0);
AnalogIn eixo_ZD(A1);

DigitalIn botao1 (PC_1); //1
DigitalIn botao2  (PC_0); //2
DigitalIn botao3  (PC_10); //3
DigitalIn botao4  (PC_12); //4
DigitalIn emergencia(PC_4);// butão de emergencia

float x;
float y;
float w;
float u;
float zu;
float zd;
float a;
float pos;
float sp;
float vc;

int Dir_X;
int Dir_Y;
int Dir_Z;
int segu_fim_curso;

int count;

int dif_posX1;
int dif_posY1;
int dif_posZ1;
int dif_posX2;
int dif_posY2;
int dif_posZ2;
int dif_posX3;
int dif_posY3;
int dif_posZ3;

int listaX[20];
int listaY[20];
int listaZ[20];
int listaCola[20];

int index = 0;

bool flagX1=0; 
bool flagY1=0; 

int safe = 0;
int safe_emergencia = 0;
int safe_menu = 0;
int safe_eixoY = 0;

unsigned int minspeed = 1300;

//int step = 0x08;

unsigned int speed1;
unsigned int speed2;
unsigned int speed3;

//unsigned int pos3;


/* Motor Control Component. */
L6474 *motor1;
L6474 *motor2;
L6474 *motor3;


/* Main ----------------------------------------------------------------------*/
// função de correção de passos
 int correcao_passo (int passo){
     int passo_verdade;
     if (passo != 0){
     passo_verdade = passo/1.0892 - 15,56;
     }
     else {
         passo_verdade = 0;
         }
     return (passo_verdade);  
     }
// função salva posição 
 void salva_posicao(){
    listaX[index] = motor2->get_position();
    listaY[index] = motor1->get_position();
    listaZ[index] = motor3->get_position();
        for (int j=0; j<=index; j++) {
            printf("posX%d %d: \r\n", j+1, listaX[j]);
            printf("posY%d %d: \r\n", j+1, listaY[j]);
            printf("posZ%d %d: \r\n", j+1, listaZ[j]);
            printf("Cola? :entre %d e %d %d \r\n", j+1,j, listaCola[j]);
            }
        index = index+1;
        wait_ms(DELAY_2);
    }
// função percorre caminho
 void vai_caminho_desejado (int listapontosX[20], int listapontosY[20], int listapontosZ[20], int listapontosCola[20], int index_local) {
    int PYatual = motor1->get_position();
    int PXatual = motor2->get_position();
    int PZatual = motor3->get_position();
     
    int calcdeltaX = correcao_passo(listapontosX[0] - PXatual);
    int calcdeltaY = correcao_passo(listapontosY[0] - PYatual);
    int calcdeltaZ = correcao_passo(listapontosZ[0] - PZatual);
     
    if (calcdeltaZ > 0) {
        motor3->move(StepperMotor::FWD, calcdeltaZ);
        motor3->wait_while_active();
        }
        else {
            motor3->move(StepperMotor::BWD, -calcdeltaZ);
            motor3->wait_while_active();
            }
            
    if (calcdeltaX >= 0 and calcdeltaY >= 0 ) {
        motor2->move(StepperMotor::FWD, calcdeltaX);
        motor1->move(StepperMotor::FWD, calcdeltaY);
        motor2->wait_while_active();
        motor1->wait_while_active();
        }
    if (calcdeltaX >= 0 and calcdeltaY < 0 ) {
        motor2->move(StepperMotor::FWD, calcdeltaX);
        motor1->move(StepperMotor::BWD, -calcdeltaY);
        motor2->wait_while_active();
        motor1->wait_while_active();
        }
    if (calcdeltaX < 0 and calcdeltaY >= 0 ) {
        motor2->move(StepperMotor::BWD, -calcdeltaX);
        motor1->move(StepperMotor::FWD, calcdeltaY);
        motor2->wait_while_active();
        motor1->wait_while_active();
        }
    if (calcdeltaX < 0 and calcdeltaY < 0 ) {
        motor2->move(StepperMotor::BWD, -calcdeltaX);
        motor1->move(StepperMotor::BWD, -calcdeltaY);
        motor2->wait_while_active();
        motor1->wait_while_active();
        }
       
    
            
    printf("O for vai rodar: %d \r\n", index_local-1);
    for (int i = 1; i< index_local ; i++) {
        if (i==index_local-1){
            solenoide.write(0);
            }
            
        if (listapontosCola[i] == 0){
            printf("solenoide desliga %i \r\n",listapontosCola[i]);
            solenoide.write(1);
            }
            
        else {
            printf("solenoide liga %i \r\n",listapontosCola[i]);
            solenoide.write(0);
            }
        
        calcdeltaX = correcao_passo(listapontosX[i] - listapontosX[i-1]);
        calcdeltaY = correcao_passo(listapontosY[i] - listapontosY[i-1]);
        calcdeltaZ = correcao_passo(listapontosZ[i] - listapontosZ[i-1]);
        
        if (calcdeltaZ > 0) {
        motor3->move(StepperMotor::FWD, calcdeltaZ);
        motor3->wait_while_active();
        }
            else {
            motor3->move(StepperMotor::BWD, -calcdeltaZ);
            motor3->wait_while_active();
            }
            
        if (calcdeltaX >= 0 and calcdeltaY >= 0) {
        motor2->move(StepperMotor::FWD, calcdeltaX);
        motor1->move(StepperMotor::FWD, calcdeltaY);
        motor2->wait_while_active();
        motor1->wait_while_active();
        }
        if (calcdeltaX >= 0 and calcdeltaY < 0) {
        motor2->move(StepperMotor::FWD, calcdeltaX);
        motor1->move(StepperMotor::BWD, -calcdeltaY);
        motor2->wait_while_active();
        motor1->wait_while_active();
        }
        if (calcdeltaX < 0 and calcdeltaY >= 0) {
        motor2->move(StepperMotor::BWD, -calcdeltaX);
        motor1->move(StepperMotor::FWD, calcdeltaY);
        motor2->wait_while_active();
        motor1->wait_while_active();
        }
        if (calcdeltaX < 0 and calcdeltaY < 0) {
        motor2->move(StepperMotor::BWD, -calcdeltaX);
        motor1->move(StepperMotor::BWD, -calcdeltaY);
        motor2->wait_while_active();
        motor1->wait_while_active();
        }
        }
    }
        
int main() 
{
    /*----- Initialization. -----*/

    /* Initializing SPI bus. */
    DevSPI dev_spi(D11, D12, D13);

    /* Initializing Motor Control Components. */
    motor1 = new L6474(D2, D8, D7, D9, D10, dev_spi);
    motor2 = new L6474(D2, D8, D4, D3, D10, dev_spi);
    motor3 = new L6474(D2, D8, D5, D6, D10, dev_spi);
    if (motor1->init() != COMPONENT_OK) {
        exit(EXIT_FAILURE);
    }
    if (motor2->init() != COMPONENT_OK) {
        exit(EXIT_FAILURE);
    }
    if (motor3->init() != COMPONENT_OK) {
        exit(EXIT_FAILURE);
    }

//ligando lcd
    lcd.setBacklight(TextLCD::LightOn);
    lcd.setCursor(TextLCD::CurOff_BlkOff);
//abrindo menu inicial lcd    
            lcd.cls();
            lcd.setAddress(3,0);
            lcd.printf("MENU PRINCIPAL");
            lcd.setAddress(0,2);
            lcd.printf("1-SALVAR PONTOS");
            lcd.setAddress(0,3);
            lcd.printf("2-INICIAR PROGRAMA");
 printf("programa rodando \r\n");
    while(true) {
        if (!botao1 && !botao2 && !botao3 && !botao4 && !emergencia){
            safe = 0;
            }
        solenoide.write(1);
        count = count +1;
        //printf("programa rodando \r\n");
        // Leitura analógica
        u = eixo_X.read();
        w = eixo_Y.read();
        zu = eixo_ZU.read();
        zd = eixo_ZD.read();
        sp = botao1.read();
        vc = botao2.read();
        if (count == 50) {
            //printf("programa rodando \r\n");
            printf("u =%f \r\n\n", u);
            wait_ms(DELAY_1);

            count = 0;
            }

    
 // Movimentando eixo Z cima caso botão 1 apertado
    if (zu >0.7 && !fim_curso_Z ) {
        motor3->run(StepperMotor::FWD);
        segu_fim_curso = 1;
        Dir_Z = 1;
        wait_ms(DELAY_1);
        }           
                     
// Movimentando eixo Z baixo caso botão 2 apertado
    else if (zd >0.7 && !fim_curso_Z ) {
        motor3->run(StepperMotor::BWD);
        segu_fim_curso = 1;
        Dir_Z = 0;
        wait_ms(DELAY_1);
        }
                
  // Parando eixo Z caso botão 1 e 2 liberado              
    else /*(zu< 0.7 && zd < 0.7 )*/{
            motor3->hard_stop();
            wait_ms(DELAY_1);
            if (fim_curso_Z && segu_fim_curso==1) {
                    printf("Fim de curso Z apertada \r\n");
                    if (Dir_Z == 1){
                        motor3->move(StepperMotor::BWD, 1000);
                        motor3->wait_while_active();
                        segu_fim_curso = 0;
                        wait_ms(DELAY_1);
                        }
                    if (Dir_Z == 0){
                        motor3->move(StepperMotor::FWD, 1000);
                        motor3->wait_while_active();
                        segu_fim_curso = 0;
                        wait_ms(DELAY_1);
                        }
                    }
            }
            
// Movimentando eixo Y duas direções com joystick       
// Movimentando eixo Y fwd        
        if (u>0.820 && !fim_curso_Y && !botao1 && !botao2 && zu<0.7 && zd<0.7) {
            printf("motor y indo para frente");
            motor1->run(StepperMotor::FWD);
            Dir_Y = 1;
            segu_fim_curso = 1;
            wait_ms(DELAY_1);
            }
// Movimentando eixo Y bwd             
            else if(u<0.65 && !fim_curso_Y && !botao1 && !botao2 && zu<0.7 && zd<0.7) {
                printf("motor y indo para tras");
                motor1->run(StepperMotor::BWD);
                Dir_Y = 0;
                segu_fim_curso = 1;
                wait_ms(DELAY_1);
                }
                
// parando eixo Y
            else {
                motor1->hard_stop();
                wait_ms(DELAY_1);
                if (fim_curso_Y && segu_fim_curso==1 ) {
                    printf("Fim de curso Y apertada \r\n");
                    if (Dir_Y == 1){
                        motor1->move(StepperMotor::BWD, 1000);
                        motor1->wait_while_active();
                        segu_fim_curso = 0;
                        wait_ms(DELAY_1);
                        }
                    if (Dir_Y == 0){
                        motor1->move(StepperMotor::FWD, 1000);
                        motor1->wait_while_active();
                        segu_fim_curso = 0;
                        wait_ms(DELAY_1);
                        }
                    }
                }
                
// Movimentando eixo X duas direções com joystick
// Movimentando eixo X fwd   
        if (w>0.80 && !fim_curso_X) {
            motor2->run(StepperMotor::BWD);
            Dir_X = 1;
            segu_fim_curso = 1;
            wait_ms(DELAY_1);
                }
            
// Movimentando eixo X Bwd 
            else if(w<0.60 && !fim_curso_X) {
                motor2->run(StepperMotor::FWD); 
                Dir_X = 0;
                segu_fim_curso = 1;
                wait_ms(DELAY_1);
                }
// parando eixo X
            else{
                motor2->hard_stop();
                wait_ms(DELAY_1);        
                if (fim_curso_X && segu_fim_curso==1) {
                    printf("Fim de curso X apertada \r\n");
                    if (Dir_X == 1){
                        motor2->move(StepperMotor::FWD, 1000);
                        motor2->wait_while_active();
                        segu_fim_curso = 0;
                        wait_ms(DELAY_1);
                        }
                    if (Dir_X == 0){
                        motor2->move(StepperMotor::BWD, 1000);
                        motor2->wait_while_active();
                        segu_fim_curso = 0;
                        wait_ms(DELAY_1);
                        }
                    }
               }
            
// botão de emergencia apertado  
   if(emergencia && safe_emergencia==0){
           lcd.cls();
           lcd.setAddress(0,1);
           lcd.printf("BOTAO DE EMERGENCIA");
           lcd.setAddress(4,2);
           lcd.printf("PRESSIONADO");
           safe_emergencia=1;
           }
        if(!emergencia && safe_emergencia==1){
            lcd.cls();
            lcd.setAddress(0,0);
            lcd.printf("PONTOS PERDIDOS");
            lcd.setAddress(0,1);
            lcd.printf("MAQUINA REINICIANDO");
            wait(2);
            safe_emergencia=0;      
            NVIC_SystemReset();
            }
            
            
// Menus 

    if (menu == 0 && safe_menu == 0 ){
        lcd.cls();
        lcd.setAddress(3,0);
        lcd.printf("MENU PRINCIPAL");
        lcd.setAddress(0,2);
        lcd.printf("1-SALVAR NOVOS PONTOS");
        lcd.setAddress(0,3);
        lcd.printf("2-INICIAR PROGRAMA"); 
        safe_menu = 1;
        }
        
    if (menu == 1 && safe_menu == 0 ) {
        lcd.cls();
        lcd.setCursor(TextLCD::CurOff_BlkOff);
        lcd.setAddress(0,0);
        lcd.printf("1-SALVAR PONTO %i",index+1);
        lcd.setAddress(0,1);
        lcd.printf("2-VOLTAR");
        lcd.setAddress(0,2);
        lcd.printf("3-CANCELAR ");
        lcd.setAddress(0,3);
        lcd.printf("4-CONCLUIR ");
        safe_menu = 1;
        }
    
    if (menu == 2 && safe_menu == 0 ) {
        lcd.cls();
        lcd.setCursor(TextLCD::CurOff_BlkOff);
        lcd.setAddress(0,0);
        lcd.printf("QUER APLICAR COLA ");
        lcd.setAddress(0,1);
        lcd.printf("ENTRE PONTO %i E %i",index,index-1);
        lcd.setAddress(0,3);
        lcd.printf("1-SIM ");
        lcd.setAddress(8,3);
        lcd.printf("2-NAO");
        safe_menu = 1;
        }
    
    if (menu == 3 && safe_menu == 0 ) {
        lcd.cls();
        lcd.setAddress(2,0);
        lcd.printf("TEM CERTEZA QUE");
        lcd.setAddress(2,1);
        lcd.printf("DESEJA CANCELAR?");
        lcd.setAddress(1,2);
        lcd.printf("PONTOS IRAO APAGAR");
        lcd.setAddress(1,4);
        lcd.printf("1-SIM");
        lcd.setAddress(10,4);
        lcd.printf("2-NAO");
        safe_menu = 1;
        }
    
    if (menu == 4 && safe_menu == 0 ){
        lcd.cls();
        lcd.setAddress(0,0);
        lcd.printf("PONTOS SALVOS");
        lcd.setAddress(0,2);
        lcd.printf("1-INICIAR PROGRAMA");
        lcd.setAddress(0,3);
        lcd.printf("2-VOLTAR A SALVAR");
        safe_menu = 1;
        }
    
    if (menu == 5 && safe_menu == 0 ){
        lcd.cls();
        lcd.setAddress(0,0);
        lcd.printf("SALVE SEU PONTO");
        lcd.setAddress(0,1);
        lcd.printf("DE RETIRADA");
        lcd.setAddress(0,4);
        lcd.printf("1-SALVAR PONTO");
        safe_menu = 1;
        }
        
    if (menu ==6 && safe_menu == 0) {
        lcd.cls();
        lcd.setAddress(0,1);
        lcd.printf("NAO HA PONTOS SALVOS");
        wait(2);
        menu=0;
        }
    
    if (menu == 7 && safe_menu == 0) {    
        lcd.setAddress(0,1);
        lcd.printf("AGUARDE...");
        safe_menu =1;
        }
        
    
 //   botao1
 
    if (botao1 && menu == 0 && safe == 0){
        for (int i=0; i<21;i++){
            listaX[i] = 0;
            listaY[i] = 0;
            listaZ[i] = 0;
            } 
        index = 0;    
        menu = 0;
        menu = 1;
        safe =1;
        safe_menu = 0;
        }
    
    if (botao1 && menu == 1 && safe == 0) {
        if (index<1){
            listaCola[index-1]=0;
            salva_posicao();
            }
        else {
            menu = 2;
            salva_posicao();
            }
        safe = 1;
        safe_menu = 0;
        }
    
    if (botao1 && menu == 2 && safe == 0) {
        listaCola[index-1]=1;
        for (int a=0; a<=index; a++){
            printf("cola : %i \r\n", listaCola[a]);
            }
        menu = 1;
        safe = 1;
        safe_menu = 0;
        }
    
    if (botao1 && menu == 3 && safe == 0) {
        for (int i=0; i<21;i++){
            listaX[i] = 0;
            listaY[i] = 0;
            listaZ[i] = 0;
            } 
        index = 0;    
        lcd.cls();
        lcd.setAddress(0,1);
        lcd.printf("OPERACAO CANCELADA!");
        lcd.setAddress(0,2);
        lcd.printf("PONTOS APAGADOS!");
        wait(2);
        menu = 0;
        safe = 1;
        safe_menu = 0;
        }
    
    if (botao1 && menu == 4 && safe == 0) {

        menu = 5;
        safe = 1;
        safe_menu = 0;
        }
    
    if (botao1 && menu == 5 && safe == 0) {
        salva_posicao();
        listaCola[index]=0;
        lcd.cls();
        lcd.setAddress(0,1);
        lcd.printf("AGUARDE...");
        vai_caminho_desejado(listaX, listaY, listaZ, listaCola, index);
        menu = 0;
        safe = 1;
        safe_menu = 0;
        }
        
// botao2

    if (botao2 && menu == 0  && safe == 0) {
        if (listaX[1]==0&& listaY[1]==0 && listaZ[1]==0) {
            menu = 6;
            }
        else {
            vai_caminho_desejado(listaX, listaY, listaZ, listaCola, index);
            menu = 0;
            }
        safe = 1;
        safe_menu = 0;
        }
        
    if (botao2 && menu == 1  && safe == 0) {
        listaX[index] = 0;
        listaY[index] = 0;
        listaZ[index] = 0; 
        listaCola[index]=0;
        index = index - 1;
        menu = 1;
        if (index<0){
            index=0;
            menu=0;
            }
        safe = 1;
        safe_menu = 0;
        }
        
    if (botao2 && menu == 2  && safe == 0) {
        listaCola[index-1]=0;
        for (int a=0; a<=index; a++){
            printf("cola : %i \r\n", listaCola[a]);
            }
        menu=1;
        safe = 1;
        safe_menu = 0;
        }
    
    if (botao2 && menu == 3  && safe == 0) {
        menu = 1;
        safe = 1;
        safe_menu = 0;
        }
        
    if (botao2 && menu == 4  && safe == 0) {
        menu = 1;
        safe = 1;
        safe_menu = 0;
        }
    
// botao3 

    if (botao3 && menu == 1  && safe == 0) {
        menu = 3;
        safe = 1;
        safe_menu = 0;
        }
        
// botao4
    
    if (botao4 && menu == 1 && safe ==0) {
        menu = 4;
        safe = 1;
        safe_menu = 0;
        }
        
        

        }   

}