Diff: main.cpp

Revision:

1:a2c7dd0a0f6e

Parent:

0:7ab5f1f9dcb8

Child:

2:0351727f6354

Child:

3:8f57f69f6e64

--- a/main.cpp	Wed Feb 06 11:17:11 2019 +0000
+++ b/main.cpp	Fri Mar 29 11:31:22 2019 +0000
@@ -1,65 +1,346 @@
-/*  sensor  : ultrasonic di depan kiri
-    sensor 2: ultrasonic di depan kanan
-    sensor 3: ultrasonic di belakang kiri
-    sensor 4: ultrasonic di belakang kanan
-*/
-
+//DEKLARASI LIBRARY
 #include "mbed.h"
-#include "hcsr04.h"
-#include "RGBLed.h"
-#include "LCD_DISCO_F429ZI.h"
-//#include "string.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
 
-LCD_DISCO_F429ZI lcd;
-//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 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;
+        }
 
 
-//D12 TRIGGER D11 ECHO
-   HCSR04 sensor(PA_4, PA_5); 
-   HCSR04 sensor2(PC_4, PC_5);
-  // HCSR04 sensor3(PA4, PA5);
-  // HCSR04 sensor4(PA6, PA7);
- //  DigitalOut led1(PE_10);
- //  DigitalOut led2(PE_11); 
- //  DigitalOut led3(PE_12);
+        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;
+}
 
-int main() {
-    uint8_t text[10];
-   // led1=0;
-   // led2=0;
-   // led3=0;
-    long dkiri, dkanan;
-        BSP_LCD_SetFont(&Font20);
-      lcd.DisplayStringAt(0, LINE(5), (uint8_t *)"TOUCHSCREEN", CENTER_MODE);
-      lcd.DisplayStringAt(0, LINE(6), (uint8_t *)"INIT OK", CENTER_MODE);
-      wait(3);
-    while(1) {
-    BSP_LCD_SetFont(&Font20);
-    lcd.Clear(LCD_COLOR_BLACK);
-      lcd.SetBackColor(LCD_COLOR_BLACK);
-      lcd.SetTextColor(LCD_COLOR_WHITE);
-      dkiri  = sensor.distance();
-      dkanan = sensor2.distance();
-    //  bkiri  = sensor3.distance();
-     // bkanan = sensor4.distance();   
-      sprintf((char*)text, "i=%d a=%d", (int *)dkiri,(int *) dkanan);
-      lcd.DisplayStringAt(0, LINE(6), (uint8_t *)text, CENTER_MODE);
-     //lcd.DisplayStringAt(0, LINE(5), (uint8_t *)"distance front right %d\n", dkanan, CENTER_MODE);
-         /* pc.printf("distance front left  %d\n",dkiri);
-          pc.printf("distance front right  %d\n",dkanan);
-          pc.printf("distance back left  %d\n",bkiri);
-          pc.printf("distance back left  %d\n",bkanan);*/
- // 1 sec  
-      wait(0.3);
-    /*  if ((dkanan >=150) || (dkiri>=150))
-        {
-          led1 = 1;
-          udhbelok = 1;
+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;
         }
-      if ((udhbelok==1) && (bkanan >= 100))
-        {
-          led1 = 0;
-          led3 = 1;
-        } */
     }
-}
\ No newline at end of file
+    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
+    #    #          #    #
+    #    #          #    #
+     ####            ####
+    */
+}