KRAI 2016
hadah. jajal
Dependencies: ESC Motor NewTextLCD PID PS_PAD Ping Servo mbed millis
Fork of
Base_Hybrid_Latihan_Ok_Hajar_servo_pwm
by 
KRAI 2016
Diff: main.cpp
- Revision:
- 0:ac7353383a8e
- Child:
- 1:c9f11055fb12
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Feb 09 14:15:11 2016 +0000
@@ -0,0 +1,488 @@
+/*********************************************************************************************/
+/** GARUDAGO-ITB (KRAI2016) **/
+/** #ROADTOBANGKOK! **/
+/** **/
+/** MAIN PROGRAM ROBOT HYBRID SEMI OTOMATIS **/
+/** **/
+/** **/
+/** Created by : **/
+/** Rizqi Cahyo Yuwono **/
+/** EL'14 - 13214090 **/
+/** **/
+/** Last Update : 19 Desember 2015, 06.10 WIB **/
+/*********************************************************************************************/
+
+/*********************************************************************************************/
+/** FILE HEADER **/
+/*********************************************************************************************/
+#include "mbed.h"
+#include "Motor.h"
+#include "NewTextLCD.h"
+#include "PS_PAD.h"
+#include "PID.h"
+
+/*********************************************************************************************/
+/** DEKLARASI INPUT OUTPUT **/
+/*********************************************************************************************/
+// serial pc
+Serial pc(USBTX,USBRX);
+
+// LCD 20x4
+TextLCD lcd(PC_5, PB_1, PA_7, PC_4, PA_5, PA_6, TextLCD::LCD20x4); //(rs,e,d4,d5,d6,d7)
+
+// joystick PS2
+PS_PAD ps2(PB_15,PB_14,PB_13, PB_12); //(mosi, miso, sck, ss)
+
+// PID sensor garis
+PID PID(0.432,0.000,0.31,0.001); //(P,I,D, time sampling)
+
+// Motor(pwm, fwd, rev)
+Motor gripper(PB_6, PB_8, PB_9);
+Motor motor3(PC_6, PC_8, PC_9);
+Motor motor2(PB_3, PB_5, PB_4); //kanan
+Motor motor1(PA_8, PA_9, PC_7); //kiri
+
+// Sensor
+DigitalIn S1(PC_0);
+DigitalIn S2(PC_1);
+DigitalIn S3(PC_2);
+DigitalIn S4(PC_3);
+DigitalIn S5(PA_0);
+DigitalIn S6(PA_1);
+DigitalIn S7(PA_4);
+DigitalIn S8(PB_0);
+DigitalIn S9(PB_2);
+DigitalIn S10(PB_10);
+DigitalIn S11(PA_10);
+DigitalIn S12(PA_11);
+DigitalIn S13(PA_12);
+DigitalOut calibrate(PA_15);
+
+DigitalIn button(USER_BUTTON);
+
+bool sensor[13];
+
+//DigitalIn limit_switch1(A0);
+//DigitalIn limit_switch2(A1);
+
+
+// Multitasker
+Ticker timer;
+
+
+/*********************************************************************************************/
+/** DEKLARASI VARIABEL GLOBAL **/
+/*********************************************************************************************/
+float gMax_speed=0.3; //nilai maksimum kecepatan motor
+float gMin_speed=-0.05; //nilai minimum kecepatan motor
+
+unsigned char gMode=0; //variabel mode driving (manual = 0 otomatis = 1)
+unsigned char gCase=0; //variabel keadaan proses
+
+unsigned char i; // variabel iterasi
+
+
+
+/*********************************************************************************************/
+/** DEKLARASI PROSEDUR DAN FUNGSI **/
+/*********************************************************************************************/
+void init_sensor()
+{
+ if(button.read()== 0)
+ {
+ calibrate=0;
+ }
+ else
+ {
+ calibrate=1;
+ }
+
+ sensor[0]=(S1.read()==1);
+ sensor[1]=(S2.read()==1);
+ sensor[2]=(S3.read()==1);
+ sensor[3]=(S4.read()==1);
+ sensor[4]=(S5.read()==1);
+ sensor[5]=(S6.read()==1);
+ sensor[6]=(S7.read()==1);
+ sensor[7]=(S8.read()==1);
+ sensor[8]=(S9.read()==1);
+ sensor[9]=(S10.read()==1);
+ sensor[10]=(S11.read()==1);
+ sensor[11]=(S12.read()==1);
+ sensor[12]=(S13.read()==1);
+}
+
+
+void PIDrunning() //menjalankan perintah untuk line follower
+{
+ int pv;
+ int over=0;
+ float speedR,speedL;
+
+ //init_sensor();
+ //////////////////logic dari PV (present Value)/////////////////////////
+ if(sensor[0]){
+ pv = -12;
+ over=1;
+ }
+ else if(sensor[12]){
+ pv = 12;
+ over=2;
+ }
+ else if(sensor[0] && sensor[1]){
+ pv = -10;
+ }
+ else if(sensor[11] && sensor[12]){
+ pv = 10;
+ }
+ else if(sensor[1]){
+ pv = -9;
+ }
+ else if(sensor[11]){
+ pv = 9;
+ }
+ else if(sensor[1] && sensor[2]){
+ pv = -8;
+ }
+ else if(sensor[10] && sensor[11]){
+ pv = 8;
+ }
+ else if(sensor[2]){
+ pv = -7;
+ }
+ else if(sensor[10]){
+ pv = 7;
+ }
+ else if(sensor[2] && sensor[3]){
+ pv = -6;
+ }
+ else if(sensor[9] && sensor[10]){
+ pv = 6;
+ }
+ else if(sensor[3]){
+ pv = -5;
+ }
+ else if(sensor[9]){
+ pv = 5;
+ }
+ else if(sensor[3] && sensor[4]){
+ pv = -4;
+ }
+ else if(sensor[8] && sensor[9]){
+ pv = 4;
+ }
+ else if(sensor[4]){
+ pv = -3;
+ }
+ else if(sensor[8]){
+ pv = 3;
+ }
+ else if(sensor[4] && sensor[5]){
+ pv = -2;
+ }
+ else if(sensor[7] && sensor[8]){
+ pv = 2;
+ }
+ else if(sensor[5]){
+ pv = -1;
+ }
+ else if(sensor[7]){
+ pv = 1;
+ }
+ else if(sensor[5] && sensor[6]){
+ pv = -0.5;
+ }
+ else if(sensor[6] && sensor[7]){
+ pv = 0.5;
+ }
+ else if (sensor[6]){
+ pv = 0;
+ }
+ ///////////////// robot bergerak keluar dari sensor/////////////////////
+ if(over==1){
+ /*if(speed_ka > 0){
+ wait_ms(10);
+ motor2.speed(-speed_ka);
+ wait_ms(10);
+ }
+ else{
+ motor2.speed(speed_ka);
+ }
+ wait_ms(10);*/
+
+ motor1.brake(1);
+ //wait_ms(100);
+
+ }
+ else if(over==2){
+ /*if(speed_ki > 0){
+ wait_ms(10);
+ motor1.speed(-speed_ki);
+ wait_ms(10);
+ }
+ else{
+ wait_ms(10);
+ motor1.speed(speed_ki);
+ wait_ms(10);
+ }
+ wait_ms(10);*/
+ motor2.brake(1);
+ //wait_ms(100);
+ }
+
+ PID.setProcessValue(pv);
+ PID.setSetPoint(0);
+
+ // memulai perhitungan PID
+
+ speedR = gMax_speed - PID.compute();
+ if(speedR > gMax_speed){
+ speedR = gMax_speed;
+ }
+ else if(speedR < gMin_speed)
+ speedR = gMin_speed;
+ motor2.speed(speedR);
+
+ speedL = gMax_speed + PID.compute();
+ if(speedL > gMax_speed)
+ speedL = gMax_speed;
+ else if(speedL < gMin_speed)
+ speedL = gMin_speed;
+ motor1.speed(speedL);
+}
+
+void showLCD() //menampilkan user interface pada LCD
+{
+ lcd.cls();
+ lcd.locate(5,0);
+ lcd.printf("GarudaGo !!");
+
+ switch(gCase)
+ {
+ case 0 :
+ {
+ lcd.locate(2,2);
+ lcd.printf("speed = %.4f",gMax_speed);
+ break;
+ }
+ case 1 :
+ {
+ if (gMode == 1)
+ {
+ lcd.locate(3,1);
+ lcd.printf("Mode = Otomatis");
+ }
+ else if (gMode==0)
+ {
+ lcd.locate(3,1);
+ lcd.printf("Mode = Manual");
+ }
+ break;
+ }
+ }
+
+ lcd.locate(2,3);
+ lcd.printf("%d",S1.read());
+ lcd.locate(3,3);
+ lcd.printf("%d",S2.read());
+ lcd.locate(4,3);
+ lcd.printf("%d",S3.read());
+ lcd.locate(5,3);
+ lcd.printf("%d",S4.read());
+ lcd.locate(6,3);
+ lcd.printf("%d",S5.read());
+
+ lcd.locate(7,3);
+ lcd.printf("%d",S6.read());
+ lcd.locate(8,3);
+ lcd.printf("%d",S7.read());
+ lcd.locate(9,3);
+ lcd.printf("%d",S8.read());
+ lcd.locate(10,3);
+ lcd.printf("%d",S9.read());
+ lcd.locate(11,3);
+ lcd.printf("%d",S10.read());
+
+ lcd.locate(12,3);
+ lcd.printf("%d",S11.read());
+ lcd.locate(13,3);
+ lcd.printf("%d",S12.read());
+ lcd.locate(14,3);
+ lcd.printf("%d",S13.read());
+}
+
+
+void running() //prosedur utama untuk kendali robot
+{
+ float speed=gMax_speed;
+ float k=1;
+
+ switch(gCase)
+ {
+ case 0 :
+ {
+ motor1.brake(1);
+ motor2.brake(1);
+
+ if((ps2.read(PS_PAD::PAD_R2)==1) && (ps2.read(PS_PAD::PAD_L2)==0))
+ gMax_speed += 0.00008;
+ else if((ps2.read(PS_PAD::PAD_R2)==0) && (ps2.read(PS_PAD::PAD_L2)==1))
+ gMax_speed -= 0.00008;
+
+ if (gMax_speed > 1.0)
+ gMax_speed = 1.0;
+ else if(gMax_speed < 0)
+ gMax_speed = 0.0;
+
+ if((ps2.read(PS_PAD::PAD_START)==1) && (ps2.read(PS_PAD::PAD_R2)==0) && (ps2.read(PS_PAD::PAD_L2)==0))
+ {
+ gCase++;
+ }
+ break;
+ }
+ case 1 :
+ {
+ if (gMode == 1)
+ {
+ if ((ps2.read(PS_PAD::PAD_X)==1))
+ {
+ PIDrunning();
+ pc.printf("PID \t %f \t ",PID.compute());
+
+ for(i=0;i<13;i++)
+ {
+ pc.printf("%i \t",sensor[i]);
+ }
+ pc.printf("\n");
+ }
+ else if((ps2.read(PS_PAD::PAD_X)==0))
+ {
+ motor1.brake(1);
+ motor2.brake(1);
+ pc.printf("PID stop \n");
+ }
+ }
+ else if (gMode == 0)
+ {
+ if ((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==0) && (ps2.read(PS_PAD::PAD_R1)==0) && (ps2.read(PS_PAD::PAD_L1)==0)) //maju
+ {
+ if (speed < 0.1)
+ speed = 0.1;
+ else
+ speed += 0.0001;
+
+ if (speed >= gMax_speed)
+ speed = gMax_speed;
+
+ motor1.speed(speed*k);
+ motor2.speed(speed*k);
+ pc.printf("maju \n");
+ }
+ else if ((ps2.read(PS_PAD::PAD_TOP)==0) && (ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_R1)==0) && (ps2.read(PS_PAD::PAD_L1)==0)) //mundur
+ {
+ if (speed < 0.1)
+ speed = 0.1;
+ else
+ speed += 0.0001;
+
+ if (speed >= gMax_speed)
+ speed = gMax_speed;
+
+ motor1.speed(-speed*k);
+ motor2.speed(-speed*k);
+ pc.printf("mundur \n");
+ }
+ else if ((ps2.read(PS_PAD::PAD_TOP)==0) && (ps2.read(PS_PAD::PAD_BOTTOM)==0) && (ps2.read(PS_PAD::PAD_R1)==0) && (ps2.read(PS_PAD::PAD_L1)==1)) //pivot kiri
+ {
+ if (speed < 0.1)
+ speed = 0.1;
+ else
+ speed += 0.0001;
+
+ if (speed >= gMax_speed)
+ speed = gMax_speed;
+
+ motor1.speed(speed*k*0.7);
+ motor2.speed(-speed*k*0.7);
+ pc.printf("piv kiri \n");
+ }
+ else if ((ps2.read(PS_PAD::PAD_TOP)==0) && (ps2.read(PS_PAD::PAD_BOTTOM)==0) && (ps2.read(PS_PAD::PAD_R1)==1) && (ps2.read(PS_PAD::PAD_L1)==0)) //pivot kanan
+ {
+ if (speed < 0.1)
+ speed = 0.1;
+ else
+ speed += 0.0001;
+
+ if (speed >= gMax_speed)
+ speed = gMax_speed;
+
+ motor1.speed(-speed*k*0.7);
+ motor2.speed(speed*k*0.7);
+ pc.printf("piv kanan \n");
+ }
+ else if ((ps2.read(PS_PAD::PAD_TOP)==0) && (ps2.read(PS_PAD::PAD_BOTTOM)==0) && (ps2.read(PS_PAD::PAD_R1)==0) && (ps2.read(PS_PAD::PAD_L1)==0))
+ {
+ motor1.brake(0);
+ motor2.brake(0);
+ pc.printf("stop \n");
+ }
+
+ }
+ else
+ {
+ motor1.brake(1);
+ motor2.brake(1);
+ pc.printf("no PID no MANUAL \n");
+ }
+
+ if((ps2.read(PS_PAD::PAD_SELECT)==1) && (ps2.read(PS_PAD::PAD_START)==0))
+ {
+ if (gMode==1)
+ {
+ gMode=0;
+ }
+ else
+ {
+ gMode=1;
+ }
+ }
+
+ if((ps2.read(PS_PAD::PAD_START)==1) && (ps2.read(PS_PAD::PAD_SELECT)==0))
+ {
+ gCase--;
+ }
+
+ break;
+ }
+ }
+}
+
+/*********************************************************************************************/
+/*********************************************************************************************/
+/** PROGRAM UTAMA **/
+/*********************************************************************************************/
+/*********************************************************************************************/
+int main(void){
+ //inisialisasi joystick
+ ps2.init();
+
+ //inisialisasi PID
+ PID.setInputLimits(-15,15);
+ PID.setOutputLimits(-1.0,1.0);
+ PID.setMode(1.0);
+ PID.setBias(0.0);
+ PID.reset();
+
+ // serial PC
+ pc.baud(115200);
+
+ //multitasking untuk menampilkan layar
+ timer.attach(&showLCD,0.2);
+
+ while(1)
+ {
+ init_sensor();
+ ps2.poll();
+ running();
+ }
+}
\ No newline at end of file