Diff: main.cpp

Revision:

4:483c07cc22e1

Parent:

3:1287fccc11be

Child:

5:3aa203218306

--- a/main.cpp	Sun Oct 16 06:27:39 2016 +0000
+++ b/main.cpp	Tue Oct 25 13:21:57 2016 +0000
@@ -26,14 +26,15 @@
 #define vmax 1
 #define vmaxserong 0.9
 #define vmaxpivot 0.7
+#define vmaxanalog 0.9
 #define ax 0.005
 //#define koefperlambatan 0.8
 
 // Deklarasi variabel motor
-Motor motor1(PB_6, PA_7 , PB_12); // pwm, fwd, rev
-Motor motor2(PB_9, PA_12, PC_5); // pwm, fwd, rev
-Motor motor3(PB_7, PA_15, PA_14); // pwm, fwd, rev
-Motor motor4(PB_8, PB_1 , PA_13); // pwm, fwd, rev
+Motor motor1(PB_6, PB_4 , PB_5); // pwm, fwd, rev
+Motor motor2(PB_7, PA_4, PC_1); // pwm, fwd, rev
+Motor motor3(PB_8, PC_12, PD_2); // pwm, fwd, rev
+Motor motor4(PB_9, PC_10 , PC_11); // pwm, fwd, rev
 
 // Inisialisasi  Pin TX-RX Joystik dan PC
 joysticknucleo joystick(PA_0,PA_1);
@@ -43,20 +44,33 @@
 char case_ger;
 bool maju=false,mundur=false,pivka=false,pivki=false,kiri=false,kanan=false,saka=false,saki=false,sbka=false,sbki=false,cw1=false,ccw1=false,cw2=false,ccw2=false,cw3=false,ccw3=false,analog=false;
 bool stop = true;
-int jLX,jLY;
+float jLX,jLY;
 double vcurr=0;
-float x,y,xk,yk; // untuk analog, x sebelum kalibrasi xk sesudah kalibrasi
+float x,y; // untuk analoghat kiri
 float errorx=0,errory=0;
 
-// Fungsi mapping 0-255 ke -1 sampai 1
-float mapping(int x)
+// Fungsi mapping 0-255 ke -128 sampai 127
+float mapping(float a,float error)
 {   
-    float hasil;
-    hasil = ((x+1)/128)-1;
-    
+     float hasil,b;
+    b = (float)((a-128)/128);
+    if (b>(error - 0.2) && b<(error + 0.2))
+    {
+        hasil = 0;
+    } else {
+        hasil = b;
+        }
     return (hasil);
 }
 
+// Kalibrasi tombol analog kiri
+void kalibrasi()
+{
+    errorx = (jLX - 128)/128;
+    errory = (jLY - 128)/128;    
+
+}
+
 // simpan data analog
 void baca_analog()
 {
@@ -64,12 +78,8 @@
         jLY = joystick.LY;
         
         // Pembacaan nilai Y terbalik
-        x = mapping(jLX);
-        y = -mapping(jLY); 
-            
-        // Setelah di kalibrasi
-        xk = x + errorx;
-        yk = y + errory;
+        x = mapping(jLX,errorx);
+        y = -mapping(jLY,errory); 
 }
 
 // Gerak dari Motor base
@@ -108,8 +118,6 @@
         // Kiri
         casegerak = 10;        
     } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {  
-        // analog switch mode
-        if (joystick.SELECT_click) {analog=!analog;}
         // case gerak analog on off
         if (analog){
             casegerak = 11;
@@ -143,7 +151,7 @@
 void aktuator()
 {
     double koef;
-    double s1=0,s2=0,s3=0,s4=0;
+    double s1=0,s2=0,s3=0,s4=0,s1t=0,s2t=0,s3t=0,s4t=0;
     
     // MOTOR
     switch (case_ger) 
@@ -235,7 +243,7 @@
            }
     case (3):
         {   
-            if (maju) {
+            if (maju) { 
                 if(vcurr<0.1) {
                     vcurr=0.1;
                 } else {
@@ -582,21 +590,7 @@
         }
     case (11): 
         {          
-            if (analog) {
-                if(vcurr<0.1) {
-                    vcurr=0.1;
-                } else {
-                    vcurr+=ax;
-                }
-                //perlambatan=0;
-            } else { 
-                //perlambatan=1;
-            } 
-            
-            if (vcurr>=vmax) {
-                vcurr=vmax; 
-            }
-            
+                     
             if(joystick.R2==255 && joystick.L2==0) {
                 koef=2;
             } else if (joystick.L2==255 && joystick.R2==0) {
@@ -606,17 +600,22 @@
                 koef=1;
             }
             
-            s1 = (float)(0.5*koef*vcurr*((-xk)+(yk)));
-            s2 = (float)(0.5*koef*vcurr*((-xk)+(-yk)));
-            s3 = (float)(0.5*koef*vcurr*((xk)+(-yk)));
-            s4 = (float)(0.5*koef*vcurr*((xk)+(yk)));    
+            s1t = (vmaxanalog*(-x+y));
+            s2t = (vmaxanalog*(-x-y));
+            s3t = (vmaxanalog*(x-y));
+            s4t = (vmaxanalog*(x+y));
+            
+            s1 = (float)(0.5*koef*s1t);
+            s2 = (float)(0.5*koef*s2t);
+            s3 = (float)(0.5*koef*s3t);
+            s4 = (float)(0.5*koef*s4t);  
             
                                   
                        
             analog=true;
             maju=mundur=kiri=kanan=saka=saki=sbka=sbki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
             
-            pc.printf("analog x= %d  y= %d  /n  ",x,y);
+            pc.printf("analog X =%.2f   Y =%.2f \n  ",x,y);
             
        
                 motor1.speed(s1);
@@ -661,14 +660,6 @@
     }
 }
 
-// Kalibrasi tombol analog kiri
-void kalibrasi()
-{
-    errorx = 0-x;
-    errory = 0-y;    
-
-}
-
 
 int main (void)
 {
@@ -676,6 +667,7 @@
     joystick.setup();
     pc.baud(115200);
     pc.printf("Ready...\n");
+    kalibrasi();
 
     while(1)
     {
@@ -690,10 +682,11 @@
             case_ger = case_gerak();
             aktuator();
             //kalibrasi
-            if (joystick.START_click){
-                kalibrasi();
-            }  
-
+            if (joystick.START){
+                kalibrasi();}
+             // analog switch mode
+            if (joystick.SELECT_click) {analog=!analog;}  
+            //pc.printf(" X =%.2f   Y =%.2f \n  ",x,y);
         } else {
             joystick.idle();