2 Motores + Joystick
Dependencies: X_NUCLEO_IHM01A1 TextLCD
Fork of HelloWorld_IHM01A1_2Motors_mbedOS by
Diff: main.cpp
- Revision:
- 44:eecdc0b60f14
- Parent:
- 43:d08a3f6b65b5
- Child:
- 45:8fab21ab1251
diff -r d08a3f6b65b5 -r eecdc0b60f14 main.cpp --- a/main.cpp Tue May 15 18:47:13 2018 +0000 +++ b/main.cpp Wed May 16 15:40:31 2018 +0000 @@ -109,6 +109,27 @@ int position3X ; int position3Z ; +int positionY_agora1; +int positionX_agora1; +int positionZ_agora1; +int positionY_agora2; +int positionX_agora2; +int positionZ_agora2; +int positionY_agora3; +int positionX_agora3; +int positionZ_agora3; + +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; + + bool flagX1=0; bool flagY1=0; @@ -168,7 +189,12 @@ flagY1 = 0; } */ - + int correcao_passo (int passo){ + int passo_verdade; + passo_verdade = passo/1.0892 - 15,56; + return (passo_verdade); + } + int main() { /*----- Initialization. -----*/ @@ -299,117 +325,386 @@ wait_ms(DELAY_1); } - /* if (a==0);{ - motor2->move(StepperMotor::BWD, 2000); + /* if (vai_cami){ + position1Y = motor1->get_position(); + position1X = motor2->get_position(); + position1Z = motor3->get_position(); + + printf( "pos1Y = %i \n\r", position1Y); + printf( "pos1X = %i \n\r", position1X); + printf( "pos1Z = %i \n\r", position1Z); + + + motor2->move(StepperMotor::BWD, 4574); motor2->wait_while_active(); - a= 1; + + + + positionY_agora1 = motor1->get_position(); + positionX_agora1= motor2->get_position(); + positionZ_agora1 = motor3->get_position(); + printf( "posY_agora1 = %i \n\r", positionY_agora1); + printf( "posX_agora1 = %i \n\r", positionX_agora1); + printf( "posZ_agora1 = %i \n\r", positionZ_agora1); + a= 1; } - if (a==1);{ - motor2->move(StepperMotor::FWD, 2000); + if (salva_pos){ + position2Y = motor1->get_position(); + position2X = motor2->get_position(); + position2Z = motor3->get_position(); + + printf( "pos2Y = %i \n\r", position2Y); + printf( "pos2X = %i \n\r", position2X); + printf( "pos2Z = %i \n\r", position2Z); + motor2->move(StepperMotor::FWD, 4574); motor2->wait_while_active(); + positionY_agora2 = motor1->get_position(); + positionX_agora2= motor2->get_position(); + positionZ_agora2 = motor3->get_position(); + printf( "posY_agora2 = %i \n\r", positionY_agora2); + printf( "posX_agora2 = %i \n\r", positionX_agora2); + printf( "posZ_agora2 = %i \n\r", positionZ_agora2); + a= 0; } + */ - if (salva_pos) { + if (salva_pos) { if (pos==0) { - int position1Y = motor1->get_position(); - int position1X = motor2->get_position(); - int position1Z = motor3->get_position(); + position1Y = motor1->get_position(); + position1X = motor2->get_position(); + position1Z = motor3->get_position(); printf( "pos1Y = %i \n\r", position1Y); printf( "pos1X = %i \n\r", position1X); printf( "pos1Z = %i \n\r", position1Z); pos = 1; + printf( "pos = %f \n\r", pos); 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(); + position2Y = motor1->get_position(); + position2X = motor2->get_position(); + position2Z = motor3->get_position(); printf( "pos2Y = %i \n\r", position2Y); printf( "pos2X = %i \n\r", position2X); printf( "pos2Z = %i \n\r", position2Z); pos = 2; + printf( "pos = %f \n\r", pos); 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(); + position3Y = motor1->get_position(); + position3X = motor2->get_position(); + position3Z = motor3->get_position(); printf( "pos3Y = %i \n\r", position3Y); printf( "pos3X = %i \n\r", position3X); printf( "pos3Z = %i \n\r", position3Z); pos = 3; + printf( "pos = %f \n\r", pos); wait_ms(DELAY_2); } - } - + } if (vai_cami) { + printf( "pos = %f \n\r", pos); 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); + printf( "pos1Y = %i \n\r", position1Y); + printf( "pos1X = %i \n\r", position1X); + printf( "pos1Z = %i \n\r", position1Z); + positionY_agora1 = motor1->get_position(); + positionX_agora1= motor2->get_position(); + positionZ_agora1 = motor3->get_position(); + printf( "posY_agora1 = %i \n\r", positionY_agora1); + printf( "posX_agora1 = %i \n\r", positionX_agora1); + printf( "posZ_agora1 = %i \n\r", positionZ_agora1); + dif_posX1 = position1X - positionX_agora1; + dif_posY1 = position1Y - positionY_agora1; + dif_posZ1 = position1Z - positionZ_agora1; + + printf( "difY1 = %i \n\r", correcao_passo(dif_posY1)); + printf( "difX1 = %i \n\r", correcao_passo(dif_posX1)); + printf( "difZ1 = %i \n\r", correcao_passo(dif_posZ1)); + if (dif_posX1 >0){ + motor2->move(StepperMotor::FWD, correcao_passo(dif_posX1)); + motor2->wait_while_active(); + } + if (dif_posX1 <0){ + dif_posX1 = -dif_posX1; + motor2->move(StepperMotor::BWD, correcao_passo(dif_posX1)); + motor2->wait_while_active(); + } + if (dif_posY1 >0){ + motor1->move(StepperMotor::FWD, correcao_passo(dif_posY1)); motor1->wait_while_active(); - motor2->wait_while_active(); + } + if (dif_posY1 <0){ + dif_posY1 = -dif_posY1; + motor1->move(StepperMotor::BWD, correcao_passo(dif_posY1)); + motor1->wait_while_active(); + } + if (dif_posZ1 >0){ + motor3->move(StepperMotor::FWD, correcao_passo(dif_posZ1)); motor3->wait_while_active(); } + if (dif_posZ1 <0){ + dif_posZ1 = -dif_posZ1; + motor3->move(StepperMotor::BWD, correcao_passo(dif_posZ1)); + motor3->wait_while_active(); + } + + positionY_agora1 = motor1->get_position(); + positionX_agora1= motor2->get_position(); + positionZ_agora1 = motor3->get_position(); + printf( "posY_agora1 = %i \n\r", positionY_agora1); + printf( "posX_agora1 = %i \n\r", positionX_agora1); + printf( "posZ_agora1 = %i \n\r", positionZ_agora1); + } if (pos == 2) { - motor1->go_to(position1Y); - motor2->go_to(position1X); - motor3->go_to(position1Z); + printf( "pos1Y = %i \n\r", position1Y); + printf( "pos1X = %i \n\r", position1X); + printf( "pos1Z = %i \n\r", position1Z); + positionY_agora1 = motor1->get_position(); + positionX_agora1= motor2->get_position(); + positionZ_agora1 = motor3->get_position(); + printf( "posY_agora1 = %i \n\r", positionY_agora1); + printf( "posX_agora1 = %i \n\r", positionX_agora1); + printf( "posZ_agora1 = %i \n\r", positionZ_agora1); + dif_posX1 = position1X - positionX_agora1; + dif_posY1 = position1Y - positionY_agora1; + dif_posZ1 = position1Z - positionZ_agora1; + + printf( "difY1 = %i \n\r", correcao_passo(dif_posY1)); + printf( "difX1 = %i \n\r", correcao_passo(dif_posX1)); + printf( "difZ1 = %i \n\r", correcao_passo(dif_posZ1)); + if (dif_posX1 >0){ + motor2->move(StepperMotor::FWD, correcao_passo(dif_posX1)); + motor2->wait_while_active(); + } + if (dif_posX1 <0){ + dif_posX1 = -dif_posX1; + motor2->move(StepperMotor::BWD, correcao_passo(dif_posX1)); + motor2->wait_while_active(); + } + if (dif_posY1 >0){ + motor1->move(StepperMotor::FWD, correcao_passo(dif_posY1)); motor1->wait_while_active(); - motor2->wait_while_active(); + } + if (dif_posY1 <0){ + dif_posY1 = -dif_posY1; + motor1->move(StepperMotor::BWD, correcao_passo(dif_posY1)); + motor1->wait_while_active(); + } + if (dif_posZ1 >0){ + motor3->move(StepperMotor::FWD, correcao_passo(dif_posZ1)); motor3->wait_while_active(); - wait_ms(DELAY_3); - motor1->go_to(position2Y); - motor2->go_to(position2X); - motor3->go_to(position2Z); + } + if (dif_posZ1 <0){ + dif_posZ1 = -dif_posZ1; + motor3->move(StepperMotor::BWD, correcao_passo(dif_posZ1)); + motor3->wait_while_active(); + } + + positionY_agora2 = motor1->get_position(); + positionX_agora2= motor2->get_position(); + positionZ_agora2 = motor3->get_position(); + printf( "posY_agora2 = %i \n\r", positionY_agora2); + printf( "posX_agora2 = %i \n\r", positionX_agora2); + printf( "posZ_agora2 = %i \n\r", positionZ_agora2); + wait_ms(DELAY_2); + dif_posX2 = position2X - positionX_agora2; + dif_posY2 = position2Y - positionY_agora2; + dif_posZ2 = position2Z - positionZ_agora2; + + if (dif_posX2 >0){ + motor2->move(StepperMotor::FWD, correcao_passo(dif_posX2)); + motor2->wait_while_active(); + } + if (dif_posX2 <0){ + dif_posX2 = -dif_posX2; + motor2->move(StepperMotor::BWD, correcao_passo(dif_posX2)); + motor2->wait_while_active(); + } + if (dif_posY2 >0){ + motor1->move(StepperMotor::FWD, correcao_passo(dif_posY2)); motor1->wait_while_active(); - motor2->wait_while_active(); + } + if (dif_posY2 <0){ + dif_posY2 = -dif_posY2; + motor1->move(StepperMotor::BWD, correcao_passo(dif_posY2)); + motor1->wait_while_active(); + } + if (dif_posZ2 >0){ + motor3->move(StepperMotor::FWD, correcao_passo(dif_posZ2)); 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(); + if (dif_posZ2 <0){ + dif_posZ2 = -dif_posZ2; + motor3->move(StepperMotor::BWD, correcao_passo(dif_posZ2)); motor3->wait_while_active(); - wait_ms(DELAY_3); - motor1->go_to(position2Y); - motor2->go_to(position2X); - motor3->go_to(position2Z); + } + positionY_agora2 = motor1->get_position(); + positionX_agora2= motor2->get_position(); + positionZ_agora2 = motor3->get_position(); + printf( "posY_agora2 = %i \n\r", positionY_agora2); + printf( "posX_agora2 = %i \n\r", positionX_agora2); + printf( "posZ_agora2 = %i \n\r", positionZ_agora2); + } + + if (pos == 3) { + printf( "pos1Y = %i \n\r", position1Y); + printf( "pos1X = %i \n\r", position1X); + printf( "pos1Z = %i \n\r", position1Z); + positionY_agora1 = motor1->get_position(); + positionX_agora1= motor2->get_position(); + positionZ_agora1 = motor3->get_position(); + printf( "posY_agora1 = %i \n\r", positionY_agora1); + printf( "posX_agora1 = %i \n\r", positionX_agora1); + printf( "posZ_agora1 = %i \n\r", positionZ_agora1); + dif_posX1 = position1X - positionX_agora1; + dif_posY1 = position1Y - positionY_agora1; + dif_posZ1 = position1Z - positionZ_agora1; + + printf( "difY1 = %i \n\r", correcao_passo(dif_posY1)); + printf( "difX1 = %i \n\r", correcao_passo(dif_posX1)); + printf( "difZ1 = %i \n\r", correcao_passo(dif_posZ1)); + if (dif_posX1 >0){ + motor2->move(StepperMotor::FWD, correcao_passo(dif_posX1)); + motor2->wait_while_active(); + } + if (dif_posX1 <0){ + dif_posX1 = -dif_posX1; + motor2->move(StepperMotor::BWD, correcao_passo(dif_posX1)); + motor2->wait_while_active(); + } + if (dif_posY1 >0){ + motor1->move(StepperMotor::FWD, correcao_passo(dif_posY1)); motor1->wait_while_active(); - motor2->wait_while_active(); + } + if (dif_posY1 <0){ + dif_posY1 = -dif_posY1; + motor1->move(StepperMotor::BWD, correcao_passo(dif_posY1)); + motor1->wait_while_active(); + } + if (dif_posZ1 >0){ + motor3->move(StepperMotor::FWD, correcao_passo(dif_posZ1)); 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(); + } + if (dif_posZ1 <0){ + dif_posZ1 = -dif_posZ1; + motor3->move(StepperMotor::BWD, correcao_passo(dif_posZ1)); motor3->wait_while_active(); } + + positionY_agora2 = motor1->get_position(); + positionX_agora2= motor2->get_position(); + positionZ_agora2 = motor3->get_position(); + printf( "posY_agora2 = %i \n\r", positionY_agora2); + printf( "posX_agora2 = %i \n\r", positionX_agora2); + printf( "posZ_agora2 = %i \n\r", positionZ_agora2); + wait_ms(DELAY_2); + dif_posX2 = position2X - positionX_agora2; + dif_posY2 = position2Y - positionY_agora2; + dif_posZ2 = position2Z - positionZ_agora2; + + if (dif_posX2 >0){ + motor2->move(StepperMotor::FWD, correcao_passo(dif_posX2)); + motor2->wait_while_active(); } + if (dif_posX2 <0){ + dif_posX2 = -dif_posX2; + motor2->move(StepperMotor::BWD, correcao_passo(dif_posX2)); + motor2->wait_while_active(); + } + if (dif_posY2 >0){ + motor1->move(StepperMotor::FWD, correcao_passo(dif_posY2)); + motor1->wait_while_active(); + } + if (dif_posY2 <0){ + dif_posY2 = -dif_posY2; + motor1->move(StepperMotor::BWD, correcao_passo(dif_posY2)); + motor1->wait_while_active(); + } + if (dif_posZ2 >0){ + motor3->move(StepperMotor::FWD, correcao_passo(dif_posZ2)); + motor3->wait_while_active(); + } + if (dif_posZ2 <0){ + dif_posZ2 = -dif_posZ2; + motor3->move(StepperMotor::BWD, correcao_passo(dif_posZ2)); + motor3->wait_while_active(); + } + positionY_agora2 = motor1->get_position(); + positionX_agora2= motor2->get_position(); + positionZ_agora2 = motor3->get_position(); + printf( "posY_agora2 = %i \n\r", positionY_agora2); + printf( "posX_agora2 = %i \n\r", positionX_agora2); + printf( "posZ_agora2 = %i \n\r", positionZ_agora2); + + + positionY_agora3 = motor1->get_position(); + positionX_agora3= motor2->get_position(); + positionZ_agora3 = motor3->get_position(); + printf( "posY_agora3 = %i \n\r", positionY_agora3); + printf( "posX_agora3 = %i \n\r", positionX_agora3); + printf( "posZ_agora3 = %i \n\r", positionZ_agora3); + wait_ms(DELAY_2); + dif_posX3 = position3X - positionX_agora3; + dif_posY3 = position3Y - positionY_agora3; + dif_posZ3 = position3Z - positionZ_agora3; + + if (dif_posX3 >0){ + motor2->move(StepperMotor::FWD, correcao_passo(dif_posX3)); + motor2->wait_while_active(); + } + if (dif_posX3 <0){ + dif_posX3 = -dif_posX3; + motor2->move(StepperMotor::BWD, correcao_passo(dif_posX3)); + motor2->wait_while_active(); + } + if (dif_posY3 >0){ + motor1->move(StepperMotor::FWD, correcao_passo(dif_posY3)); + motor1->wait_while_active(); + } + if (dif_posY3 <0){ + dif_posY3 = -dif_posY3; + motor1->move(StepperMotor::BWD, correcao_passo(dif_posY3)); + motor1->wait_while_active(); + } + if (dif_posZ3 >0){ + motor3->move(StepperMotor::FWD, correcao_passo(dif_posZ3)); + motor3->wait_while_active(); + } + if (dif_posZ3 <0){ + dif_posZ3 = -dif_posZ3; + motor3->move(StepperMotor::BWD, correcao_passo(dif_posZ3)); + motor3->wait_while_active(); + } + positionY_agora3 = motor1->get_position(); + positionX_agora3= motor2->get_position(); + positionZ_agora3 = motor3->get_position(); + printf( "posY_agora3 = %i \n\r", positionY_agora3); + printf( "posX_agora3 = %i \n\r", positionX_agora3); + printf( "posZ_agora3 = %i \n\r", positionZ_agora3); + } + } + } } -} \ No newline at end of file + +//}