Diff: main.cpp

Revision:

1:fc1535231c0d

Parent:

0:edddd373a163

Child:

2:df6c49846367

--- a/main.cpp	Tue Apr 19 02:47:31 2016 +0000
+++ b/main.cpp	Tue Apr 19 13:32:26 2016 +0000
@@ -28,6 +28,18 @@
 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);
+*/
+
 //pnuematik
 DigitalOut pnuematik1(PC_11);
 DigitalOut pnuematik2(PC_10);
@@ -57,7 +69,7 @@
 /*********************************************************************************************/ 
 float gMax_speed = 1;
 float v0 = 0.6;
-float ax = 0.0007;
+float ax = 0.0005;
 
 // inisialisasi pwm awal servo
 double pwm = 0.00;
@@ -154,33 +166,39 @@
 /*********************************************************************************************/ 
 void aktuator(){
     float speed = v0;
-    float tuning = 0.01;
+    float tuning1 = 0.0;
+    float tuning2 = 0.23;
+    float tuning3 = 0.1;
+    float tuning4 = 0.26;
+
+    if(v0 >= gMax_speed)    v0 = gMax_speed;
+    
         if((ps2.read(PS_PAD::PAD_L1)==1) && (ps2.read(PS_PAD::PAD_R1)==0)){
             //pivot kiri                      
-            motor2.speed(speed*0.7);
-            motor4.speed(-speed*0.7);
-            motor1.speed(speed*0.7-tuning);
-            motor3.speed(-(speed*0.7-tuning));
+            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));
             pc.printf("pivot kiri \n");
             
             v0 += ax;
         }
         else if((ps2.read(PS_PAD::PAD_R1)==1) && (ps2.read(PS_PAD::PAD_L1)==0)){
             //pivot kanan                      
-            motor2.speed(-speed*0.7);
-            motor4.speed(speed*0.7);
-            motor1.speed(-(speed*0.7-tuning));
-            motor3.speed(speed*0.7-tuning);
+            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));
             pc.printf("pivot kanan \n");
             
             v0 += ax;
         }
         else if((ps2.read(PS_PAD::PAD_RIGHT)==1) && (ps2.read(PS_PAD::PAD_TOP)==1)){
             //serong atas kanan                      
-            motor2.speed(speed);
+            motor2.speed(speed-tuning2);
             motor4.brake(1);
             motor1.brake(1);
-            motor3.speed(speed-tuning);
+            motor3.speed(speed-tuning3);
             pc.printf("serong atas kanan \n");
             
             v0 += ax;
@@ -188,8 +206,8 @@
         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);
-            motor1.speed(-(speed-tuning));
+            motor4.speed(-(speed-tuning4));
+            motor1.speed(-(speed-tuning1));
             motor3.brake(1);
             pc.printf("serong atas kiri \n");
             
@@ -197,10 +215,10 @@
         }
         else if((ps2.read(PS_PAD::PAD_LEFT)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==1)){
             //serong bawah kiri                     
-            motor2.speed(-speed);
+            motor2.speed(-(speed-tuning2));
             motor4.brake(1);
             motor1.brake(1);
-            motor3.speed(-(speed-tuning));
+            motor3.speed(-(speed-tuning3));
             pc.printf("serong bawah kiri \n");
             
             v0 += ax;
@@ -208,8 +226,8 @@
         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);
-            motor1.speed(speed-tuning);
+            motor4.speed(speed-tuning4);
+            motor1.speed(speed-tuning1);
             motor3.brake(1);
             pc.printf("serong bawah kanan \n");
             
@@ -217,40 +235,40 @@
         }
         else if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==0)){
             //maju
-            motor1.speed(-(speed-tuning));
-            motor3.speed(speed-tuning);
-            motor2.speed(speed);
-            motor4.speed(-speed);
+            motor1.speed(-(speed-tuning1));
+            motor3.speed(speed-tuning3);
+            motor2.speed(speed-tuning2);
+            motor4.speed(-(speed-tuning4));
             pc.printf("maju \n");
             
             v0 += ax;
         }
         else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_TOP)==0)){
             //mundur     
-            motor1.speed(speed-tuning);
-            motor3.speed(-(speed-tuning));
-            motor2.speed(-speed);
-            motor4.speed(speed);
+            motor1.speed(speed-tuning1);
+            motor3.speed(-(speed-tuning3));
+            motor2.speed(-(speed-tuning2));
+            motor4.speed(speed-tuning4);
             pc.printf("mundur \n");
             
             v0 += ax;
         } 
         else if((ps2.read(PS_PAD::PAD_RIGHT)==1) && (ps2.read(PS_PAD::PAD_LEFT)==0)){
             //kanan                     
-            motor2.speed(speed);
-            motor4.speed(speed);
-            motor1.speed(speed);
-            motor3.speed(speed);
+            motor2.speed(speed-tuning2);
+            motor4.speed(speed-tuning4);
+            motor1.speed(speed-tuning1);
+            motor3.speed(speed-tuning3);
             pc.printf("kanan \n");
             
             v0 += ax;
         }
         else if((ps2.read(PS_PAD::PAD_RIGHT)==0) && (ps2.read(PS_PAD::PAD_LEFT)==1)){
             //kiri                       
-            motor2.speed(-speed);
-            motor4.speed(-speed);
-            motor1.speed(-speed);
-            motor3.speed(-speed);
+            motor2.speed(-(speed-tuning2));
+            motor4.speed(-(speed-tuning4));
+            motor1.speed(-(speed-tuning1));
+            motor3.speed(-(speed-tuning3));
             pc.printf("kiri \n");
             
             v0 += ax;