File content as of revision 3:8f57f69f6e64:

//DEKLARASI LIBRARY
#include "mbed.h"
#include "millis.h"
#include "Motor.h"
#include "encoderKRAI.h"
#include "CMPS12_KRAI.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

#define Kp_A 0.06//0.078
#define Ki_A 8.554e-5
#define Kd_A 0
DigitalIn Ubutton(USER_BUTTON);
Serial pc(USBTX, USBRX, 115200);
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 motorKi(PA_11,  PB_12,PA_7);                  //motor kanan
Motor motorKa(PA_5, PA_12, PA_6);                    //motor kiri

DigitalIn switchKi(PA_9,PullUp);
DigitalIn switchKa(PD_2,PullUp);
DigitalOut pneu1[2] = {(PC_1),(PA_1)};          //kanan belakang , kiri depan
DigitalOut pneu2[2] = {(PB_0),(PA_0)};           //kiri belakang, kanan depan
int pulsetemp;
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 angkat;
int bouncing = 0;
double pulseA, pulse_B;
const double KP = 0.05,
             KI1 = 0.00000,
             KI2 = 0.00000;
unsigned long lastParameter,    //millis utk perhintungan parameter
         last_mt_printPID,
         last_mt_print=0.0,  //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,
         lastThetaTarget,
         lastSafety,
         lastAngkat;
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 thetaKa;
int thetaKi;
int pulseLangkah;
double pulse_A, c_e_A, p_e_A,
        p2_e_A, prev_speedA, speedA;
double target_pulse=0;
int bouncingKi, bouncingKa,  angkatKa, angkatKi;
double pulsetempKa, pulsetempKi;
int kiri, kanan;
unsigned long lastStall;
int stall,langkah;

double pulseAngkatA=75.0,       //kanan belakang
    pulseAngkatB=95.0,  //87    kiri depan
    pulseAngkatC= 465.0,    //97    kiri belakang
    pulseAngkatD=580.0, //565   //kanan depan
    pulseTurunA=287.0,          //kanan belakang
    pulseTurunB=277.0, //305    kiri depan
    pulseTurunC=700.0,          //kiri belakang
    pulseTurunD=760; //785      kanan depan
int pulseKi, pulseKa;
int angkatA=0, angkatB=0, angkatC=0,angkatD=0;
int mulai = 0;

void PIDMotor();
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();
void FSM();
void angkatKaki();
//int main(){
//  while(1){
//      if (!Ubutton){
//          motorKa.speed(0.4);
//          motorKi.speed(0.4);
//      } else {
//          motorKa.speed(0.0);
//          motorKi.speed(0.0);
//      }
//  }
//}
int main() {
    pulseKi=0;
    pulseKa=0;
    speedA =0;
    encSteer.reset();
    encKanan.reset();
    encKiri.reset();
    startMillis();
    theta = 0;
    cp = 1;
    state=0;
    theta_sp =0;
    ohstart=0;
    KI = KI1;

    angkatKi = 0;
    angkatKa = 0;
    initPneu();
    kanan=0;
    mulai=0;
    while(1) {
//      pc.printf("TES");
//      ohstart=1;
//      if (millis() - last_mt_print>17) {
//          pc.printf("TES %d - %.2f %.2f  %.2f\n",ohstart,pulse_A,pulse_B, speedA);//encKanan.getPulses(), encKiri.getPulses());
//          last_mt_print=millis();
//      }
//      if (millis()>=10300)
//          ohstart = 0;
//      if(!Ubutton && millis() - last_mt_button>1000) {  //mulai gerak maju
//          pulse_A +=0.3;
//          last_mt_button=millis();
//      }
//        if(!Ubutton) {  //mulai gerak maju
//            wait_ms(2000);  //diam 2 detik sebelum mulai
//            ohstart = 1;
//            state=1;
//            last_start=millis();
//        }
//
//        if (millis() - last_start>15000) { //maksimal waktu kuda gerak 17 detik
//            ohstart=0;
//        }
//
//        if (ohstart) {
//
//
//        }
        if (millis()- last_mt_print > 17){
            //pc.printf("%.2f\n",speed_maju);
            if(!switchKa){
                kanan = 1;

//              pc.printf("KENA Kanan\t");
            }
            else{
                kanan = 0;
                bouncingKa = 0;
            }
            if(!switchKi){
                kiri = 1;
//              pc.printf("KENA Kiri\t");
            }
            else{
                kiri = 0;
                bouncingKi = 0;
            }
            last_mt_print = millis();
            pc.printf("%d %d s = %d\t%d\t %d%d%d%d \n",
                    kanan, ohstart,
                    pulseKa, pulseKi, angkatA,
                    angkatB, angkatC, mulai);//kanan,speedA, pulseKi, pulseKa);//,pingKidep.distance(),pingKadep.distance());
        }
        if (millis() - lastAngkat>11){
                    angkatKaki();
                    lastAngkat=millis();
                }
        if (millis()>=28300)
            ohstart = 0;
        if(!Ubutton){   //mulai gerak maju
            wait_ms(2000);  //diam 2 detik sebelum mulai
            ohstart = 1;
            state=1;
            stall=0;
            lastStall=millis();
            //pwm_maju = 0.8;
            //speed_maju=0.7;
            target_pulse = 5.0;
            last_start=millis();
        }
//      urutan prioritas
//      if (millis() - last_mt_printPID > 7){
//          if (!(fabs(theta_sp- theta) < TOLERANCET)){
//              hitungPIDTheta(theta_sp);
//          }
//          last_mt_printPID = millis();
//      }
//
//      if (millis() - lastCompass>3) {
//          getHeading();
//          lastCompass = millis();
//      }

       //state machine
//      if (millis() - last_start>7000) {
//          ohstart = 0;    //kembali ke posisi awal (mati) setelah 15 detik
//          target_pulse=0;
//      }
//      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 (millis() - lastParameter > 5){
            hitungParameter();
            PIDMotor();
            lastParameter = millis();
        }

        if (ohstart){
            if (millis() - lastFSM > 9){
                FSM();
                lastFSM=millis();
            }
            if (millis() - lastMotor>2){
                gerakMotor();
                lastMotor=millis();
            }

//          angkatKakiKi();
//          angkatKakiKa();
//
//          if (millis() - last_ping > 9) {
//              kidep_dist = pingKidep.distance();
//              kadep_dist = pingKadep.distance();
//              //belki_dist = pingKibel.distance();
//              //belka_dist = pingKabel.distance();
//              last_ping = millis();
//          }
        }
        else {
            target_pulse=0;
            motorKa.speed(0);
            motorKi.speed(0);
//          motorKa.brake(1);
//          motorKi.brake(1);
        }
        //fungsi untuk safety dari stall yang terlalu lama
        if (millis() - lastSafety>19){
            if (pulse_A<=0.01 && !stall){
                lastStall=millis();
                stall=1;
            }
            if (pulse_A>0.3 && stall){
                lastStall=millis();
                stall=0;
            }
            if (millis() - lastStall>3000 && stall){
                ohstart=0;
                target_pulse=0;
            }
        }

        //fungsi angkatKaki
        //if (millis() - last_start>5000)
        if (mulai>3){
//          pneu1[1]=1;
//          pneu2[1]=1;
            angkatKaki();
        }else{
            pneu1[0]=0;
            pneu1[1]=0;
            pneu2[0]=0;
            pneu2[1]=0;
        }
    }

}

void FSM(){
    if (state==1){  //state maju lurus
        theta_sp=0;
        belok=0;
        //pwm_maju=0.80;
        target_pulse = 3;
    }
    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
    }*/
}
void hitungParameter(){
    pulse = (double)encSteer.getPulses()*PULSE_TO_DEG;
    encSteer.reset();
    theta += pulse;
}
void gerakMotor()
{
//  state=1;
//  if (!Ubutton){
//      speedA = 0.78;
//  }
//  else
//      speedA = 0.0;
//  speedA=1.0;
    if (speedA > 0.15 && (ohstart)) {
//  if (ohstart){
        motorKa.speed(speedA);
        motorKi.speed(speedA);
    } else {
        motorKa.speed(0);
        motorKi.speed(0);
//        motorKa.brake(1);
//        motorKi.brake(1);
    }
}

void PIDMotor(){
        //target_pulse = 10.0;
        pulse_A = (double)encKiri.getPulses();
        //pulse_B = (double)encKanan.getPulses();
        //encKanan.reset();
        encKiri.reset();
        pulseKi += (int)pulse_A;
        pulseKa += (int)pulse_A;
//        pulseKa = pulseKa%360;
//        pulseKi = pulseKa%360;

//        if (millis()-lastStall>2000){
//          ohstart=0;
//          speedA=0;
//          target_pulse=0;
//          state=0;
//        }

        c_e_A = (double) target_pulse - pulse_A;
//         thetaKa += pulse_A;
        speedA = (double) ( prev_speedA +
                Kp_A*(c_e_A  - p_e_A) +
                Ki_A*TS*(c_e_A + p_e_A)/2 +
                Kd_A*(c_e_A - 2*p_e_A + p2_e_A) );
        if (speedA>0.90)
            speedA = 0.90;
        else if (speed<-0.90)
            speedA = -0.90;
        if (!ohstart)
            speedA=0.0;
        prev_speedA = speedA;
        p2_e_A = p_e_A;
        p_e_A = c_e_A;
}
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 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 angkatKaki(){
    //1 putaran 650 pulse
//  pulseKa=pulseKa%890;
//  pulseKi=pulseKi%650;

    if(kiri && bouncingKi == 0){
        bouncingKi = 1;
//      pulseKi = 0;
//      pulseKa = 325;
    }
    if (pulseKa>900){
            mulai++;
            pulseKa=0;
        }
    else if(kanan && bouncingKa == 0){
        bouncingKa = 1;
        pulseKi = 325;
        pulseKa = 0;
        mulai++;
    }
//  //if (millis()>9000){
////    if (pulseKi > pulseAngkatA && pulseKi < pulseTurunA && !angkatA){
////        pneu1[0] = 1;       //blkg
////        angkatA = 1;
////        bouncingKi = 0;
////    }
////    else if (pulseKi>pulseTurunA && angkatA){
////        pneu1[0] = 0;
////        angkatA = 0;
////    }
//      if (pulseKa > pulseAngkatB && pulseKa < pulseTurunB ){
//          pneu1[0] = 0;       //kiri dpn
//          angkatB = 0;
//      }
//      else if (pulseKa < pulseAngkatB || pulseKa>pulseTurunB ){
//          pneu1[0] = 1;       //kiri dpn
//          angkatB = 1;
//      }
////    if (pulseKa > pulseAngkatC && pulseKa < pulseTurunC && !angkatC){
////        pneu2[0] = 1;       //blkg
////        angkatC = 1;
////    }
////    else if (pulseKa>pulseTurunC && angkatC){
////        pneu2[0] = 0;
////        angkatC = 0;
////        bouncingKa = 0;
////    }
//      if (pulseKa > pulseAngkatD && pulseKa < pulseTurunD){
//          pneu2[0] = 0;       //kanan dpn
//          angkatD = 0;
//      }
//      else if (pulseKa < pulseAngkatD || pulseKa>pulseTurunD){
//          pneu2[0] = 1;       //kanan dpn
//          angkatD = 1;
//
//      }
////    if (pulseKi > pulseAngkatA && pulseKi < pulseTurunA && !angkatA){
////        pneu1[0] = 1;       //blkg
////        angkatA = 1;
////        bouncingKi = 0;
////    }
////    else if (pulseKi>pulseTurunA && angkatA){
////        pneu1[0] = 0;
////        angkatA = 0;
////    }
    if (mulai>9){

    } else if (mulai>3 && mulai<7){
        if (pulseKa > pulseAngkatB && pulseKa < pulseTurunB ){
            pneu1[1] = 1;       //kiri dpn
            angkatB = 1;
        }
        else if (pulseKa < pulseAngkatB || pulseKa>pulseTurunB ){
            pneu1[1] = 0;       //kiri dpn
            angkatB = 0;
        }
            if (pulseKa > pulseAngkatD && pulseKa < pulseTurunD){
                pneu2[1] = 1;       //kanan dpn
                angkatD = 1;
            }
            else if (pulseKa < pulseAngkatD || pulseKa>pulseTurunD){
                pneu2[1] = 0;       //kanan dpn
                angkatD = 0;
            }
    } else if (mulai>=7 && mulai<10){
        if (mulai<9){
            pneu1[1]=1;
            pneu2[1]=1;
        } else{
            pneu1[1]=0;
            pneu2[1]=0;
        }
        if (pulseKa > pulseAngkatA && pulseKa < pulseTurunA){
            pneu1[0] = 1;       //blkg kanan
            angkatA = 1;
            //bouncingKi = 0;
        }
        else if (pulseKa < pulseAngkatA ||pulseKa>pulseTurunA){
            pneu1[0] = 0;
            angkatA = 0;
        }

        if (pulseKa > pulseAngkatC && pulseKa < pulseTurunC){
            pneu2[0] = 1;       //blkg kiri
            angkatC = 1;
        }
        else if (pulseKa < pulseAngkatC || pulseKa>pulseTurunC){
            pneu2[0] = 0;
            angkatC = 0;
            //bouncingKa = 0;
        }
    } else if(mulai>10 && mulai<12){
        pneu1[1]=0;
        pneu2[1]=0;
        pneu1[0]=1;
        pneu2[0]=1;
    }
}