Diff: main.cpp

Revision:

3:1287fccc11be

Parent:

1:56bd3e8f38c5

Child:

4:483c07cc22e1

--- a/main.cpp	Sat Jun 27 07:37:28 2015 +0000
+++ b/main.cpp	Sun Oct 16 06:27:39 2016 +0000
@@ -1,9 +1,7 @@
 /**
-Base 4 Nasional
-
 Case Gerak
-1.  Pivot Kanan
-2.  Pivot Kiri
+1.  cw
+2.  ccw
 3.  Maju
 4.  Mundur
 5.  Serong Atas Kanan
@@ -12,9 +10,12 @@
 8.  Serong Bawah Kiri
 9.  Kanan
 10. Kiri
-11. Stop
+11. Analog kiri base
+12. stop
 
+Urutan motor 1234 searah jarum jam
 
+Source Code dari
 Bima Sahbani            EL'12
 Fanny Achmad Hindrarta  EL'12
 **/
@@ -29,80 +30,93 @@
 //#define koefperlambatan 0.8
 
 // Deklarasi variabel motor
-Motor motor1(PA_15, PA_13, PA_14); // pwm, fwd, rev
-Motor motor2(PA_0, PC_14, PC_15); // pwm, fwd, rev
-Motor motor3(PA_1, PH_1, PH_0); // pwm, fwd, rev
-Motor motor4(PC_6, PC_9, PC_8); // pwm, fwd, rev
-
-// Deklarasi Register Pneumatik
-DigitalOut pneumatik1(PC_11);
-DigitalOut pneumatik2(PD_2);
-
-// Deklarasi Timer Pneumatik
-Timer timer_pneu;
+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
 
 // Inisialisasi  Pin TX-RX Joystik dan PC
-joysticknucleo joystick(PA_11,PA_12);
-//Serial pc(USBTX,USBRX);
+joysticknucleo joystick(PA_0,PA_1);
+Serial pc(USBTX,USBRX);
 
 //bool perlambatan=0;
 char case_ger;
-bool maju=false,mundur=false,kiri=false,kanan=false,saka=false,saki=false,sbka=false,sbki=false,pivki=false,pivka=false,cw1=false,ccw1=false,cw2=false,ccw2=false,cw3=false,ccw3=false;
+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;
-bool t1, t2, pneu1, pneu2;
-int delay_pneumatik;
-double vcurr;
+int jLX,jLY;
+double vcurr=0;
+float x,y,xk,yk; // untuk analog, x sebelum kalibrasi xk sesudah kalibrasi
+float errorx=0,errory=0;
+
+// Fungsi mapping 0-255 ke -1 sampai 1
+float mapping(int x)
+{   
+    float hasil;
+    hasil = ((x+1)/128)-1;
+    
+    return (hasil);
+}
 
+// simpan data analog
+void baca_analog()
+{
+        jLX = joystick.LX;
+        jLY = joystick.LY;
+        
+        // Pembacaan nilai Y terbalik
+        x = mapping(jLX);
+        y = -mapping(jLY); 
+            
+        // Setelah di kalibrasi
+        xk = x + errorx;
+        yk = y + errory;
+}
 
+// Gerak dari Motor base
 int case_gerak()
 {
     int casegerak;
-    if (!joystick.L1 && joystick.R1) {
+    baca_analog();
+     if (!joystick.L1 && joystick.R1) {
         // Pivot Kanan
         casegerak = 1;
     } else if (!joystick.R1 && joystick.L1) {
         // Pivot Kiri
         casegerak = 2;
-    } else if (((!(joystick.L1&&joystick.R1)) && (joystick.LX>110 && joystick.LX<190) && (joystick.LY<=50)) || ((joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) ) {  
+    } else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {  
         // Maju
-        casegerak = 3; 
-    } else if (((!(joystick.L1&&joystick.R1)) && (joystick.LX>90 && joystick.LX<190) && (joystick.LY>=200) )|| ((!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)))  {  
+        casegerak = 3;
+    } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri))  {  
         // Mundur
         casegerak = 4;
-    } else if ((!(joystick.L1&&joystick.R1)) && (((joystick.LX>=200)&&(joystick.LY<=50)) || ((joystick.atas)&&(!joystick.bawah)&&(!joystick.kiri)&&(joystick.kanan))))   {   
+    } else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kiri)&&(joystick.kanan))   {   
         // Serong Atas Kanan
         casegerak = 5;
-    } else if(((!(joystick.L1&&joystick.R1)) && (((joystick.LX>=200)&&(joystick.LY>=200)) || ((!joystick.atas)&&(joystick.bawah)&&(!joystick.kiri)&&(joystick.kanan))))) {   
+    } else if((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kiri)&&(joystick.kanan)) {   
         // Serong Bawah Kanan
         casegerak = 6;
-    } else if ((!(joystick.L1&&joystick.R1)) && (((joystick.LX<=50)&&(joystick.LY<=50)) || ((joystick.atas)&&(!joystick.bawah)&&(joystick.kiri)&&(!joystick.kanan)))) {   
+    } else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(joystick.kiri)&&(!joystick.kanan)) {   
         // Serong Atas Kiri
         casegerak = 7;
-    } else if ((!(joystick.L1&&joystick.R1)) && (((joystick.LX<=50)&&(joystick.LY>=200)) || ((!joystick.atas)&&(joystick.bawah)&&(joystick.kiri)&&(!joystick.kanan)))) {   
+    } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(joystick.kiri)&&(!joystick.kanan)) {   
         // Serong Bawah Kiri
         casegerak = 8;
-    } else if (((!(joystick.L1&&joystick.R1)) && (joystick.LX>=210) && (joystick.LY>80 && joystick.LY<200))|| ((!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) )  {   
+    } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri))  {   
         // Kanan
         casegerak = 9;
-    } else if (((!(joystick.L1&&joystick.R1)) && (joystick.LX<=50) && (joystick.LY>80 && joystick.LY<200))|| ((!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri))) {   
+    } else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {   
         // Kiri
-        casegerak = 10;
-    } else {
-        // Stop
-        casegerak = 11;
-    }
-
-    if(joystick.silang_click && t1==0 && t2==0) {
-        pneu1 = 1;
-    } else {
-        pneu1 = 0;
-    }
-    
-    if(joystick.kotak_click && t1==0 && t2==0) {
-        pneu2 = 1;
-    } else {
-        pneu2 = 0;
-    }
+        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;
+        } else {
+            casegerak = 12;
+        }    
+}
     return(casegerak);
 }
 
@@ -110,7 +124,7 @@
 
 /**
 
-**  Case 1  : Pivot Kanan
+**  Case 1  : Pivot kanan
 **  Case 2  : Pivot Kiri
 **  Case 3  : Maju
 **  Case 4  : Mundur
@@ -120,47 +134,21 @@
 **  Case 8  : Serong Bawah Kiri
 **  Case 9  : Kanan
 **  Case 10 : Kiri
-**  Case 11 : Break
+**  Case 11 : Analog
+**  Case 11 : break
 
 **/
+
+// Pergerakan dari base
 void aktuator()
 {
     double koef;
     double s1=0,s2=0,s3=0,s4=0;
     
-    // PNEUMATIK
-    if(t1==1) {
-        if(timer_pneu.read_ms() - delay_pneumatik > 800) {
-            pneumatik1 = 1;
-            t1=0;
-        }
-    }
-    if(t2==1) {
-        if(timer_pneu.read_ms() - delay_pneumatik > 800) {
-            pneumatik2 = 1;
-            t2=0;
-        }
-    }
-    
-    if (pneu1 == 1 || pneu2==1) {
-        timer_pneu.reset();
-        delay_pneumatik = timer_pneu.read_ms();
-        if(pneu1 == 1) {
-            pneumatik1 = 0;
-            t1 = 1;
-            pneu1 = 0;
-        } else if(pneu2 == 1) {
-            pneumatik2 = 0;
-            t2 = 1;
-            pneu2 = 0;
-        }
-        
-    }
-
     // MOTOR
     switch (case_ger) 
     {
-    case (1): 
+case (1): 
         {       
             if (pivka) {
                 if(vcurr<0.1) {
@@ -187,14 +175,14 @@
             }
             
             s1 = (float)(-0.5*koef*vcurr);
-            s2 = (float)(0.5*koef*vcurr);
+            s2 = (float)(-0.5*koef*vcurr);
             s3 = (float)(-0.5*koef*vcurr);
-            s4 = (float)(0.5*koef*vcurr);    
+            s4 = (float)(-0.5*koef*vcurr);    
             
             pivka=true;         
-            maju=mundur=kiri=kanan=saka=saki=sbka=sbki=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
+            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);
@@ -229,14 +217,14 @@
             }
             
             s1 = (float)(0.5*koef*vcurr);
-            s2 = (float)(-0.5*koef*vcurr);
+            s2 = (float)(0.5*koef*vcurr);
             s3 = (float)(0.5*koef*vcurr); 
-            s4 = (float)(-0.5*koef*vcurr);    
+            s4 = (float)(0.5*koef*vcurr);    
             
             pivki=true; 
-            maju=mundur=kiri=kanan=saka=saki=sbka=sbki=pivka=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
+            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);
@@ -282,9 +270,9 @@
             //s4 =koef*vcurr;    
             
             maju=true;             
-            mundur=kiri=kanan=saka=saki=sbka=sbki=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
+            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);
@@ -324,9 +312,9 @@
             s4 = (float)(1*koef*(vcurr+0.005));
 
             mundur=true; 
-            maju=kiri=kanan=saka=saki=sbka=sbki=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
+            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);
@@ -364,9 +352,9 @@
             s4 = (float)(0);                 //-koef*0.1*vcurr;                
             
             saka=true; 
-            maju=mundur=kiri=kanan=sbka=saki=sbki=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
+            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);
@@ -408,9 +396,9 @@
             s4 = (float)(koef*vcurr);
             
             sbka=true; 
-            maju=mundur=kiri=kanan=saka=saki=sbki=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
+            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);
@@ -452,9 +440,9 @@
             s4 = (float)(-koef*vcurr);  
             
             saki=true; 
-            maju=kiri=kanan=saka=mundur=sbka=sbki=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
+            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);
@@ -496,9 +484,9 @@
             s4 = (float)(0);                 //koef*0.1*vcurr;
             
             sbki=true; 
-            maju=kiri=kanan=saka=saki=sbka=mundur=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
+            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);
@@ -540,9 +528,9 @@
             s4 =(float)(1.0*koef*vcurr);           
             
             kanan=true; 
-            maju=kiri=mundur=saka=saki=sbka=sbki=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
+            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);
@@ -581,9 +569,9 @@
             s4 =(float)(-1.0*koef*vcurr);
             
             kiri=true; 
-            maju=kanan=mundur=saka=saki=sbka=sbki=pivka=pivki=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
+            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);
@@ -592,7 +580,52 @@
               
             break;
         }
-    default :
+    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) {
+                koef=0.5;
+            }
+            else { 
+                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)));    
+            
+                                  
+                       
+            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);
+            
+       
+                motor1.speed(s1);
+                motor2.speed(s2);
+                motor3.speed(s3);
+                motor4.speed(s4);
+            break;
+    }
+        default :
         {
             //if (mundur||kiri||kanan||saka||saki||sbka||sbki||pivki||pivka||cw1||ccw1||cw2||ccw2||cw3||ccw3) wait_ms(100);
             //if (maju && (vcurr>=0.5)) wait_ms(100); 
@@ -618,28 +651,32 @@
                 motor4.brake(1);
             //}
 
-            maju=mundur=kiri=kanan=saka=saki=sbka=sbki=pivki=pivka=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
+            maju=mundur=kiri=kanan=saka=saki=sbka=sbki=analog=cw1=ccw1=cw2=ccw2=cw3=ccw3=false;
             stop = true;
 
             //s1 = 0;s2 =0; s3 =0; s4 =0;
 
-            //pc.printf("Stop\n");
-        }
+            pc.printf("Stop\n");
+        }  
     }
 }
 
+// Kalibrasi tombol analog kiri
+void kalibrasi()
+{
+    errorx = 0-x;
+    errory = 0-y;    
+
+}
+
 
 int main (void)
 {
     // Set baud rate - 115200
     joystick.setup();
-    //pc.baud(115200);
-    //pc.printf("Ready...\n");
-    timer_pneu.start();
-    pneumatik1=1;
-    pneumatik2=1;
-    t1=0;
-    t2=0;
+    pc.baud(115200);
+    pc.printf("Ready...\n");
+
     while(1)
     {
         // Interrupt Serial
@@ -651,7 +688,11 @@
             joystick.olah_data();
             //pc.printf("%2x %2x %2x %2x %3d %3d %3d %3d %3d %3d\n\r",joystick.button, joystick.RL, joystick.button_click, joystick.RL_click, joystick.R2, joystick.L2, joystick.RX, joystick.RY, joystick.LX, joystick.LY);
             case_ger = case_gerak();
-            aktuator();  
+            aktuator();
+            //kalibrasi
+            if (joystick.START_click){
+                kalibrasi();
+            }  
 
         } else {
             joystick.idle();