KRAI 2016
base buat latihan
Dependencies: ESC Motor NewTextLCD PID PS_PAD Ping Servo mbed millis
Fork of
Base_Hybrid_Latihan_Ok_Hajar
by 
KRAI 2016
Diff: main.cpp
- Revision:
- 2:2f7bed7fb055
- Parent:
- 1:c9f11055fb12
- Child:
- 3:d3708c3ed288
--- a/main.cpp Mon Feb 15 17:36:33 2016 +0000
+++ b/main.cpp Tue Feb 16 18:33:25 2016 +0000
@@ -21,6 +21,8 @@
#include "PS_PAD.h"
#include "PID.h"
#include "millis.h"
+#include "esc.h"
+#include "Servo.h"
/*********************************************************************************************/
/** DEKLARASI INPUT OUTPUT **/
@@ -29,20 +31,34 @@
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)
+//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)
+
+// tambahan power
+// inisialisasi pwm awal servo
+float pwm = 0.00;
+
+// inisialisasi sudut awal
+int sudut = 0;
+
+//membatasi nilai brushless pada edf
+float min=0;
+float max=0.50;
+
// PID sensor garis
-PID PID(0.992,0.000,0.81,0.001); //(P,I,D, time sampling)
+//PID PID(0.992,0.000,0.81,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
+Motor motor2(PC_6, PC_8, PC_9); //gripper
+Motor motor1(PA_15, PA_14, PA_13); //motor3
+//Motor gripper(PB_5, PB_4, PB_3);
+//Motor motor2(PA_3, PC_15, PC_14); //kanan
+//Motor motor1(PA_1, PH_0, PH_1); //kiri
+/*
// Sensor
DigitalIn S1(PC_0);
DigitalIn S2(PC_1);
@@ -58,446 +74,194 @@
DigitalIn S12(PA_11);
DigitalIn S13(PA_12);
DigitalOut calibrate(PA_15);
+*/
DigitalIn button(USER_BUTTON);
-bool sensor[13];
+
+//bool sensor[13];
//DigitalIn limit_switch1(A0);
//DigitalIn limit_switch2(A1);
// Multitasker
-Ticker timer;
+//Ticker timer;
/*********************************************************************************************/
/** DEKLARASI VARIABEL GLOBAL **/
/*********************************************************************************************/
-float gMax_speed=0.7; //nilai maksimum kecepatan motor
+float gMax_speed=0.5; //nilai maksimum kecepatan motor
float gMin_speed=-0.05; //nilai minimum kecepatan motor
-float gTuning = 0.36;
+float gTuning = 0.34;
unsigned char gMode=0; //variabel mode driving (manual = 0 otomatis = 1)
unsigned char gCase=0; //variabel keadaan proses
unsigned char i; // variabel iterasi
int over=0;
+int lapangan = 1;
/*********************************************************************************************/
/** DEKLARASI PROSEDUR DAN FUNGSI **/
/*********************************************************************************************/
-void init_sensor()
-{
- if((ps2.read(PS_PAD::PAD_CIRCLE)==1))
- {
- 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;
-
- 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;
- }
- else
- {
- ///////////////// 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++;
- wait_ms(200);
- }
- 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-gTuning);
- 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;
- wait_ms(200);
- }
- else
- {
- gMode=1;
- wait_ms(200);
- }
- }
-
- if((ps2.read(PS_PAD::PAD_START)==1) && (ps2.read(PS_PAD::PAD_SELECT)==0))
- {
- gCase--;
- wait_ms(200);
- }
-
- break;
- }
- }
-}
-
/*********************************************************************************************/
/*********************************************************************************************/
/** PROGRAM UTAMA **/
/*********************************************************************************************/
/*********************************************************************************************/
+
+void init_servo(int i){
+ if (i){
+ if (sudut>90){
+ sudut=90;
+ }
+ if (sudut<0){
+ sudut=0;
+ }
+ } else {
+
+ if (sudut>0){
+ sudut=0;
+ }
+ if (sudut<-90){
+ sudut=-90;
+ }
+ }
+}
+
+void init_pwm (){
+ if (pwm>max){
+ pwm = max;
+ }
+
+ if (pwm<min){
+ pwm = min;
+ }
+}
+
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();
+ //tambahan power
+ ESC edf(PB_9,20); //p wm esc pin PC_7
+ Servo myservo(PB_8); //pwm servo pin PA_8
+ //set inisialisasi servo pada posisi 0
+ myservo.position(sudut);
+ //set edf pada posisi bukan kalibrasi, yaitu set edf 0
+ edf.setThrottle(pwm);
+ edf.pulse();
- // serial PC
pc.baud(115200);
-
- //multitasking untuk menampilkan layar
- startMillis();
- uint32_t lastTime = 0;
- int lcd_refresh_rate = 10;
+ float speed;
while(1)
{
- if(millis() - lastTime >= lcd_refresh_rate)
+ ps2.poll();
+
+ if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==0)){
+ //MOTOR DEPAN
+ speed = gMax_speed;
+
+ motor1.speed(speed);
+ motor2.speed(speed);
+ pc.printf("maju \n");
+ }
+ else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_TOP)==0)){
+ //MOTOR BELAKANG
+ speed = gMax_speed;
+
+ motor1.speed(-speed);
+ motor2.speed(-speed);
+ pc.printf("mundur \n");
+ }
+ else if((ps2.read(PS_PAD::PAD_L1)==1) && (ps2.read(PS_PAD::PAD_R1)==0)){
+ //pivot kiri
+ speed = gMax_speed;
+
+ motor1.speed(-speed*0.8);
+ motor2.speed(speed*0.8);
+ pc.printf("kiri \n");
+ }
+ else if((ps2.read(PS_PAD::PAD_R1)==1) && (ps2.read(PS_PAD::PAD_L1)==0)){
+ //pivot kanan
+ speed = gMax_speed;
+
+ motor1.speed(speed*0.8);
+ motor2.speed(-speed*0.8);
+ pc.printf("kanan \n");
+ }
+ else
{
- showLCD;
+ motor1.brake(1);
+ motor2.brake(1);
+ }
+
+ if(ps2.read(PS_PAD::ANALOG_LX)<=-100){
+ //SLIDER KIRI
+ pc.printf("slide kiri \n");
+ }
+ else if(ps2.read(PS_PAD::ANALOG_LX)>=100){
+ //SLIDER KANAN
+ pc.printf("slide kanan \n");
}
- init_sensor();
- ps2.poll();
- running();
+ else
+ {
+ pc.printf("slide diam \n");
+ }
+
+ /*if(ps2.read(PS_PAD::ANALOG_LY)<=-100){
+ //POWER WINDOW ATAS
+ gripper.speed(0.75);
+ pc.printf("up \n");
+ }
+ else if(ps2.read(PS_PAD::ANALOG_LY)>=100){
+ //POWER WINDOW BAWAH
+ gripper.speed(-0.2);
+ pc.printf("down \n");
+ }
+ else
+ {
+ gripper.brake(1);
+ pc.printf("power diam \n");
+ }*/
+
+ if(ps2.read(PS_PAD::ANALOG_RY)<=-100){
+ //PWM ++
+ pwm += 0.01;
+ pc.printf("gaspol \n");
+ }
+
+ if(ps2.read(PS_PAD::ANALOG_RY)>=100){
+ //PWM--
+ pwm -= 0.01;
+ pc.printf("rem ndeng \n");
+ }
+
+ if(ps2.read(PS_PAD::ANALOG_RX)<=-100){
+ //SERVO --
+ sudut -= 3;
+ pc.printf("servo min \n");
+ }
+
+ if(ps2.read(PS_PAD::ANALOG_RX)>=100){
+ //SERVO ++
+ sudut += 3;
+ pc.printf("servo max \n");
+ }
+
+ init_servo(lapangan);
+ init_pwm();
+ myservo.position(sudut);
+ wait_ms(25);
+ edf.setThrottle(pwm);
+ edf.pulse();
+ wait_ms(25);
}
}
\ No newline at end of file