Diff: main.cpp

Revision:

6:68293bed71ea

Parent:

5:3aa203218306

Child:

7:d138c56dab20

--- a/main.cpp	Tue Nov 22 15:30:31 2016 +0000
+++ b/main.cpp	Sun Nov 27 09:38:37 2016 +0000
@@ -17,16 +17,25 @@
 /*   2. Deklarasi penggunaan library omniPos pada bagian deklarasi encoder  */
 /*                                                                          */
 /****************************************************************************/
+/*                                                                          */
+/*    Joystick                                                              */
+/*  kanan => posisi target x ditambah 0.01                                  */
+/*  kiri  => posisi target x dikurang 0.01                                  */
+/*  atas  => posisi target y ditambah 0.01                                  */
+/*  bawah => posisi target y dikurang 0.01                                  */
+/*                                                                          */
+/*  Tombol silang   => Kembali keposisi Awal                                */
+/*  Tombol segitiga => Aktif motor Launcher                                 */
+/*  Tombol kotak    => Aktif servo Launcher                                 */
+/*                                                                          */
+/****************************************************************************/
+
 
 #include "mbed.h"
 #include "JoystickPS3.h"
 #include "Motor.h"
+#include "Servo.h"
 
-#define vmax 1
-#define vmaxserong 0.9
-#define vmaxpivot 0.7
-#define vmaxanalog 0.9
-#define ax 0.005
 //#define koefperlambatan 0.8
 #include "encoderKRAI.h"
 
@@ -34,6 +43,8 @@
 #define diaEncoder 0.058
 #define PPR 1000
 #define diaRobot 0.64
+#define pinservo1 PC_9
+#define pinservo2 PC_8
 
 float K_enc = pi*diaEncoder;
 float K_robot = pi*diaRobot;
@@ -44,13 +55,15 @@
 float speed4=0.6;
 
 float KpX=0.5, KpY=0.5, Kp_tetha=0.03;
+
+Timer waktu;
 //float jarakGlobalY;
 
 // Deklarasi variabel PID
 //PID PID(0.992,0.000,0.81,0.001); //(P,I,D, time sampling)
 
 // Deklarasi variabel encoder
-encoderKRAI encoderDepan( D3, D4, 2000, encoderKRAI::X2_ENCODING);   //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan
+encoderKRAI encoderDepan( PB_14, PB_13, 2000, encoderKRAI::X2_ENCODING);   //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan
 encoderKRAI encoderBelakang( PC_4, PB_15, 2000, encoderKRAI::X2_ENCODING);
 encoderKRAI encoderKanan( PD_2, PC_12, 720, encoderKRAI::X2_ENCODING);
 encoderKRAI encoderKiri( PC_11, PC_10, 2000, encoderKRAI::X2_ENCODING);
@@ -61,6 +74,14 @@
 Motor motor3(PB_6, PA_7 , PB_12); // pwm, fwd, rev
 Motor motor4(PB_7, PA_14 ,PA_15); // pwm, fwd, rev
 
+//Motor Atas
+Motor motorld(PA_8, PB_0 ,PC_15); // pwm, fwd, rev
+Motor motorlb(PA_11, PA_6 ,PA_5); // pwm, fwd, rev
+
+//Servo Atas
+Servo servoS(pinservo1);
+Servo servoB(pinservo2);
+
 // Deklarasi variabel posisi robot
 //robotPos posisi();
 
@@ -100,6 +121,7 @@
 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;
+bool Launcher = false,ServoGo = false;
 float jLX,jLY;
 double vcurr;
 float x,y; // untuk analoghat kiri
@@ -207,8 +229,37 @@
 // Pergerakan dari base
 void aktuator()
 {
+    //Servo
+    /*if (ServoGo){
+        servoS.position(90);
+        servoB.position(-90);
+        if (waktu.read_ms()>=15){
+            pc.printf("Servo samping...");
+        }else if ( waktu.read()<10) {
+        // Delay
+        servoB.position(90);
+        pc.printf("Servo belakang...");
+        } else {
+        // Delay
+        ServoGo = false;
+        }        
+        
+    }else{
+        servoS.position(90);
+        servoB.position(0);
     
-    // MOTOR
+    }*/
+    
+    // Motor Atas
+    if (Launcher) {
+            motorld.speed(0.2);
+            motorlb.speed(0.2);
+    }else{
+            motorld.speed(0);
+            motorlb.speed(0);
+    }
+    
+    // MOTOR Bawah
     switch (case_ger) 
     {
 case (1): 
@@ -458,7 +509,7 @@
     // Set baud rate - 115200
     joystick.setup();
     pc.baud(115200);
-        wait_ms(1000);
+    wait_ms(1000);
     setCenter();
     wait_ms(500);
     
@@ -468,7 +519,9 @@
     Tetha = 0;
     pc.printf("Ready...\n");
     kalibrasi();
-
+    //servoS.position(90);
+    //servoB.position(0);
+    waktu.start();
     while(1)
     {
         // Interrupt Serial
@@ -481,20 +534,26 @@
             //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();
+            
+            pc.printf("bacaS = %.2f\tbacaB = %.2f\n",servoS.read(), servoB.read());
+            
             //kalibrasi
             if (joystick.START){
                 kalibrasi();}
              // analog switch mode
-            if (joystick.SELECT_click) {analog=!analog;}  
-            //pc.printf(" X =%.2f   Y =%.2f \n  ",x,y);
+            if (joystick.SELECT_click)      {analog = !analog;}  
+            if (joystick.segitiga_click)    {Launcher = !Launcher;}
+            if (joystick.lingkaran_click){
+                ServoGo = true;
+                waktu.reset();
+                }  
             if (joystick.silang) {
                 XT = 0;
                 YT = 0;
                 Tetha = 0;
                 pc.printf("x..\n");
                 }
-              
-       
+            
         } else {
             joystick.idle();