KRAI 2016
base ultimate + line following HYBRID
Dependencies: ESC Motor PS_PAD hadah mbed Ping
Fork of
Ultimate_Hybrid
by 
KRAI 2016
Diff: main.cpp
- Revision:
- 3:43d4cb3ece1b
- Parent:
- 2:df6c49846367
- Child:
- 4:7a7a8aa33fd5
--- a/main.cpp Fri Apr 22 08:39:32 2016 +0000
+++ b/main.cpp Tue Apr 26 14:57:35 2016 +0000
@@ -4,7 +4,6 @@
#include "mbed.h"
#include "Motor.h"
#include "PS_PAD.h"
-#include "PID.h"
#include "esc.h"
#include "Servo.h"
@@ -21,24 +20,12 @@
//motor (PWM, forward, reverse)
Motor motor1(PA_8, PB_0, PC_15);
Motor motor2(PA_11, PA_6, PA_5);
-//Motor gripper(PB_6, PA_7, PB_12);
Motor motor3(PA_9, PC_2, PC_3);
-Motor motor4(PB_7, PA_14, PA_15);
+Motor motor4(PA_10, PB_5, PB_4);
+Motor motorS(PB_7, PA_14, PA_15);
Motor motorC1(PB_9, PA_12, PC_5);
Motor motorC2(PB_8, PB_1, PA_13);
-Motor motorS(PA_10, PB_5, PB_4);
-
-/* coba coba
-//motor (PWM, forward, reverse)
-Motor motor1(PA_8, PB_0, PC_15);
-Motor motorC1(PA_11, PA_6, PA_5);
-//Motor motor3(PB_6, PA_7, PB_12);
-Motor motorC2(PA_9, PC_2, PC_3);
-Motor motor4(PB_7, PA_14, PA_15);
-Motor motor2(PB_9, PA_12, PC_5);
-Motor motor3(PB_8, PB_1, PA_13);
-Motor motorS(PA_10, PB_5, PB_4);
-*/
+//Motor motor4(PB_6, PA_7, PB_12);
//pnuematik
DigitalOut pnuematik1(PC_11);
@@ -52,8 +39,10 @@
DigitalIn limit3(PC_1 ,PullUp);
DigitalIn limit4(PC_0 ,PullUp);
-//PID line follower(P, I, D, Time Sampling)
-PID PID(0.92,0.000,0.61,0.001);
+DigitalIn field(PB_10 ,PullUp);
+
+//laser pointer
+DigitalOut laser(PB_3);
//servo(PWM)
Servo servoEDF(PC_8);
@@ -61,30 +50,30 @@
//EDF(PWM, timer)
ESC edf(PC_6,20);
-//Timer Pnuematik
-//Timer timer;
+//Timer
+Ticker timer;
/*********************************************************************************************/
/** DEKLARASI VARIABEL, PROSEDUR, DAN FUNGSI **/
/*********************************************************************************************/
-const float gMax_speed = 0.8;
-const float gMin_speed = 0.1;
-/*const float tuning1 = 0.0;
-const float tuning2 = 0.2;
-const float tuning3 = 0.06;
-const float tuning4 = 0.24;*/
-const float v0 = 0.35;
-const float ax = 0.0006;
+const float gMax_speed = 0.85;
+//const float gMin_speed = 0.1;
+const float v0 = 0.4;
+const float ax = 0.0007;
+float vcurr = v0;
const float tuning1 = 0.0;
-const float tuning2 = 0.16;
-const float tuning3 = 0.03;
-const float tuning4 = 0.22;
+const float tuning2 = 0.04;
+const float tuning3 = 0.0;
+const float tuning4 = 0.04;
+const float driver0 = 1;
+const float driver1 = 0.85;
+const float driver2 = 0.65;
+const float driver3 = 0.2;
-float vcurr = v0;
// inisialisasi pwm awal servo
double pwm = 0.00;
@@ -102,12 +91,14 @@
//data sensor garis dan line following
int datasensor[6];
int over;
+int g_flag;
///////
-void aktuator();
+void aktuator(int f);
void edf_servo();
void init_sensor();
-void line(float speed);
+void linetracer(float speed);
+void flag_sensor();
/*********************************************************************************************/
/*********************************************************************************************/
@@ -119,6 +110,9 @@
pc.baud(115200);
com.baud(115200);
+ if(field == 1) sudut = -85;
+ else sudut = 85;
+
//inisiasi joystick
ps2.init();
@@ -129,67 +123,59 @@
edf.setThrottle(0);
edf.pulse();
+ //inisisasi laser
+ laser = 1;
+
//inisiasi pnuematik
pnuematik1 = 1;
pnuematik2 = 1;
- pnuematik3 = 1;
- pnuematik4 = 1;
-
- //inisiasi PID sensor
- PID.setInputLimits(-15,15);
- PID.setOutputLimits(-1.0,1.0);
- PID.setMode(1.0);
- PID.setBias(0.0);
- PID.reset();
+ pnuematik3 = 0;
+ pnuematik4 = 0;
//inisisasi TIMER
- //timer.start();
+ timer.attach(&flag_sensor,0.0005);
+ timer.detach();
//kondisi robot
bool manual=true;
- /*
- while(1)
- {
- ps2.poll();
- init_sensor();
-
- if(ps2.read(PS_PAD::PAD_X)==1){
- line(0.3);
- }
- else{
- motor1.brake(1);
- motor2.brake(1);
- motor3.brake(1);
- motor4.brake(1);
-
- //v =0.1;
- }
- for(int i=0; i<6; i++){
- pc.printf("%d ",datasensor[i]);
- }
- pc.printf("\n");
-
- }*/
-
+ bool pool= false;
+
while(manual){
ps2.poll();
- aktuator();
+ aktuator(field);
edf_servo();
- if(limit3==0) manual = false;
+ if(ps2.read(PS_PAD::ANALOG_LEFT)==1) manual = false;
}
+ timer.attach(&flag_sensor,0.0005);
+
+ while(!pool){
+ init_sensor();
+
+ if(vcurr > 0.3) vcurr = (float) 0.3;
+
+ linetracer(vcurr);
+ //laser = 1;
+
+ vcurr+=ax;
+
+ if(limit3==0) pool=true;
+
+ }
motor1.brake(1);
motor2.brake(1);
motor3.brake(1);
motor4.brake(1);
+ timer.detach();
+
pnuematik1=0;
wait_ms(1500);
-
+
while(limit4!=0){
motorC1.speed(1);
motorC2.speed(-1);
@@ -198,11 +184,20 @@
motorC1.brake(1);
motorC2.brake(1);
- pnuematik3 = 0;
- wait_ms(1500);
- pnuematik2 = 1;
- wait_ms(500);
- pnuematik3 = 1;
+ if(field==1){
+ pnuematik3 = 0;
+ wait_ms(1500);
+ pnuematik2 = 1;
+ wait_ms(500);
+ pnuematik3 = 1;
+ }
+ else{
+ pnuematik4 = 0;
+ wait_ms(1500);
+ pnuematik2 = 1;
+ wait_ms(500);
+ pnuematik4 = 1;
+ }
return 0;
}
@@ -210,27 +205,28 @@
/*********************************************************************************************/
/** ALGORITMA PROSEDUR DAN FUNGSI **/
/*********************************************************************************************/
-void aktuator(){
+void aktuator(int f){
float speed = vcurr;
if(vcurr >= gMax_speed) vcurr = gMax_speed;
+ if(f == 1){
if((ps2.read(PS_PAD::PAD_L1)==1) && (ps2.read(PS_PAD::PAD_R1)==0)){
//pivot kiri
- motor2.speed((float)0.2*(speed-tuning2));
- motor4.speed((float)-0.2*(speed-tuning4));
- motor1.speed((float)0.2*(speed-tuning1));
- motor3.speed((float)-0.2*(speed-tuning3));
+ motor2.speed((float)0.5*(speed-tuning2));
+ motor4.speed((float)-0.5*(speed-tuning4));
+ motor1.speed((float)0.5*(speed-tuning1));
+ motor3.speed((float)-0.5*(speed-tuning3));
pc.printf("pivot kiri \n");
vcurr += ax;
}
else if((ps2.read(PS_PAD::PAD_R1)==1) && (ps2.read(PS_PAD::PAD_L1)==0)){
//pivot kanan
- motor2.speed((float)-0.2*(speed-tuning2));
- motor4.speed((float)0.2*(speed-tuning4));
- motor1.speed((float)-0.2*(speed-tuning1));
- motor3.speed((float)0.2*(speed-tuning3));
+ motor2.speed((float)-0.5*(speed-tuning2));
+ motor4.speed((float)0.5*(speed-tuning4));
+ motor1.speed((float)-0.5*(speed-tuning1));
+ motor3.speed((float)0.5*(speed-tuning3));
pc.printf("pivot kanan \n");
vcurr += ax;
@@ -324,56 +320,169 @@
vcurr = v0;
}
-
- if((ps2.read(PS_PAD::PAD_CIRCLE)==0) && (ps2.read(PS_PAD::PAD_TRIANGLE)==1)){
- //POWER WINDOW ATAS
- motorS.speed(1);
+ }
+ else{
+ if((ps2.read(PS_PAD::PAD_L1)==1) && (ps2.read(PS_PAD::PAD_R1)==0)){
+ //pivot kiri
+ motor2.speed((float)0.5*(speed-tuning2));
+ motor4.speed((float)-0.5*(speed-tuning4));
+ motor1.speed((float)0.5*(speed-tuning1));
+ motor3.speed((float)-0.5*(speed-tuning3));
+ pc.printf("pivot kiri \n");
+
+ vcurr += ax;
+ }
+ else if((ps2.read(PS_PAD::PAD_R1)==1) && (ps2.read(PS_PAD::PAD_L1)==0)){
+ //pivot kanan
+ motor2.speed((float)-0.5*(speed-tuning2));
+ motor4.speed((float)0.5*(speed-tuning4));
+ motor1.speed((float)-0.5*(speed-tuning1));
+ motor3.speed((float)0.5*(speed-tuning3));
+ pc.printf("pivot kanan \n");
- if (limit1 == 0){
- motorS.brake(1);
- }
+ vcurr += ax;
+ }
+ else if((ps2.read(PS_PAD::PAD_RIGHT)==1) && (ps2.read(PS_PAD::PAD_TOP)==1)){
+ //serong atas kanan
+ motor2.speed(-(speed-tuning2));
+ motor4.brake(1);
+ motor1.brake(1);
+ motor3.speed(-(speed-tuning3));
+ pc.printf("serong atas kanan \n");
+ vcurr += ax;
+ }
+ else if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_LEFT)==1)){
+ //serong atas kiri
+ motor2.brake(1);
+ motor4.speed(speed-tuning4);
+ motor1.speed(speed-tuning1);
+ motor3.brake(1);
+ pc.printf("serong atas kiri \n");
+
+ vcurr += ax;
+ }
+ else if((ps2.read(PS_PAD::PAD_LEFT)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==1)){
+ //serong bawah kiri
+ motor2.speed(speed-tuning2);
+ motor4.brake(1);
+ motor1.brake(1);
+ motor3.speed(speed-tuning3);
+ pc.printf("serong bawah kiri \n");
- pc.printf("up \n");
- c++;
+ vcurr += ax;
}
- else if((ps2.read(PS_PAD::PAD_CIRCLE)==1) && (ps2.read(PS_PAD::PAD_TRIANGLE)==0)){
- //POWER WINDOW BAWAH
- motorS.speed(-0.5);
+ else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_RIGHT)==1)){
+ //serong bawah kanan
+ motor2.brake(1);
+ motor4.speed(-(speed-tuning4));
+ motor1.speed(-(speed-tuning1));
+ motor3.brake(1);
+ pc.printf("serong bawah kanan \n");
+
+ vcurr += ax;
+ }
+ else if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==0)){
+ //maju
+ motor1.speed(speed-tuning1);
+ motor3.speed(-(speed-tuning3));
+ motor2.speed(-(speed-tuning2));
+ motor4.speed(speed-tuning4);
+ pc.printf("maju \n");
- if (limit2 ==0){
- motorS.brake(1);
- }
+ vcurr += ax;
+ }
+ else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_TOP)==0)){
+ //mundur
+ motor1.speed(-(speed-tuning1));
+ motor3.speed(speed-tuning3);
+ motor2.speed(speed-tuning2);
+ motor4.speed(-(speed-tuning4));
+ pc.printf("mundur \n");
- pc.printf("down \n");
- c--;
+ vcurr += ax;
+ }
+ else if((ps2.read(PS_PAD::PAD_RIGHT)==1) && (ps2.read(PS_PAD::PAD_LEFT)==0)){
+ //kanan
+ motor2.speed(-(speed-tuning2));
+ motor4.speed(-(speed-tuning4));
+ motor1.speed(-(speed-tuning1));
+ motor3.speed(-(speed-tuning3));
+ pc.printf("kanan \n");
+
+ vcurr += ax;
+ }
+ else if((ps2.read(PS_PAD::PAD_RIGHT)==0) && (ps2.read(PS_PAD::PAD_LEFT)==1)){
+ //kiri
+ motor2.speed(speed-tuning2);
+ motor4.speed(speed-tuning4);
+ motor1.speed(speed-tuning1);
+ motor3.speed(speed-tuning3);
+ pc.printf("kiri \n");
+
+ vcurr += ax;
}
else{
- motorS.brake(1);
- if ((c <= batas_delay) && (c>=-batas_delay)){
- c=0;
- }
+ motor1.brake(1);
+ motor3.brake(1);
+ motor2.brake(1);
+ motor4.brake(1);
+ pc.printf("diam \n");
- pc.printf("diam \n");
- }
- if((c > batas_delay) && (limit1 == 0)){
- c = 0;
+ vcurr = v0;
+ }
+
+ }
+
+ if((ps2.read(PS_PAD::PAD_CIRCLE)==0) && (ps2.read(PS_PAD::PAD_TRIANGLE)==1)){
+ //POWER WINDOW ATAS
+ motorS.speed(1);
+ if (limit1 == 0){
+ motorS.brake(1);
+ }
+
+
+ pc.printf("up \n");
+ c++;
+ }
+ else if((ps2.read(PS_PAD::PAD_CIRCLE)==1) && (ps2.read(PS_PAD::PAD_TRIANGLE)==0)){
+ //POWER WINDOW BAWAH
+ motorS.speed(-0.5);
+
+ if (limit2 ==0){
motorS.brake(1);
}
- else if((c < -batas_delay) && (limit2 == 0)){
- c = 0;
- motorS.brake(1);
+
+ pc.printf("down \n");
+ c--;
+ }
+ else{
+ motorS.brake(1);
+ if ((c <= batas_delay) && (c>=-batas_delay)){
+ c=0;
}
- else if( (c > batas_delay) && (limit1 != 0)){
- motorS.speed(1);
- }
- else if ((c<-batas_delay) && (limit2 != 0)){
- motorS.speed(-0.7);
- }
+
+ pc.printf("diam \n");
+ }
+
+ if((c > batas_delay) && (limit1 == 0)){
+ c = 0;
+ motorS.brake(1);
+ }
+ else if((c < -batas_delay) && (limit2 == 0)){
+ c = 0;
+ motorS.brake(1);
+ }
+ else if( (c > batas_delay) && (limit1 != 0)){
+ motorS.speed(1);
+ }
+ else if ((c<-batas_delay) && (limit2 != 0)){
+ motorS.speed(-0.7);
+ }
- if ((ps2.read(PS_PAD::PAD_SELECT)==1))
- {
+ if ((ps2.read(PS_PAD::PAD_SELECT)==1))
+ {
//mekanisme ambil gripper
pc.printf("mekanisme gripper");
if (g==1){
@@ -388,7 +497,7 @@
wait_ms(400);
g=1;
}
- }
+ }
}
void edf_servo(){
@@ -424,14 +533,20 @@
if(ps2.read(PS_PAD::PAD_START)==1){
sudut = 0;
- pwm = 0.25;
+ pwm = 0.22;
+
+ pnuematik3 = 1;
+ pnuematik4 = 1;
}
-
+
+
servoEDF.position((float)sudut);
edf.setThrottle((float)pwm);
edf.pulse();
}
+
+/////////////////LINE FOLLOWER/////////////////////////
void init_sensor(){
char data;
if(com.readable()){
@@ -443,108 +558,112 @@
}
}
-void line(float speed){
- int pv;
+void linetracer(float speed){
float speed1,speed2,speed3,speed4;
-
+
//////////////////logic dari PV (present Value)/////////////////////////
- if(datasensor[0]){
- pv = -5;
- over=1;
+ if(datasensor[2] && datasensor[3]){
+ speed1 = speed*driver0;
+ speed2 = speed*driver0;
+ speed3 = speed*driver0;
+ speed4 = speed*driver0;
}
- else if(datasensor[5]){
- pv = 5;
- over=2;
+ else if(datasensor[2]){
+ speed1 = speed*driver1;
+ speed2 = speed*driver0;
+ speed3 = speed*driver0;
+ speed4 = speed*driver1;
}
- else if(datasensor[0] && datasensor[1]){
- pv = -4;
- }
- else if(datasensor[4] && datasensor[5]){
- pv = 4;
+ else if(datasensor[3]){
+ speed1 = speed*driver0;
+ speed2 = speed*driver1;
+ speed3 = speed*driver1;
+ speed4 = speed*driver0;
}
else if(datasensor[1]){
- pv = -3;
+ speed1 = speed*driver2;
+ speed2 = speed*driver0;
+ speed3 = speed*driver0;
+ speed4 = speed*driver2;
}
else if(datasensor[4]){
- pv = 3;
- }
- else if(datasensor[1] && datasensor[2]){
- pv = -2;
- }
- else if(datasensor[3] && datasensor[4]){
- pv = 2;
+ speed1 = speed*driver0;
+ speed2 = speed*driver2;
+ speed3 = speed*driver2;
+ speed4 = speed*driver0;
}
- else if(datasensor[2]){
- pv = -1;
+ else if(datasensor[0]){
+ speed1 = speed*driver3;
+ speed2 = speed*driver0;
+ speed3 = speed*driver0;
+ speed4 = speed*driver3;
}
- else if(datasensor[3]){
- pv = 1;
+ else if(datasensor[5]){
+ speed1 = speed*driver0;
+ speed2 = speed*driver3;
+ speed3 = speed*driver3;
+ speed4 = speed*driver0;
}
- else if(datasensor[2] && datasensor[3]){
- 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);
- motor4.brake(1);
- //wait_ms(100);
-
+ if(g_flag == 0){
+ speed1 = speed*driver0;
+ speed2 = speed*driver0;
+ speed3 = speed*driver0;
+ speed4 = speed*driver0;
+ }
+ else if(g_flag == 3){
+ speed1 = speed*driver1;
+ speed2 = speed*driver0;
+ speed3 = speed*driver0;
+ speed4 = speed*driver1;
+ }
+ else if(g_flag == 4){
+ speed1 = speed*driver0;
+ speed2 = speed*driver1;
+ speed3 = speed*driver1;
+ speed4 = speed*driver0;
}
- 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);*/
- motor3.brake(1);
- motor2.brake(1);
- //wait_ms(100);
+ else if(g_flag == 2){
+ speed1 = speed*driver2;
+ speed2 = speed*driver0;
+ speed3 = speed*driver0;
+ speed4 = speed*driver2;
+ }
+ else if(g_flag == 5){
+ speed1 = speed*driver0;
+ speed2 = speed*driver2;
+ speed3 = speed*driver2;
+ speed4 = speed*driver0;
+ }
+ else if(g_flag == 1){
+ speed1 = -speed*driver2;
+ speed2 = speed*driver2;
+ speed3 = speed*driver2;
+ speed4 = -speed*driver2;
+ }
+ else if(g_flag == 6){
+ speed1 = speed*driver2;
+ speed2 = -speed*driver2;
+ speed3 = -speed*driver2;
+ speed4 = speed*driver2;
}
}
- PID.setProcessValue(pv);
- PID.setSetPoint(0);
- // memulai perhitungan PID
-
-
- speed2 = speed - 0.0 + PID.compute();
- speed1 = speed - 0.0 + PID.compute();
- speed3 = speed - 0.2 - PID.compute();
- speed4 = speed - 0.3 - PID.compute();
-
- if(speed1 > speed) speed1 = speed;
- else if(speed1 < gMin_speed) speed1 = gMin_speed;
-
- if(speed2 > speed) speed2 = speed;
- else if(speed2 < gMin_speed) speed2 = gMin_speed;
-
- if(speed3 > speed) speed3 = speed;
- else if(speed3 < gMin_speed) speed3 = gMin_speed;
-
- if(speed4 > speed) speed4 = speed;
- else if(speed4 < gMin_speed) speed4 = gMin_speed;
-
- motor3.speed(-(speed3));
- motor2.speed(-(speed2));
-
- motor1.speed(-(speed1));
- motor4.speed(-(speed4));
+ motor1.speed((float)-(speed1-0.0));
+ motor2.speed((float)-(speed2-0.04));
+ motor3.speed((float)-(speed3-0.0));
+ motor4.speed((float)-(speed4-0.0));
+
+}
+
+void flag_sensor(){
+ if((datasensor[2] == 1) && (datasensor[3] == 1)) g_flag = 0;
+ else if(datasensor[2] == 1) g_flag = 3;
+ else if(datasensor[3] == 1) g_flag = 4;
+ else if(datasensor[1] == 1) g_flag = 2;
+ else if(datasensor[4] == 1) g_flag = 5;
+ else if(datasensor[0] == 1) g_flag = 1;
+ else if(datasensor[5] == 1) g_flag = 6;
}
\ No newline at end of file