KRAI ITB GARUDAGO / Mbed 2 deprecated fungsi_switch_kaki

Dependencies:   mbed encoderKRAI Motornew CMPS12_KRAI ping millis

main.cpp

Committer:
gianarjuna
Date:
2019-03-29
Revision:
1:a2c7dd0a0f6e
Parent:
0:7ab5f1f9dcb8
Child:
2:0351727f6354
Child:
3:8f57f69f6e64

File content as of revision 1:a2c7dd0a0f6e:

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

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] = {(PC_6),(PC_8)};          //kanan belakang , kiri depan
DigitalOut pneu2[2] = {(PC_5),(PC_9)};           //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;
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,
         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 thetaKa;
int thetaKi;
int pulseLangkah;
void hitungParameter();
void getHeading();
void hitungPIDTheta(double theta_sp);
void gerakMotor();
void tambahPWM();
void debug();
void gerakDepan(int end);
void hitungOdometry();
void hitungParameterKa();
void hitungParameterKi();
void initPneu();
void angkatKaki();
//int main(){
//  startMillis();
//  speed_maju=0.3;
//  heading0=(float)cmps.getAngle()/10;
//  while(1){
//      debug();
//      //pc.printf("TEST\n");
//  }
//}


int main()
{
    encSteer.reset();
    startMillis();
    theta = 0;
    cp = 1;
    state=0;
    theta_sp =0;
    ohstart=0;
    KI = KI1;
    angkat = 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     ");
            } else {
                kanan = 0;
            }
            if(!switchKi) {
                kiri = 1;
            } else {
                kiri = 0;
            }
            last_mt_print = millis();
            pc.printf("%d %d %.2f %d \n",kiri,kanan,speed_maju, pulsetemp);
        }
        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();
        }

        if (millis() - last_start>15000) { //maksimal waktu kuda gerak 17 detik
            ohstart=0;
        }


        if (ohstart) {
            angkatKaki();
            if (millis() - lastFSM > 9) {
                if (state==1) { //state maju lurus
                    theta_sp=0;
                    belok=0;
                    pwm_maju=0.80;
                }
                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();
                hitungParameterKa();
                hitungParameterKi();
                lastParameter = millis();
            }
            if (millis() - lastMotor>4) {
                gerakMotor();
                lastMotor=millis();
            }
        }

    }

}

void hitungParameter()
{
    pulse = (double)encSteer.getPulses()*PULSE_TO_DEG;
    encSteer.reset();
    pulseLangkah = (int)encKiri.getPulses();
    encKiri.reset();
    pulsetemp += pulseLangkah;
    if(bouncing == 1) {
        pulsetemp =0;
        bouncing = 2;//pc.printf("kena");
    }
    theta += pulse;
}
void initPneu()
{
    pneu1[0]=0;
    pneu1[1]=0;
    pneu2[0]=0;
    pneu2[1]=0;
}

void angkatKaki()
{
    if(!switchKi && bouncing == 0) {
        bouncing = 1;
    }
    if (pulsetemp > 0 && pulsetemp < 150 && !angkat) {
        pneu2[0] = 1;
        pneu2[1] = 1;
        angkat = 1;
    } else if (pulsetemp > 150 && angkat) {
        pneu2[0] = 0;
        pneu2[1] = 0;
        angkat = 0;
        bouncing = 0;
    }
}

void hitungParameterKa()
{
    int pulseKa = (double)encKanan.getPulses()*PULSE_TO_DEG;
    encKanan.reset();

    thetaKa += pulseKa;
}

void hitungParameterKi()
{
    int pulseKi = (double)encKiri.getPulses()*PULSE_TO_DEG;
    encKiri.reset();

    thetaKi += pulseKi;
}

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