ayio bukkka insadinsaofno
Dependencies: encoderKRAI mbed Motor_new
OE tadi udah di commit
Diff: main.cpp
- Revision:
- 2:710b092cfcc0
- Parent:
- 1:7533e025de71
- Child:
- 3:3ee76478b110
--- a/main.cpp Sun May 12 23:45:42 2019 +0000 +++ b/main.cpp Tue May 14 06:32:05 2019 +0000 @@ -5,6 +5,7 @@ #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 @@ -15,8 +16,8 @@ double pulse, total_pulse,total_pulse_in_degree,theta,pwm; double kp,ki,kd; bool not_stop ; -double error, integral, derivative; -double last_error ; +double galat, integral, derivative; +double last_galat ; void BacaEncoder(){// read encoder pulse = (double)encoder.getPulses(); @@ -25,12 +26,12 @@ total_pulse_in_degree = total_pulse*360/538; } -void PID(theta){ +void PID(double theta){ //pid - error = theta - total_pulse_in_degree; - derivative = error - last_error; + galat = theta - total_pulse_in_degree; + derivative = galat - last_galat; - pwm = (kp*error) + (kd*derivative); + pwm = (kp*galat) + (kd*derivative); //limit the power of motor, the max power can be changed if (pwm > 1) pwm = 1; @@ -38,13 +39,13 @@ } -void MoveMotor(theta){ +void MoveMotor(double theta){ not_stop = 1; - last_error = 0; + last_galat = 0; total_pulse =0; // move motor - last_error = theta; + last_galat = theta; while ((not_stop) && theta != 0){ BacaEncoder(); PID(theta); @@ -55,9 +56,10 @@ main_motor.brake(1); not_stop = 0; } - last_error = error; + last_galat = galat; } } +int t1; int main(){ count_ball = 0; @@ -68,7 +70,7 @@ state = 0 ; pneu = 1; // menembakkan bola count_ball++; - t1 = milis(); + t1 = clock_s(); // menggerakkan motor if (count_ball = 2) moveMotor(60);