KRAI 2016
tunninglf beta2
Dependencies: ESC Motor PID PS_PAD hadah mbed
Fork of
Ultimate_Hybrid
by 
KRAI 2016
Diff: main.cpp
- Revision:
- 2:df6c49846367
- Parent:
- 1:fc1535231c0d
- Child:
- 3:5f9fbb53c8d5
--- a/main.cpp Tue Apr 19 13:32:26 2016 +0000
+++ b/main.cpp Fri Apr 22 08:39:32 2016 +0000
@@ -21,7 +21,7 @@
//motor (PWM, forward, reverse)
Motor motor1(PA_8, PB_0, PC_15);
Motor motor2(PA_11, PA_6, PA_5);
-//Motor motor3(PB_6, PA_7, PB_12);
+//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 motorC1(PB_9, PA_12, PC_5);
@@ -53,7 +53,7 @@
DigitalIn limit4(PC_0 ,PullUp);
//PID line follower(P, I, D, Time Sampling)
-PID PID(0.992,0.000,0.81,0.001);
+PID PID(0.92,0.000,0.61,0.001);
//servo(PWM)
Servo servoEDF(PC_8);
@@ -62,14 +62,29 @@
ESC edf(PC_6,20);
//Timer Pnuematik
-Timer timer;
+//Timer timer;
+
+
/*********************************************************************************************/
/** DEKLARASI VARIABEL, PROSEDUR, DAN FUNGSI **/
/*********************************************************************************************/
-float gMax_speed = 1;
-float v0 = 0.6;
-float ax = 0.0005;
+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 tuning1 = 0.0;
+const float tuning2 = 0.16;
+const float tuning3 = 0.03;
+const float tuning4 = 0.22;
+
+
+float vcurr = v0;
// inisialisasi pwm awal servo
double pwm = 0.00;
@@ -80,9 +95,20 @@
// variabel kondisi pnuematik
int g = 1;
+//slider auto
+int c =0;
+int batas_delay = 270;
+
+//data sensor garis dan line following
+int datasensor[6];
+int over;
+
///////
void aktuator();
void edf_servo();
+void init_sensor();
+void line(float speed);
+
/*********************************************************************************************/
/*********************************************************************************************/
/** PROGRAM UTAMA **/
@@ -117,11 +143,33 @@
PID.reset();
//inisisasi TIMER
- timer.start();
+ //timer.start();
//kondisi robot
- bool manual=true;
-
+ 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");
+
+ }*/
while(manual){
@@ -156,8 +204,6 @@
wait_ms(500);
pnuematik3 = 1;
-
-
return 0;
}
@@ -165,33 +211,29 @@
/** ALGORITMA PROSEDUR DAN FUNGSI **/
/*********************************************************************************************/
void aktuator(){
- float speed = v0;
- float tuning1 = 0.0;
- float tuning2 = 0.23;
- float tuning3 = 0.1;
- float tuning4 = 0.26;
-
- if(v0 >= gMax_speed) v0 = gMax_speed;
+ float speed = vcurr;
+
+ if(vcurr >= gMax_speed) vcurr = gMax_speed;
if((ps2.read(PS_PAD::PAD_L1)==1) && (ps2.read(PS_PAD::PAD_R1)==0)){
//pivot kiri
- motor2.speed((float)0.6*(speed-tuning2));
- motor4.speed((float)-0.6*(speed-tuning4));
- motor1.speed((float)0.6*(speed-tuning1));
- motor3.speed((float)-0.6*(speed-tuning3));
+ 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));
pc.printf("pivot kiri \n");
- v0 += ax;
+ vcurr += ax;
}
else if((ps2.read(PS_PAD::PAD_R1)==1) && (ps2.read(PS_PAD::PAD_L1)==0)){
//pivot kanan
- motor2.speed((float)-0.6*(speed-tuning2));
- motor4.speed((float)0.6*(speed-tuning4));
- motor1.speed((float)-0.6*(speed-tuning1));
- motor3.speed((float)0.6*(speed-tuning3));
+ 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));
pc.printf("pivot kanan \n");
- v0 += ax;
+ vcurr += ax;
}
else if((ps2.read(PS_PAD::PAD_RIGHT)==1) && (ps2.read(PS_PAD::PAD_TOP)==1)){
//serong atas kanan
@@ -201,7 +243,7 @@
motor3.speed(speed-tuning3);
pc.printf("serong atas kanan \n");
- v0 += ax;
+ vcurr += ax;
}
else if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_LEFT)==1)){
//serong atas kiri
@@ -211,7 +253,7 @@
motor3.brake(1);
pc.printf("serong atas kiri \n");
- v0 += ax;
+ vcurr += ax;
}
else if((ps2.read(PS_PAD::PAD_LEFT)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==1)){
//serong bawah kiri
@@ -221,7 +263,7 @@
motor3.speed(-(speed-tuning3));
pc.printf("serong bawah kiri \n");
- v0 += ax;
+ vcurr += ax;
}
else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_RIGHT)==1)){
//serong bawah kanan
@@ -231,7 +273,7 @@
motor3.brake(1);
pc.printf("serong bawah kanan \n");
- v0 += ax;
+ vcurr += ax;
}
else if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==0)){
//maju
@@ -241,7 +283,7 @@
motor4.speed(-(speed-tuning4));
pc.printf("maju \n");
- v0 += ax;
+ vcurr += ax;
}
else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_TOP)==0)){
//mundur
@@ -251,7 +293,7 @@
motor4.speed(speed-tuning4);
pc.printf("mundur \n");
- v0 += ax;
+ vcurr += ax;
}
else if((ps2.read(PS_PAD::PAD_RIGHT)==1) && (ps2.read(PS_PAD::PAD_LEFT)==0)){
//kanan
@@ -261,7 +303,7 @@
motor3.speed(speed-tuning3);
pc.printf("kanan \n");
- v0 += ax;
+ vcurr += ax;
}
else if((ps2.read(PS_PAD::PAD_RIGHT)==0) && (ps2.read(PS_PAD::PAD_LEFT)==1)){
//kiri
@@ -271,7 +313,7 @@
motor3.speed(-(speed-tuning3));
pc.printf("kiri \n");
- v0 += ax;
+ vcurr += ax;
}
else{
motor1.brake(1);
@@ -280,24 +322,55 @@
motor4.brake(1);
pc.printf("diam \n");
- v0 = 0.6;
+ 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(-1);
+ motorS.speed(-0.5);
+
+ if (limit2 ==0){
+ motorS.brake(1);
+ }
+
pc.printf("down \n");
+ c--;
}
else{
motorS.brake(1);
- pc.printf("diam \n");
+ if ((c <= batas_delay) && (c>=-batas_delay)){
+ c=0;
+ }
- }
+ 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))
{
@@ -321,13 +394,13 @@
void edf_servo(){
if(ps2.read(PS_PAD::PAD_X)==1){
//PWM ++
- pwm += 0.002;
- if( pwm > 0.7) pwm = 0.7;
+ pwm += 0.0007;
+ if( pwm > 0.7) pwm = 0.8;
pc.printf("gaspol \n");
}
else if(ps2.read(PS_PAD::PAD_SQUARE)==1){
//PWM--
- pwm -= 0.002;
+ pwm -= 0.0007;
if(pwm < 0) pwm = 0.0;
pc.printf("rem ndeng \n");
@@ -357,4 +430,121 @@
servoEDF.position((float)sudut);
edf.setThrottle((float)pwm);
edf.pulse();
+}
+
+void init_sensor(){
+ char data;
+ if(com.readable()){
+ data = com.getc();
+
+ for(int i=0; i<6; i++){
+ datasensor[i] = (data >> i) & 1;
+ }
+ }
+}
+
+void line(float speed){
+ int pv;
+ float speed1,speed2,speed3,speed4;
+
+ //////////////////logic dari PV (present Value)/////////////////////////
+ if(datasensor[0]){
+ pv = -5;
+ over=1;
+ }
+ else if(datasensor[5]){
+ pv = 5;
+ over=2;
+ }
+ else if(datasensor[0] && datasensor[1]){
+ pv = -4;
+ }
+ else if(datasensor[4] && datasensor[5]){
+ pv = 4;
+ }
+ else if(datasensor[1]){
+ pv = -3;
+ }
+ else if(datasensor[4]){
+ pv = 3;
+ }
+ else if(datasensor[1] && datasensor[2]){
+ pv = -2;
+ }
+ else if(datasensor[3] && datasensor[4]){
+ pv = 2;
+ }
+ else if(datasensor[2]){
+ pv = -1;
+ }
+ else if(datasensor[3]){
+ pv = 1;
+ }
+ 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);
+
+ }
+ 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);
+ }
+ }
+ 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));
}
\ No newline at end of file