2 Motores + Joystick

Dependencies:   X_NUCLEO_IHM01A1 TextLCD

Fork of HelloWorld_IHM01A1_2Motors_mbedOS by ST

main.cpp

Committer:
leogrotti
Date:
5 months ago
Revision:
43:d08a3f6b65b5
Parent:
42:a04bff02f231
Child:
44:eecdc0b60f14

File content as of revision 43:d08a3f6b65b5:

/**
 ******************************************************************************
 * @file    main.cpp
 * @author  Davide Aliprandi, STMicroelectronics
 * @version V1.0.0
 * @date    October 14th, 2015
 * @brief   mbed test application for the STMicroelectronics X-NUCLEO-IHM01A1
 *          Motor Control Expansion Board: control of 2 motors.
 ******************************************************************************
 * @attention
 *
 * <h2><center>&copy; COPYRIGHT(c) 2015 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"


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

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

/* Delay in milliseconds. */
#define DELAY_1 20
#define DELAY_2 200
#define DELAY_3 2000

/* 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 -----------------------------------------------------------------*/
//InterruptIn sensorX1(PC_9); 
//InterruptIn sensorY1(PA_14); 



Serial pc(USBTX, USBRX);

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

DigitalIn salva_pos (PC_1);
DigitalIn vai_cami (PC_0);




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

int count;
int position1Y ;
int position1X ;
int position1Z ;
int position2Y ;
int position2X ;
int position2Z ;

int position3Y ;
int position3X ;
int position3Z ;


bool flagX1=0; 
bool flagY1=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 ----------------------------------------------------------------------*/
/*
void released_1()
{
    motor3->hard_stop();
    printf ("parou \r\n");

}

void released_2()
{
    motor3->hard_stop();
    printf ("parou \r\n");

}

void paraX1() {
    flagX1 = 1;   
    motor2->hard_stop();
    
    }
    
    void continuaX1() {
    flagX1 = 0;
    }
 
 
 void paraY1() {
    flagY1 = 1;
    motor1->hard_stop();
    
    }
    
    void continuaY1() {
    flagY1 = 0;
    }
 */
 
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);
    }

 



    /* Attaching and enabling interrupt handlers. 
    motor3->attach_flag_irq(&flag_irq_handler3);
    motor3->enable_flag_irq();
    motor1->attach_flag_irq(&flag_irq_handler1);
    motor1->enable_flag_irq();
    motor2->attach_flag_irq(&flag_irq_handler2);
    motor2->enable_flag_irq();
    */
    
    //sensorX1.rise(&paraX1);
   // sensorX1.fall(&continuaX1);

    
    //sensorY1.rise(&paraY1);
    //sensorY1.fall(&continuaY1);
    
a = 0;
pos = 0;



    while(true) {
        count = count +1;
        // Leitura analógica
        u = eixo_X.read();
        w = eixo_Y.read();
        zu = eixo_ZU.read();
        zd = eixo_ZD.read();
        sp = salva_pos.read();
        vc = vai_cami.read();
        if (count == 9999999999999999) {
            printf("zu =%f \r\n\n", zu);
            printf("zd =%f \r\n\n", zd);
            printf("sp =%f \r\n\n", sp);
            printf("vc =%f \r\n\n", vc);
            count = 0;
            }

    
 // Movimentando eixo Z cima caso botão 1 apertado
    if (zu >0.7) {
        motor3->run(StepperMotor::FWD);
        speed3 = motor3->get_speed();
        }
                
  // Parando eixo Z caso botão 1 e 2 liberado              
        if (zu< 0.7 && zd < 0.7){
            motor3->hard_stop();
            speed3 = 0;
            }
                     
// Movimentando eixo Z baixo caso botão 2 apertado
    if (zd >0.7) {
        motor3->run(StepperMotor::BWD);
        speed3 = motor3->get_speed();
        }
                
  // Parando eixo Z caso botão 1 e 2 liberado              
        if (zu< 0.7 && zd < 0.7){
            motor3->hard_stop();
            speed3 = 0;
            }
            
             
// Movimentando eixo Y duas direções com joystick
    //if (flagY1 == 0 ) {
        
// Movimentando eixo Y fwd        
        if (u>0.820) {
            motor1->run(StepperMotor::FWD);
            speed1 = motor1->get_speed();
            }
// Movimentando eixo Y bwd             
            else{ if(u<0.65) {
                motor1->run(StepperMotor::BWD);
                speed1 = motor1->get_speed(); 
                }
// parando eixo Y
            else 
                motor1->hard_stop();
                speed1 = 0;
                //}
                }
                
// Movimentando eixo X duas direções com joystick
    //if (flagX1 == 0) {
// Movimentando eixo X fwd   
        if (w>0.80) {
            motor2->run(StepperMotor::BWD);
            speed2 = motor2->get_speed();
            wait_ms(DELAY_1);
            }
// Movimentando eixo X Bwd 
            else{ if(w<0.60) {
                motor2->run(StepperMotor::FWD);
                speed2 = motor2->get_speed();  
                wait_ms(DELAY_1);
                }
// parando eixo X
            else
                motor2->hard_stop();
                speed2 = motor2->get_speed();
                wait_ms(DELAY_1);
                
               }
  /*  if (a==0);{
        motor2->move(StepperMotor::BWD, 2000);
        motor2->wait_while_active();
        a= 1;
        }
    if (a==1);{
        motor2->move(StepperMotor::FWD, 2000);
        motor2->wait_while_active();
        a= 0;
        }
    */
    
    if (salva_pos) {
            if (pos==0) {
                int position1Y = motor1->get_position();
                int position1X = motor2->get_position();
                int position1Z = motor3->get_position();
            
                printf( "pos1Y = %i \n\r", position1Y);
                printf( "pos1X = %i \n\r", position1X);
                printf( "pos1Z = %i \n\r", position1Z);
            
                pos = 1;
                wait_ms(DELAY_2);
                }
            }
        
    
    if (salva_pos) {
            if (pos==1) {
                int position2Y = motor1->get_position();
                int position2X = motor2->get_position();
                int position2Z = motor3->get_position();
            
                printf( "pos2Y = %i \n\r", position2Y);
                printf( "pos2X = %i \n\r", position2X);
                printf( "pos2Z = %i \n\r", position2Z);
            
                pos = 2;
                wait_ms(DELAY_2);
                }
            }
        
    if (salva_pos) {
            if (pos==2) {
                int position3Y = motor1->get_position();
                int position3X = motor2->get_position();
                int position3Z = motor3->get_position();
            
                printf( "pos3Y = %i \n\r", position3Y);
                printf( "pos3X = %i \n\r", position3X);
                printf( "pos3Z = %i \n\r", position3Z);
            
                pos = 3;
                wait_ms(DELAY_2);
                }
            }
                
    if (vai_cami) {
        if (pos ==0){
            printf("sem posicoes salvas");
            wait_ms(DELAY_2);
            }
        if (pos == 1) {
            motor1->go_to(position1Y);
            motor2->go_to(position1X);
            motor3->go_to(position1Z);
            motor1->wait_while_active();
            motor2->wait_while_active();
            motor3->wait_while_active();
            }
        if (pos == 2) {
            motor1->go_to(position1Y);
            motor2->go_to(position1X);
            motor3->go_to(position1Z);
            motor1->wait_while_active();
            motor2->wait_while_active();
            motor3->wait_while_active();
            wait_ms(DELAY_3);
            motor1->go_to(position2Y);
            motor2->go_to(position2X);
            motor3->go_to(position2Z);
            motor1->wait_while_active();
            motor2->wait_while_active();
            motor3->wait_while_active();
            }
        if (pos == 3) {
            motor1->go_to(position1Y);
            motor2->go_to(position1X);
            motor3->go_to(position1Z);
            motor1->wait_while_active();
            motor2->wait_while_active();
            motor3->wait_while_active();
            wait_ms(DELAY_3);
            motor1->go_to(position2Y);
            motor2->go_to(position2X);
            motor3->go_to(position2Z);
            motor1->wait_while_active();
            motor2->wait_while_active();
            motor3->wait_while_active();
            wait_ms(DELAY_3);
            motor1->go_to(position3Y);
            motor2->go_to(position3X);
            motor3->go_to(position3Z);
            motor1->wait_while_active();
            motor2->wait_while_active();
            motor3->wait_while_active();
            }
            }
            
            
            
    }
}