File content as of revision 0:ac8956bbab28:

//

//
#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
    }
}