ayio bukkka insadinsaofno
Dependencies: encoderKRAI mbed Motor_new
OE tadi udah di commit
main.cpp
- Committer:
- dwianiyulia
- Date:
- 2019-05-14
- Revision:
- 5:dee879b9ae82
- Parent:
- 4:8a50d972066d
- Child:
- 6:e9ef196da46a
File content as of revision 5:dee879b9ae82:
// // #include <stdio.h> #include <Motor.h> #include <encoderKRAI.h> #include <mbed.h> #include <stdint.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; bool not_stop ; double galat, integral, derivative; double last_galat ; void BacaEncoder(){// read encoder pulse = (double)encoder.getPulses(); encoder.reset(); total_pulse += pulse; total_pulse_in_degree = total_pulse*360/538; } void PID(double theta){ //pid galat = theta - total_pulse_in_degree; derivative = galat - last_galat; pwm = (0.06*galat) + (3*derivative); //limit the power of motor, the max power can be changed 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; total_pulse =0; // move motor last_galat = theta; while ((not_stop) && theta != 0){ BacaEncoder(); PID(theta); // set speed motor if ((pwm > 0.001) || (pwm < -0.001)) main_motor.speed(pwm); else { main_motor.speed(0); main_motor.brake(1); if (fabs(galat)<0.001) not_stop = 1; } last_galat = galat; } } int t1; int main(){ count_ball = 0; int state = 1; // posisi awal while (1){ main_motor.speed(0.5); } /*while (count_ball<6){ if (state && !infrared) { state = 0 ; pneu = 1; // menembakkan bola count_ball++; t1 = us_ticker_read(); // menggerakkan motor if (count_ball == 2) MoveMotor(60); else if (count_ball == 4) MoveMotor(90); } else { pneu = 0; if (us_ticker_read()-t1 >1000 ) state = 1; // tunggu pneumatik kembali } }*/ }