File content as of revision 6:e9ef196da46a:

//

//
#include <stdio.h>
#include <Motor.h>
#include <encoderKRAI.h>
#include <mbed.h>
#include <stdint.h>
// declare
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;
double kp,ki,kd;
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();
    encoder.reset();
    total_pulse += pulse;
    total_pulse_in_degree = total_pulse*360/538;
}

void PID(double theta){
        //pid
        galat = theta - total_pulse_in_degree;
        derivative = galat - last_galat;

        pwm =  (0.006*galat) + (3*derivative);

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

void MoveMotor(double theta){
    not_stop = 1;
    last_galat = theta;
    total_pulse =0;
    integral = 0;

    // move motor
    last_galat = theta;
    while ((not_stop) && theta != 0){
        BacaEncoder();
        //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.000001) || (pwm < -0.000001)) main_motor.speed(pwm);
        else {
            main_motor.speed(0);
            main_motor.brake(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(){
    while (1){
        BacaEncoder();
        //pc.printf("%f.2 \n",total_pulse_in_degree);
        MoveMotor(60);
        pneu1=0;
        pneu2=0;
        pneu3=0;
        pneu4=0;
        pneu5=0;
        pneu6=0;
    }
    
    
    /*while (count_ball<6){
        if (state && !infrared) {
            state = 0 ;
            pneu = 1;                   // menembakkan bola
            count_ball++;
            t1 = us_ticker_read();

            // menggerakkan motor
            if (count_ball == 2) MoveMotor(60);
            else if (count_ball == 4) MoveMotor(90);

        } else {
            pneu = 0;
            if (us_ticker_read()-t1 >1000 ) state = 1;   // tunggu pneumatik kembali
        }
    }*/
}