2 Motores + Joystick
Dependencies: X_NUCLEO_IHM01A1 TextLCD
Fork of HelloWorld_IHM01A1_2Motors_mbedOS by
main.cpp
- Committer:
- leogrotti
- Date:
- 2018-05-15
- 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>© 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(¶X1); // sensorX1.fall(&continuaX1); //sensorY1.rise(¶Y1); //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(); } } } }