ayio bukkka insadinsaofno
Dependencies: encoderKRAI mbed Motor_new
OE tadi udah di commit
Diff: main.cpp
- Revision:
- 6:e9ef196da46a
- Parent:
- 5:dee879b9ae82
- Child:
- 7:68af06a391f0
--- a/main.cpp Tue May 14 09:16:38 2019 +0000 +++ b/main.cpp Tue May 14 15:53:14 2019 +0000 @@ -7,10 +7,11 @@ #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 +Motor main_motor(PB_13, PB_14, PC_4); // input pin +DigitalOut pneu(PC_6);// input pin +DigitalIn infrared(PC_1); // input pin +encoderKRAI encoder(PC_10,PC_11,538,encoderKRAI::X4_ENCODING);// input pin +Serial pc(USBTX, USBRX,115200); int count, count_ball ; double pulse, total_pulse,total_pulse_in_degree,theta,pwm; @@ -18,6 +19,12 @@ bool not_stop ; double galat, integral, derivative; double last_galat ; +DigitalOut pneu1(PC_7); +DigitalOut pneu2(PC_8); +DigitalOut pneu3(PC_9); +DigitalOut pneu4(PC_6); +DigitalOut pneu5(PC_5); +DigitalOut pneu6(PB_6); void BacaEncoder(){// read encoder pulse = (double)encoder.getPulses(); @@ -31,7 +38,7 @@ galat = theta - total_pulse_in_degree; derivative = galat - last_galat; - pwm = (0.06*galat) + (3*derivative); + pwm = (0.006*galat) + (3*derivative); //limit the power of motor, the max power can be changed if (pwm > 0.9) pwm = 0.9; @@ -42,29 +49,45 @@ not_stop = 1; last_galat = theta; total_pulse =0; + integral = 0; // move motor last_galat = theta; while ((not_stop) && theta != 0){ BacaEncoder(); - PID(theta); + //PID(theta); + galat = theta - total_pulse_in_degree; + derivative = galat - last_galat; + integral += galat; + pwm = (0.007*galat) + (3*derivative) + (0.000007*integral) ; + + //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; // set speed motor - if ((pwm > 0.001) || (pwm < -0.001)) main_motor.speed(pwm); + if ((pwm > 0.000001) || (pwm < -0.000001)) main_motor.speed(pwm); else { main_motor.speed(0); main_motor.brake(1); - if (fabs(galat)<0.001) not_stop = 1; + if (fabs(pwm)<0.000001) not_stop = 1; } last_galat = galat; + pc.printf("%f.2 \n",total_pulse_in_degree); } } int t1; int main(){ - count_ball = 0; - int state = 1; // posisi awal while (1){ - main_motor.speed(0.5); + BacaEncoder(); + //pc.printf("%f.2 \n",total_pulse_in_degree); + MoveMotor(60); + pneu1=0; + pneu2=0; + pneu3=0; + pneu4=0; + pneu5=0; + pneu6=0; }