Diff: main.cpp

Revision:

0:ac8956bbab28

Child:

1:7533e025de71

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu May 09 10:27:51 2019 +0000
@@ -0,0 +1,79 @@
+//
+
+//
+#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
+    }
+}
\ No newline at end of file