File content as of revision 2:0351727f6354:

//DEKLARASI LIBRARY
#include "mbed.h"
#include "millis.h"
#include "Motor.h"
#include "encoderKRAI.h"
#include "CMPS12_KRAI.h"
#include "hcsr04.h"

#define PULSE_TO_DEG 0.135
#define PWM_MAX 0.85
#define PWM_MIN -0.85
#define TOLERANCET 1.0
#define TS 7.0

CMPS12_KRAI cmps(PB_3, PB_10, 0xc0);
encoderKRAI encSteer(PC_2,PC_3, 538, encoderKRAI::X4_ENCODING);  //encoder motor steering
encoderKRAI encKanan(PB_9,PB_8, 538, encoderKRAI::X4_ENCODING);
encoderKRAI encKiri(PC_11,PC_10, 538, encoderKRAI::X4_ENCODING);
Motor motorSteer(PB_14, PC_4, PB_13);   //motor arah putaran CCW ++
Motor motorKa(PA_11,  PA_7,PB_12);                  //motor kanan
Motor motorKi(PB_15, PB_1, PB_2);                    //motor kiri
DigitalIn Ubutton(USER_BUTTON);
Serial pc(USBTX, USBRX, 115200);
DigitalIn switchKi(PA_9,PullUp);
DigitalIn switchKa(PD_2,PullUp);
DigitalOut pneu1[2] = {(PB_0),(PA_1)};          //kanan belakang , kiri depan
DigitalOut pneu2[2] = {(PC_1),(PA_0)};           //kiri belakang, kanan depan
HCSR04 pingKidep(PC_12, PA_13);                //TRIGGER , ECHO
HCSR04 pingKadep(PC_15, PC_14);
//HCSR04 pingKibel(PB_4, PA_8);
//HCSR04 pingKabel(PB_, PA_);



int pulsetempKi;
int pulsetempKa;
float theta;
float target;
int cp = 0;
int brake_state = 0;
float sum_theta_error;
float theta_error_prev;
float theta_target;
float speed;
float w;
float theta_sp;
float theta_error;
float heading_temp, heading0, heading;
double pulse = 0;
double KI;
int angkatKi;
int angkatKa;
int bouncingKi = 0;
int bouncingKa = 0;

const double KP = 0.05 ,
            KI1 = 0.00000,
            KI2 = 0.00000;
unsigned long lastParameter,    //millis utk perhintungan parameter
            last_mt_printPID,
            last_mt_print,  //millis utk print
            last_mt_button, //millis utk user button
            last_start, //millis start uji coba kuda
            lastFSM,    //finite state machine millis
            lastPWM,
            lastMotor,
            lastCompass,
            last_ping,
            lastThetaTarget;
int start;
int state=0;
float pwm;
float pwm_maju=0.0;
float temp = 0;
float dThetaMax = 35.0;
float speed_maju=0;
int ohstart = 0;
int belok =0;
int kidep_dist,kadep_dist, kibel_dist, kabel_dist;
int thetaKa;
int thetaKi;
int pulseLangkahKi;
int pulseLangkahKa;
void hitungParameter();
void getHeading();
void hitungPIDTheta(double theta_sp);
void gerakMotor();
void tambahPWM();
void debug();
void gerakDepan(int end);
void hitungOdometry();
void initPneu();
void angkatKakiKi();
void angkatKakiKa();
//int main(){
//  startMillis();
//  speed_maju=0.3;
//  heading0=(float)cmps.getAngle()/10;
//  while(1){
//      debug();
//      //pc.printf("TEST\n");
//  }
//}


int main(){
    encSteer.reset();
    encKanan.reset();
    encKiri.reset();
    startMillis();
    theta = 0;
    cp = 1;
    state=0;
    theta_sp =0;
    ohstart=0;
    KI = KI1;
    angkatKi = 0;
    angkatKa = 0;
    int kanan;
    int kiri;
    initPneu();
    
    while(1){
        if (millis()- last_mt_print > 15){
            //pc.printf("%.2f\n",speed_maju);
            if(!switchKa){
                kanan = 1; 
                pc.printf("KENA Kanan\t");
            }
            else{kanan = 0;}
            if(!switchKi){
                kiri = 1;
                pc.printf("KENA Kiri\t"); 
            }   
            else{kiri = 0;}
            last_mt_print = millis();
            pc.printf("%d %d s = %.2f puls = %d %d \t%d %d \n",kiri,kanan,speed_maju, pulsetempKi, pulsetempKa,pingKidep.distance(),pingKadep.distance());
        }
        if(!Ubutton){   //mulai gerak maju
            wait_ms(2000);  //diam 2 detik sebelum mulai
            ohstart = 1;
            state=1;
            pwm_maju = 0.8;
            speed_maju=0.7;
            last_start=millis();
        }

       //state machine
        if (millis() - last_start>15000) { 
            ohstart = 0;    //kembali ke posisi awal (mati) setelah 15 detik
        }
        if (((kidep_dist < 150)||(kadep_dist < 150)) && (state == 1)) { 
            //state = 2;      //kuda mendeteksi tembok di kanan atau kiri, siap untuk belok
        }
        if (((kidep_dist < 30) || (kadep_dist < 30)) && (state == 2)) {
            state = 3;      //kuda deteksi obstacle, siap untuk angkat kaki
        }
        /*if (kondisi cek sudah lewat obstacle dan udh mau belok lagi) {
            state = 4;      //kuda belok lagi, siap untuk deteksi tali dan angkat kaki
        }*/
        /*if (kondisi deteksi tali) {
            state 5;        //angkat kaki
        }*/
        /*if (switch nanjak dipencet) {
            state 6;        //jalan lurus nanjak
        }*/  

        if (ohstart){
            angkatKakiKi();
            //angkatKakiKa();
            if (millis() - lastFSM > 9){
                if (state==1){  //state maju lurus
                    theta_sp=0;
                    belok=0;
                    pwm_maju=0.80;
                }
                else if (state == 2) {    //state belok, sudah deteksi tembok 
                    //theta_target = 15.0;
                    //ubahTheta();
                    //belok = 1;
                    //KI = KI2;
                    //pwm_maju = 0.85;
                }
                else if (state == 3) { //state angkat kaki, udh deteksi obstacle
                    //kuda deteksi obstacle, angkat kaki
                }
                /*else if (state == 4) { //state belok lagi, udh lewat sand dune
                    //kuda belok lagi, 
                }*/
                /*else if (state == 5) { //state angkat kaki, deteksi tali
                    //kuda deteksi tali, angkat kaki
                }*/
                /*else if (state == 6) { //state naik gunung
                    //jalan lurus nanjak
                }*/
                lastFSM=millis();
            }
            //urutan prioritas
            if (millis() - last_mt_printPID > 7){
                if (!(fabs(theta_sp- theta) < TOLERANCET)){
                    hitungPIDTheta(theta_sp);
                }
                last_mt_printPID = millis();
            }
            if (millis() - lastParameter > 5){
                hitungParameter();
                lastParameter = millis();
            } 
            if (millis() - lastMotor>4){
                gerakMotor();
                lastMotor=millis();
            }
            if (millis() - lastCompass>3) {
                getHeading();
                lastCompass = millis();
            }
            if (millis() - last_ping > 9) {
                kidep_dist = pingKidep.distance();
                kadep_dist = pingKadep.distance();
                //belki_dist = pingKibel.distance();
                //belka_dist = pingKabel.distance();
                last_ping = millis();
            } 
        }

    }

}

void hitungParameter(){
    pulse = (double)encSteer.getPulses()*PULSE_TO_DEG;
    encSteer.reset();
    pulseLangkahKi = (int)encKiri.getPulses();
    pulseLangkahKa = (int)encKanan.getPulses();
    encKiri.reset();
    encKanan.reset();
    pulsetempKi += pulseLangkahKi;
    pulsetempKa += pulseLangkahKa;
    theta += pulse;
}
void initPneu(){
    pneu1[0]=0;
    pneu1[1]=0;
    pneu2[0]=0;
    pneu2[1]=0;
}

void angkatKakiKi(){
    int batasKi = 15;
    if(!switchKi && bouncingKi == 0){
        bouncingKi = 1;
        pulsetempKi = 0;
    }
    if (pulsetempKi > batasKi && pulsetempKi < (batasKi + 95) && !angkatKi){
        pneu2[0] = 1;    //blkg
        pneu2[1] = 1;    //dpn
        angkatKi = 1;
    }
    else if (pulsetempKi > (batasKi+120) && angkatKi){
        pneu2[0] = 0;
        pneu2[1] = 0;
        angkatKi = 0;
        bouncingKi = 0;
    }
}

void angkatKakiKa(){
    int batasKa = 0;
    if(!switchKa && bouncingKa == 0){
        bouncingKa = 1;
        pulsetempKa = 0;
    }
    if (pulsetempKa > batasKa && pulsetempKa < (batasKa+130) && !angkatKa){
        pneu1[0] = 1;
        pneu1[1] = 1;
        angkatKa = 1;
    }
    else if (pulsetempKa > (batasKa+130) && angkatKa){
        pneu1[0] = 0;
        pneu1[1] = 0;
        angkatKa = 0;
        bouncingKa = 0;
    }
}

void getHeading() {
    heading_temp = (cmps.getAngle()/10 - heading0);                //cari perubahan sudut badan
    if(heading_temp > 180.0 && heading_temp <= 360.0)             //kondisi agar pembacaan 0 - 180 derajat
        heading = (heading_temp - 360.0 );
    else if(heading_temp < -180.0 && heading_temp >= -360.0)
        heading = (heading_temp  + 360.0 );
    else
        heading = heading_temp;
    heading = heading*(-1);                                   //output heading
}
void hitungPIDTheta(double theta_sp){
    if (theta_sp>dThetaMax)
        theta_sp=dThetaMax;
    theta_error = theta_sp - theta;
    if (theta_error > 180){
        theta_error = theta_error - 360;
    }
    else if (theta_error < -179){
        theta_error = theta_error + 360;
    }

    sum_theta_error += (theta_error);
    w =KP * theta_error + KI *sum_theta_error*TS;
    speed = w;
    theta_error_prev = theta_error;

    if(speed >= PWM_MAX){
        pwm = PWM_MAX;
    } else if (speed <= PWM_MIN){
        pwm = PWM_MIN;
    } else
        pwm = speed;
}
void gerakMotor(){
    //if ((theta_error > 1.0 && (fabs(pwm) > 0.15)))// && (state!=0)))
    motorSteer.speed(pwm);
//  else {
//        motorSteer.speed(0.0);
//        motorSteer.brake(BRAKE_HIGH);
//    }

    tambahPWM();
    if ( (state>0) && (fabs(speed_maju) > 0.15) && (ohstart)){
        motorKa.speed(speed_maju);
        motorKi.speed(speed_maju);
    } else {
        motorKa.speed(0);
        motorKi.speed(0);
        motorKa.brake(1);
        motorKi.brake(1);
    }
}

void tambahPWM(){
    if (millis() - lastPWM>50){
        if (speed_maju>pwm_maju){
//            speed_maju -=0.05;
        }
        else if (speed_maju<pwm_maju){
                speed_maju+=0.05;
        } 
    }
    lastPWM = millis();
}
void debug(){   //program utk cek konstanta derajat
    theta = encSteer.getPulses()*PULSE_TO_DEG;
//    if (!Ubutton)
//      motorKa.speed(0.8);
//      motorKi.speed(0.8);
//    else
//      motorKa.speed(0);
//      motorKi.speed(0);
//    if (temp>20.0){
//      belok =  1;
//    }
//    if (temp<-20.0){
//      belok=0;
//    }
//   if (millis() - lastCompass>3){
//      getHeading();
//      lastCompass=millis();
//  }
////    if (!belok)
////        motorSteer.speed(speed_maju);
////    else
////        motorSteer.speed(-1*speed_maju);
//   if (!Ubutton)
//     theta_sp=20.0;
//   else
//     theta_sp=-20.0;
//
//  if (millis() - last_mt_printPID > 7){
//      if (!(fabs(theta_sp- theta) < TOLERANCET)){
//          hitungPIDTheta(theta_sp);
//      }
//      last_mt_printPID = millis();
//  }
//  gerakMotor();
//  //motorSteer.speed(pwm);
//    printf("pulse = %.2f\t theta = %.2f theta = %.2f pidT=%.2f pwm=%.2f\n",
//          theta, heading, theta_error, w,pwm);
}

void ubahTheta() {
    if (millis() - lastThetaTarget >23) {
        if (theta_sp < theta_target) {
            theta_sp += 1.0;
        }
        else if (theta_sp > theta_target) {
            theta_sp -= 1.0;
        }
        lastThetaTarget = millis();
    }
}

void hitungOdometry(){
    //Tolong isi program odometry utk pisisi robot terhadap lapangan
    //berdasarkan geometry robot dan posisi ping (asumsi ping diletakan di tengah kaki bagian luar
    //ukuran robot minta ama mekanik
    /*  POSISI PING utk di kaki depan
     PING            PING
     ####            ####
    #    #          #    #
    #    #          #    #
PING#    #          #    # PING
    #    #          #    #
    #    #          #    #
     ####            ####
    */
}