Joystick controlando 3 motores

Dependencies:   mbed X_NUCLEO_IHM01A1

main.cpp

Committer:
Gabiuas
Date:
2019-04-30
Revision:
32:74a4c733c11b
Parent:
31:3aae82e6353c

File content as of revision 32:74a4c733c11b:

#include "mbed.h"
#include "DevSPI.h"
#include "L6474.h"

#define VRx A0
#define VRy A1
#define VRz A2
#define SW D5

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

/* Delay in milliseconds. */
#define DELAY_1 2000
#define DELAY_2 6000
#define DELAY_3 8000

/* 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 2400
#define SPEED_2 1200

/* Joystick */
AnalogIn x_axis(VRx);                       // Create the analog x movement object
AnalogIn y_axis(VRy);                       // Create the analog y movement object

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

int main(){
    /* 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, D11, D5, D6, dev_spi); // Mudar Entradas
    
    if (motor1->init() != COMPONENT_OK) {
        exit(EXIT_FAILURE);
    }
    if (motor2->init() != COMPONENT_OK) {
        exit(EXIT_FAILURE);
    }
    if (motor3->init() != COMPONENT_OK) {
        exit(EXIT_FAILURE);
    }

    
    while(1)
    {
        printf("\n");
        
        
        /*----- Movendo Eixo X -----*/
        
        printf("\n   X axis: %f", x_axis.read());
        
        if (x_axis.read()>0.6){
            printf("--> Movendo X frente; Motor 1");
            motor1->run(StepperMotor::FWD);
        }
        else if (x_axis.read()<0.4){
            printf("--> Movendo X tras; Motor 1");
            motor1->run(StepperMotor::BWD);
        }
        else if (0.4<x_axis.read() and x_axis.read()<0.6){
            printf("--> Parado; motor 1");
            motor1->soft_stop();
        }
        
        
        /*----- Movendo Eixo Y -----*/
        
        printf("              Y axis: %f", y_axis.read());
        
        if (y_axis.read()<0.4){
            printf("--> Movendo Y frente; Motor 2");
            motor2->run(StepperMotor::FWD);
        }
        else if (y_axis.read()>0.6){
            printf("--> Movendo Y tras; Motor 2");
            motor2->run(StepperMotor::BWD);
        }
        else if (0.4<y_axis.read() and y_axis.read()<0.6){
            printf("--> Parado; motor 2");
            motor2->soft_stop();
        }
        
        
        /*----- Movendo Eixo Z -----*/
        
        printf("              Z axis: %f", z_axis.read());
        
        if (z_axis.read()>0.6){
            printf("--> Movendo Z frente; Motor 3");
            motor3->run(StepperMotor::FWD);
        }
        else if (z_axis.read()<0.4){
            printf("--> Movendo Z tras; Motor 3");
            motor3->run(StepperMotor::BWD);
        }
        else if (0.4<z_axis.read() and x_axis.read()<0.6){
            printf("--> Parado; motor 3");
            motor3->soft_stop();
        }
        
        
        
        if (button==0){
            break;
        }
        
    }
}