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);