ayio bukkka insadinsaofno
Dependencies: encoderKRAI mbed Motor_new
OE tadi udah di commit
main.cpp
- Committer:
- oktavianusirvan
- Date:
- 2019-05-09
- Revision:
- 0:ac8956bbab28
- Child:
- 1:7533e025de71
File content as of revision 0:ac8956bbab28:
// // #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 } }