ayio bukkka insadinsaofno
Dependencies: encoderKRAI mbed Motor_new
OE tadi udah di commit
Diff: main.cpp
- Revision:
- 0:ac8956bbab28
- Child:
- 1:7533e025de71
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Thu May 09 10:27:51 2019 +0000 @@ -0,0 +1,79 @@ +// + +// +#include <stdio.h> +#include <Motor.h> +#include <encoderKRAI.h> +#include <mbed.h> +// declare +Motor main_motor(PB_13, PB_14, PB_15 ); // input pin +DigitalOut pneu(PC_5);// input pin +DigitalIn infrared(PC_5); // input pin +encoderKRAI encoder(PB_1,PB_2,538,encoderKRAI::X4_ENCODING);// input pin + +int count, count_ball ; +double pulse, total_pulse,total_pulse_in_degree,theta,pwm; +double kp,ki,kd; + +void BacaEncoder(){// read encoder + pulse = (double)encoder.getPulses(); + encoder.reset(); + total_pulse +=pulse; + total_pulse_in_degree = total_pulse*360/538; +} + +void MoveMotor(int count_ball){ + bool not_stop = 1; + double error, integral, derivative; + double last_error = 0; + total_pulse =0; + + // take max theta + if (count_ball % 2 == 0){ + if (count_ball == 2) theta = 60; + else if (count_ball == 4) theta = 90; + else theta = 0; + } + + // move motor + while ((not_stop) && theta != 0){ + BacaEncoder(); + + //pid + error = theta - total_pulse_in_degree; + integral = integral + error; + derivative = error - last_error; + + pwm = (kp*error) + (ki*integral) + (kd*derivative); + + //limit the power of motor, the max power can be changed + if (pwm > 255) pwm = 255; + else if (pwm < -255) pwm = -255; + + // set speed motor + if (pwm > 0) main_motor._fwd(pwm); + else if (pwm < 0) main_motor._rev(pwm); + else { + main_motor.speed(0); + main_motor.brake(1); + not_stop = 0; + } + last_error = 0; + } + +} + +int main(){ + count_ball = 0 ; + + while (count_ball < 6 ) { // eop if all ball has been throwen + if (infrared ){ + pneu = 1; + milis(500); + pneu = 0; + count_ball +=1; // increment of ball + } + moveMotor(count_ball); + milis(500); // adjust with try + } +} \ No newline at end of file