on Motor
Dependencies: encoderKRAI mbed Motor_new
Diff: main.cpp
- Revision:
- 5:dee879b9ae82
- Parent:
- 4:8a50d972066d
- Child:
- 6:e9ef196da46a
diff -r 8a50d972066d -r dee879b9ae82 main.cpp --- a/main.cpp Tue May 14 08:37:02 2019 +0000 +++ b/main.cpp Tue May 14 09:16:38 2019 +0000 @@ -22,7 +22,7 @@ void BacaEncoder(){// read encoder pulse = (double)encoder.getPulses(); encoder.reset(); - total_pulse +=pulse; + total_pulse += pulse; total_pulse_in_degree = total_pulse*360/538; } @@ -34,11 +34,10 @@ pwm = (0.06*galat) + (3*derivative); //limit the power of motor, the max power can be changed - if (pwm > 1) pwm = 1; - else if (pwm < -1) pwm = -1; + if (pwm > 0.9) pwm = 0.9; + else if (pwm < -0.9) pwm = -0.9; +} - -} void MoveMotor(double theta){ not_stop = 1; last_galat = theta; @@ -46,17 +45,15 @@ // move motor last_galat = theta; - BacaEncoder(); - PID(theta); while ((not_stop) && theta != 0){ BacaEncoder(); PID(theta); // set speed motor - if ((pwm > 0.001) || (pwm < -0.001)) main_motor.speed(pwm); + if ((pwm > 0.001) || (pwm < -0.001)) main_motor.speed(pwm); else { main_motor.speed(0); main_motor.brake(1); - not_stop = 0; + if (fabs(galat)<0.001) not_stop = 1; } last_galat = galat; } @@ -66,10 +63,11 @@ int main(){ count_ball = 0; int state = 1; // posisi awal - while (count_ball != 2){ - move_motor(60); - count_ball++; + while (1){ + main_motor.speed(0.5); } + + /*while (count_ball<6){ if (state && !infrared) { state = 0 ;