2 Motores + Joystick
Dependencies: X_NUCLEO_IHM01A1 TextLCD
Fork of HelloWorld_IHM01A1_2Motors_mbedOS by
Diff: main.cpp
- Revision:
- 45:8fab21ab1251
- Parent:
- 44:eecdc0b60f14
- Child:
- 46:63b15a56cbe5
--- a/main.cpp Wed May 16 15:40:31 2018 +0000 +++ b/main.cpp Wed May 16 18:32:22 2018 +0000 @@ -191,8 +191,11 @@ */ int correcao_passo (int passo){ int passo_verdade; + if (passo != 0){ passo_verdade = passo/1.0892 - 15,56; + } return (passo_verdade); + } int main() @@ -438,31 +441,31 @@ 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){ + if (correcao_passo(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)); + if (correcao_passo(dif_posX1) <0){ + // correcao_passo(dif_posX1) = -correcao_passo(dif_posX1); + motor2->move(StepperMotor::BWD, -correcao_passo(dif_posX1)); motor2->wait_while_active(); } - if (dif_posY1 >0){ + if (correcao_passo(dif_posY1) >0){ motor1->move(StepperMotor::FWD, correcao_passo(dif_posY1)); motor1->wait_while_active(); } - if (dif_posY1 <0){ - dif_posY1 = -dif_posY1; - motor1->move(StepperMotor::BWD, correcao_passo(dif_posY1)); + if (correcao_passo(dif_posY1) <0){ + //correcao_passo(dif_posY1) = -correcao_passo(dif_posY1); + motor1->move(StepperMotor::BWD, -correcao_passo(dif_posY1)); motor1->wait_while_active(); } - if (dif_posZ1 >0){ + if (correcao_passo(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)); + if (correcao_passo(dif_posZ1) <0){ + //correcao_passo(dif_posZ1) = -correcao_passo(dif_posZ1); + motor3->move(StepperMotor::BWD, -correcao_passo(dif_posZ1)); motor3->wait_while_active(); } @@ -490,31 +493,31 @@ 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){ + if (correcao_passo(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)); + if (correcao_passo(dif_posX1) <0){ + //correcao_passo(dif_posX1) = -correcao_passo(dif_posX1); + motor2->move(StepperMotor::BWD, -correcao_passo(dif_posX1)); motor2->wait_while_active(); } - if (dif_posY1 >0){ + if (correcao_passo(dif_posY1) >0){ motor1->move(StepperMotor::FWD, correcao_passo(dif_posY1)); motor1->wait_while_active(); } - if (dif_posY1 <0){ - dif_posY1 = -dif_posY1; - motor1->move(StepperMotor::BWD, correcao_passo(dif_posY1)); + if (correcao_passo(dif_posY1) <0){ + //correcao_passo(dif_posY1) = -correcao_passo(dif_posY1); + motor1->move(StepperMotor::BWD, -correcao_passo(dif_posY1)); motor1->wait_while_active(); } - if (dif_posZ1 >0){ + if (correcao_passo(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)); + if (correcao_passo(dif_posZ1) <0){ + //correcao_passo(dif_posZ1) = -correcao_passo(dif_posZ1); + motor3->move(StepperMotor::BWD, -correcao_passo(dif_posZ1)); motor3->wait_while_active(); } @@ -527,33 +530,42 @@ wait_ms(DELAY_2); dif_posX2 = position2X - positionX_agora2; dif_posY2 = position2Y - positionY_agora2; - dif_posZ2 = position2Z - positionZ_agora2; + dif_posZ2 = position2Z - positionZ_agora2; + printf( "difY2 = %i \n\r", correcao_passo(dif_posY2)); + printf( "difX2 = %i \n\r", correcao_passo(dif_posX2)); + printf( "difZ2 = %i \n\r", correcao_passo(dif_posZ2)); - if (dif_posX2 >0){ + if (correcao_passo(dif_posX2) >0){ + printf( "fwd difX2 = %i \n\r", correcao_passo(dif_posX2)); 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)); + if (correcao_passo(dif_posX2) <0){ + //correcao_passo(dif_posX2) = -correcao_passo(dif_posX2); + printf( "bwd difX2 = %i \n\r", correcao_passo(dif_posX2)); + motor2->move(StepperMotor::BWD, -correcao_passo(dif_posX2)); motor2->wait_while_active(); } - if (dif_posY2 >0){ + if (correcao_passo(dif_posY2) >0){ + printf( "fwd difY2 = %i \n\r", correcao_passo(dif_posY2)); 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)); + if (correcao_passo(dif_posY2) <0){ + //correcao_passo(dif_posY2) = -correcao_passo(dif_posY2); + printf( "bwd difY2 = %i \n\r", correcao_passo(dif_posY2)); + motor1->move(StepperMotor::BWD, -correcao_passo(dif_posY2)); motor1->wait_while_active(); } - if (dif_posZ2 >0){ + if (correcao_passo(dif_posZ2) >0){ + printf( "fwd difZ2 = %i \n\r", correcao_passo(dif_posZ2)); 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)); + if (correcao_passo(dif_posZ2) <0){ + //correcao_passo(dif_posZ2) = -correcao_passo(dif_posZ2); + printf( "bwd difZ2 = %i \n\r", correcao_passo(dif_posZ2)); + motor3->move(StepperMotor::BWD, -correcao_passo(dif_posZ2)); motor3->wait_while_active(); } positionY_agora2 = motor1->get_position(); @@ -581,31 +593,31 @@ 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){ + if (correcao_passo(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)); + if (correcao_passo(dif_posX1) <0){ + //correcao_passo(dif_posX1) = -correcao_passo(dif_posX1); + motor2->move(StepperMotor::BWD, -correcao_passo(dif_posX1)); motor2->wait_while_active(); } - if (dif_posY1 >0){ + if (correcao_passo(dif_posY1) >0){ motor1->move(StepperMotor::FWD, correcao_passo(dif_posY1)); motor1->wait_while_active(); } - if (dif_posY1 <0){ - dif_posY1 = -dif_posY1; - motor1->move(StepperMotor::BWD, correcao_passo(dif_posY1)); + if (correcao_passo(dif_posY1) <0){ + //correcao_passo(dif_posY1) = -correcao_passo(dif_posY1); + motor1->move(StepperMotor::BWD, -correcao_passo(dif_posY1)); motor1->wait_while_active(); } - if (dif_posZ1 >0){ + if (correcao_passo(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)); + if (correcao_passo(dif_posZ1) <0){ + //correcao_passo(dif_posZ1) = -correcao_passo(dif_posZ1); + motor3->move(StepperMotor::BWD, -correcao_passo(dif_posZ1)); motor3->wait_while_active(); } @@ -620,31 +632,31 @@ dif_posY2 = position2Y - positionY_agora2; dif_posZ2 = position2Z - positionZ_agora2; - if (dif_posX2 >0){ + if (correcao_passo(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)); + if (correcao_passo(dif_posX2) <0){ + //correcao_passo(dif_posX2) = -correcao_passo(dif_posX2); + motor2->move(StepperMotor::BWD, -correcao_passo(dif_posX2)); motor2->wait_while_active(); } - if (dif_posY2 >0){ + if (correcao_passo(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)); + if (correcao_passo(dif_posY2) <0){ + //correcao_passo(dif_posY2) = -correcao_passo(dif_posY2); + motor1->move(StepperMotor::BWD, -correcao_passo(dif_posY2)); motor1->wait_while_active(); } - if (dif_posZ2 >0){ + if (correcao_passo(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)); + if (correcao_passo(dif_posZ2) <0){ + //correcao_passo(dif_posZ2) = -correcao_passo(dif_posZ2); + motor3->move(StepperMotor::BWD, -correcao_passo(dif_posZ2)); motor3->wait_while_active(); } positionY_agora2 = motor1->get_position(); @@ -666,31 +678,31 @@ dif_posY3 = position3Y - positionY_agora3; dif_posZ3 = position3Z - positionZ_agora3; - if (dif_posX3 >0){ + if (correcao_passo(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)); + if (correcao_passo(dif_posX3) <0){ + //correcao_passo(dif_posX3) = -correcao_passo(dif_posX3); + motor2->move(StepperMotor::BWD, -correcao_passo(dif_posX3)); motor2->wait_while_active(); } - if (dif_posY3 >0){ + if (correcao_passo(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)); + if (correcao_passo(dif_posY3) <0){ + //correcao_passo(dif_posY3) = -correcao_passo(dif_posY3); + motor1->move(StepperMotor::BWD, -correcao_passo(dif_posY3)); motor1->wait_while_active(); } - if (dif_posZ3 >0){ + if (correcao_passo(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)); + if (correcao_passo(dif_posZ3) <0){ + //correcao_passo(dif_posZ3) = -correcao_passo(dif_posZ3); + motor3->move(StepperMotor::BWD, -correcao_passo(dif_posZ3)); motor3->wait_while_active(); } positionY_agora3 = motor1->get_position();