KRAI 2016
tunninglf beta2
Dependencies: ESC Motor PID PS_PAD hadah mbed
Fork of
Ultimate_Hybrid
by 
KRAI 2016
Diff: main.cpp
- Revision:
- 3:5f9fbb53c8d5
- Parent:
- 2:df6c49846367
--- a/main.cpp Fri Apr 22 08:39:32 2016 +0000
+++ b/main.cpp Fri Apr 22 23:58:19 2016 +0000
@@ -21,12 +21,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 motor4(PB_6, PA_7, PB_12);
Motor motor3(PA_9, PC_2, PC_3);
-Motor motor4(PB_7, PA_14, PA_15);
+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);
+Motor motor4(PA_10, PB_5, PB_4);
/* coba coba
//motor (PWM, forward, reverse)
@@ -53,7 +53,7 @@
DigitalIn limit4(PC_0 ,PullUp);
//PID line follower(P, I, D, Time Sampling)
-PID PID(0.92,0.000,0.61,0.001);
+PID PID(0.32,0.000,0.1,0.001);
//servo(PWM)
Servo servoEDF(PC_8);
@@ -79,9 +79,9 @@
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;
+const float tuning2 = 0.04;
+const float tuning3 = 0.0;
+const float tuning4 = 0.04;
float vcurr = v0;
@@ -101,7 +101,7 @@
//data sensor garis dan line following
int datasensor[6];
-int over;
+int g_flag;
///////
void aktuator();
@@ -109,6 +109,10 @@
void init_sensor();
void line(float speed);
+void majulf(float i, float j, float k, float l);
+void kirilf(float e, float f, float g, float h);
+void kananlf(float a, float b, float c, float d);
+
/*********************************************************************************************/
/*********************************************************************************************/
/** PROGRAM UTAMA **/
@@ -147,14 +151,14 @@
//kondisi robot
bool manual=true;
- /*
+
while(1)
{
ps2.poll();
init_sensor();
if(ps2.read(PS_PAD::PAD_X)==1){
- line(0.3);
+ line(0.35);
}
else{
motor1.brake(1);
@@ -169,7 +173,7 @@
}
pc.printf("\n");
- }*/
+ }
while(manual){
@@ -181,6 +185,34 @@
if(limit3==0) manual = false;
}
+ /*
+ while(1){
+ kananlf(0.1,0.1,0.1,0.1);
+ wait_ms(3000);
+ motor1.brake(1);
+ motor2.brake(1);
+ motor3.brake(1);
+ motor4.brake(1);
+ wait_ms(3000);
+ kirilf(0.1,0.1,0.1,0.1);
+ wait_ms(3000);
+ motor1.brake(1);
+ motor2.brake(1);
+ motor3.brake(1);
+ motor4.brake(1);
+ wait_ms(3000);
+ majulf(0.1,0.1,0.1,0.1);
+ wait_ms(3000);
+ motor1.brake(1);
+ motor2.brake(1);
+ motor3.brake(1);
+ motor4.brake(1);
+ wait_ms(3000);
+ }*/
+
+ //bool lf = true;
+ //int v_sensor;
+
motor1.brake(1);
motor2.brake(1);
@@ -210,6 +242,7 @@
/*********************************************************************************************/
/** ALGORITMA PROSEDUR DAN FUNGSI **/
/*********************************************************************************************/
+
void aktuator(){
float speed = vcurr;
@@ -217,20 +250,20 @@
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;
@@ -391,6 +424,7 @@
}
}
+
void edf_servo(){
if(ps2.read(PS_PAD::PAD_X)==1){
//PWM ++
@@ -457,28 +491,28 @@
over=2;
}
else if(datasensor[0] && datasensor[1]){
- pv = -4;
+ pv = -3.5;
}
else if(datasensor[4] && datasensor[5]){
- pv = 4;
+ pv = 3.5;
}
else if(datasensor[1]){
- pv = -3;
+ pv = -2;
}
else if(datasensor[4]){
- pv = 3;
+ pv = 2;
}
else if(datasensor[1] && datasensor[2]){
- pv = -2;
+ pv = -1;
}
else if(datasensor[3] && datasensor[4]){
- pv = 2;
+ pv = 1;
}
else if(datasensor[2]){
- pv = -1;
+ pv = -0.5;
}
else if(datasensor[3]){
- pv = 1;
+ pv = 0.5;
}
else if(datasensor[2] && datasensor[3]){
pv = 0;
@@ -487,50 +521,70 @@
{
///////////////// 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);
+ //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);
+
+ //motor3.brake(1);
+ //motor2.brake(1);
//wait_ms(100);
}
}
+ /*
+ if(pv > 1){
+ //kanan
+ speed1 = -(0.4-tuning1);
+ speed2 = 0;
+ speed3 = 0;
+ speed4 = -(0.5-tuning4);
+ motor3.brake(1);
+ motor2.brake(1);
+ motor1.speed(speed1);
+ motor4.speed(speed4);
+ }
+ else if(pv < -1){
+ //kiri
+ speed1 = 0;
+ speed2 = -(0.4-tuning2);
+ speed3 = -(0.4-tuning3);
+ speed4 = 0;
+
+ motor3.brake(1);
+ motor2.speed(speed2);
+ motor1.speed(speed1);
+ motor4.brake(1);
+ }
+ else{
+ //gaspoll
+ speed1 = -(0.7-tuning1);
+ speed2 = -(0.7-tuning2);
+ speed3 = -(0.7-tuning3);
+ speed4 = -(0.7-tuning4);
+
+ motor3.speed(speed3);
+ motor2.speed(speed2);
+ motor1.speed(speed1);
+ motor4.speed(speed4);
+ } */
PID.setProcessValue(pv);
PID.setSetPoint(0);
// memulai perhitungan PID
+ //speed1 = speed - tuning1 + PID.compute();
+ //speed2 = speed - tuning2 - PID.compute();
+ //speed3 = speed - tuning3 - PID.compute();
+ //speed4 = speed - tuning4 + PID.compute();
-
- 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;
+ speed1 = -0.74;
+ speed2 = -0.74;
+ speed3 = -0.8;
+ speed4 = -0.8;
+
+ /*if(speed1 > speed) speed1 = speed;
else if(speed1 < gMin_speed) speed1 = gMin_speed;
if(speed2 > speed) speed2 = speed;
@@ -541,10 +595,15 @@
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
+ //motor3.speed(-speed3);
+ //motor2.speed(-speed2);
+ //motor1.speed(-speed1);
+ //motor4.speed(-speed4);
+
+ motor3.speed(speed3);
+ motor2.speed(speed2);
+ motor1.speed(speed1);
+ motor4.speed(speed4);
+}