2 Motores + Joystick
Dependencies: X_NUCLEO_IHM01A1 TextLCD
Fork of HelloWorld_IHM01A1_2Motors_mbedOS by
Diff: main.cpp
- Revision:
- 43:d08a3f6b65b5
- Parent:
- 42:a04bff02f231
- Child:
- 44:eecdc0b60f14
--- a/main.cpp Tue May 15 14:23:01 2018 +0000 +++ b/main.cpp Tue May 15 18:47:13 2018 +0000 @@ -56,8 +56,8 @@ /* Delay in milliseconds. */ #define DELAY_1 20 -#define DELAY_2 6000 -#define DELAY_3 8000 +#define DELAY_2 200 +#define DELAY_3 2000 /* Speed in pps (Pulses Per Second). In Full Step mode: 1 pps = 1 step/s). @@ -80,6 +80,10 @@ AnalogIn eixo_ZU(A0); AnalogIn eixo_ZD(A1); +DigitalIn salva_pos (PC_1); +DigitalIn vai_cami (PC_0); + + float x; @@ -88,8 +92,22 @@ 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; @@ -192,6 +210,10 @@ //sensorY1.rise(¶Y1); //sensorY1.fall(&continuaY1); +a = 0; +pos = 0; + + while(true) { count = count +1; @@ -200,45 +222,39 @@ w = eixo_Y.read(); zu = eixo_ZU.read(); zd = eixo_ZD.read(); - if (count == 10000) { - printf("zu =%f \r\n\n", zu); - printf("zd =%f \r\n\n", zd); - count = 0; - } - - //printf("X =%i \r\n\n", flagX1); - // printf("Y =%i \r\n\n", flagY1); + 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); - //wait_ms(DELAY_1); speed3 = motor3->get_speed(); - //wait_ms(DELAY_1); } // Parando eixo Z caso botão 1 e 2 liberado if (zu< 0.7 && zd < 0.7){ motor3->hard_stop(); - // wait_ms(DELAY_1); speed3 = 0; - //wait_ms(DELAY_1); } // Movimentando eixo Z baixo caso botão 2 apertado if (zd >0.7) { motor3->run(StepperMotor::BWD); - //wait_ms(DELAY_1); speed3 = motor3->get_speed(); - //wait_ms(DELAY_1); } // Parando eixo Z caso botão 1 e 2 liberado if (zu< 0.7 && zd < 0.7){ motor3->hard_stop(); - // wait_ms(DELAY_1); speed3 = 0; - //wait_ms(DELAY_1); } @@ -248,47 +264,152 @@ // Movimentando eixo Y fwd if (u>0.820) { motor1->run(StepperMotor::FWD); - //wait_ms(DELAY_1); speed1 = motor1->get_speed(); - //wait_ms(DELAY_1); } // Movimentando eixo Y bwd else{ if(u<0.65) { motor1->run(StepperMotor::BWD); - //wait_ms(DELAY_1); - speed1 = motor1->get_speed(); - // wait_ms(DELAY_1); + speed1 = motor1->get_speed(); } // parando eixo Y else motor1->hard_stop(); - //wait_ms(DELAY_1); speed1 = 0; - //wait_ms(DELAY_1); //} } // Movimentando eixo X duas direções com joystick //if (flagX1 == 0) { -// Movimentando eixo Y fwd +// Movimentando eixo X fwd if (w>0.80) { motor2->run(StepperMotor::BWD); speed2 = motor2->get_speed(); - printf("speed2 =%d \r\n\n", speed2); - printf("W =%f \r\n\n", w); + wait_ms(DELAY_1); } -// Movimentando eixo Y Bwd +// Movimentando eixo X Bwd else{ if(w<0.60) { motor2->run(StepperMotor::FWD); speed2 = motor2->get_speed(); - printf("speed2 =%d \r\n\n", speed2); - printf("W =%f \r\n\n", w); + wait_ms(DELAY_1); } // parando eixo X else motor2->hard_stop(); - speed2 = 0; - printf("speed2 =%d \r\n\n", speed2); + 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(); + } + } + + + } } \ No newline at end of file