Diff: main.cpp

Revision:

17:703072f5dce1

Parent:

16:89093194ccc2

Child:

18:23412e950394

diff -r 89093194ccc2 -r 703072f5dce1 main.cpp
--- a/main.cpp	Thu Jan 12 11:15:01 2017 +0000
+++ b/main.cpp	Sat Jan 14 08:01:09 2017 +0000
@@ -30,10 +30,10 @@
 #define PPR 1000
 #define diaRobot 0.64
 */
-#define vmax 1
-#define vmaxserong 0.9
-#define vmaxpivot 0.7
-#define vmaxanalog 0.9
+#define vmax 0.3    //0.4
+#define vmaxserong 0.3 //0.3
+#define vmaxpivot 0.3   //0.3
+#define vmaxanalog 0.3  //0.3
 #define ax 0.005
 #define pinservo1 PC_8
 #define pinservo2 PC_9
@@ -48,10 +48,10 @@
 
 
 // Deklarasi variabel motor
-Motor motor1(PB_7, PA_14 , PA_15); // pwm, fwd, rev
-Motor motor2(PB_8, PA_13 ,PB_0); // pwm, fwd, rev
-Motor motor3(PB_9, PA_12 ,PC_5); // pwm, fwd, rev
-Motor motor4(PB_6, PB_1 ,PB_12); // pwm, fwd, rev
+Motor motor1(PB_7, PA_15 , PA_14); // pwm, fwd, rev
+Motor motor2(PB_8, PB_0 ,PA_13); // pwm, fwd, rev
+Motor motor3(PB_9, PC_5 ,PA_12); // pwm, fwd, rev
+Motor motor4(PB_6, PB_12 ,PB_1); // pwm, fwd, rev
 
 //Motor Atas
 Motor motorld(PA_8, PC_1 , PC_2); // pwm, fwd, rev
@@ -254,7 +254,7 @@
             pivka=true;         
             maju=mundur=analog=kiri=kanan=saka=saki=sbka=sbki=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
             
-            pc.printf("pivKa\n");
+            //pc.printf("pivKa\n");
             
             motor1.speed(s1);
             motor2.speed(s2);
@@ -296,7 +296,7 @@
             pivki=true; 
             maju=mundur=kiri=analog=kanan=saka=saki=sbka=sbki=pivka=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
             
-            pc.printf("pivKi\n");
+            //pc.printf("pivKi\n");
             
             motor1.speed(s1);
             motor2.speed(s2);
@@ -330,8 +330,7 @@
             }
             
             //Case s1 untuk mode L2 lebih lambat
-            s1 = (float)(-1*koef*(vcurr+0.005));
-            
+            s1 = (float)(-1*koef*(vcurr));
             s2 = (float)(1.0*koef*vcurr); 
             s3 = (float)(1.0*koef*vcurr); 
             s4 = (float)(-1*koef*vcurr);  
@@ -344,7 +343,7 @@
             maju=true;             
             mundur=kiri=kanan=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
 
-            pc.printf("maju\n");
+            //pc.printf("maju\n");
 
             motor1.speed(s1);
             motor2.speed(s2);
@@ -377,16 +376,16 @@
             } else { 
                 koef=1;  
             }
-            //Motor 4  telat mulai
-            s1 = (float)(1*koef*(vcurr-0.008));
-            s2 = (float)(-1*koef*(vcurr-0.005)); 
-            s3 = (float)(-1*koef*(vcurr-0.005)); 
-            s4 = (float)(1*koef*(vcurr+0.005));
+            
+            s1 = (float)(1*koef*(vcurr));
+            s2 = (float)(-1*koef*(vcurr)); 
+            s3 = (float)(-1*koef*(vcurr)); 
+            s4 = (float)(1*koef*(vcurr));
 
             mundur=true; 
             maju=kiri=kanan=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
 
-            pc.printf("mundur\n");
+            //pc.printf("mundur\n");
 
             motor1.speed(s1);
             motor2.speed(s2);
@@ -426,7 +425,7 @@
             saka=true; 
             maju=mundur=kiri=kanan=sbka=saki=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
             
-            pc.printf("saka\n");
+            //pc.printf("saka\n");
             
             motor1.speed(s1);
             motor2.brake(1);
@@ -470,7 +469,7 @@
             sbka=true; 
             maju=mundur=kiri=kanan=saka=saki=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
             
-            pc.printf("sbka\n");
+            //pc.printf("sbka\n");
 
             //motor1.speed(s1);
             motor1.brake(1);
@@ -514,7 +513,7 @@
             saki=true; 
             maju=kiri=kanan=saka=mundur=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
             
-            pc.printf("saki\n");
+            //pc.printf("saki\n");
             
             //motor1.speed(s1);
             motor1.brake(1);
@@ -558,7 +557,7 @@
             sbki=true; 
             maju=kiri=kanan=saka=saki=sbka=mundur=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
             
-            pc.printf("sbki\n");
+            //pc.printf("sbki\n");
             
             motor1.speed(s1);
             //motor2.speed(s2);
@@ -597,12 +596,12 @@
             s1 =(float)(-1*koef*vcurr);
             s2 =(float)(-1.0*koef*vcurr); 
             s3 =(float)(1*koef*vcurr); 
-            s4 =(float)(1.0*koef*vcurr);           
+            s4 =(float)(1.0*koef*(vcurr+0.005));           
             
             kanan=true; 
             maju=kiri=mundur=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
             
-            pc.printf("Kanan\n");
+            //pc.printf("Kanan\n");
             
             motor1.speed(s1);
             motor2.speed(s2);
@@ -636,14 +635,14 @@
             }
             
             s1 =(float)(1*koef*vcurr);
-            s2 =(float)(1*koef*vcurr); 
+            s2 =(float)(1*koef*(vcurr+0.003)); 
             s3 =(float)(-1*koef*vcurr); 
             s4 =(float)(-1.0*koef*vcurr);
             
             kiri=true; 
             maju=kanan=mundur=saka=saki=sbka=sbki=analog=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
             
-            pc.printf("Kiri\n");
+            //pc.printf("Kiri\n");
             
             motor1.speed(s1);
             motor2.speed(s2);
@@ -679,7 +678,7 @@
             analog=true;
             maju=mundur=kiri=kanan=saka=saki=sbka=sbki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
             
-            pc.printf("analog X =%.2f   Y =%.2f \n  ",x,y);
+            //pc.printf("analog X =%.2f   Y =%.2f \n  ",x,y);
             
        
                 motor1.speed(s1);
@@ -699,7 +698,7 @@
             stop = true;
 
 
-            pc.printf("Stop\n");
+            //pc.printf("Stop\n");
         }   
     }  
 }
@@ -755,10 +754,6 @@
             if (joystick.lingkaran_click){
                 ServoGo = true;
                 }  
-            if (joystick.silang) {
-
-                //pc.printf("x..\n");
-                }
             speedLauncher();
          
         } else {