Diff: main.cpp

Revision:

23:023b522977b2

Parent:

22:4632f58461e0

Child:

24:b3e632cc4533

--- a/main.cpp	Sat Jan 28 10:19:36 2017 +0000
+++ b/main.cpp	Wed Feb 01 15:00:08 2017 +0000
@@ -64,16 +64,16 @@
 encoderKRAI encoderKiri(      PC_4, PB_15, 2000, encoderKRAI::X2_ENCODING);   //inA, inB, pin Indeks (NC = tak ada), resolusi, pembacaan
 
 /* Deklarasi Motor Base */
-Motor motor1(PB_7, PA_14, PA_15);  // pwm, fwd, rev, Motor Depan
-Motor motor2(PB_8, PA_13, PB_0);   // pwm, fwd, rev, Motor Belakang
+Motor motor1(PB_9, PA_12, PC_5);  // pwm, fwd, rev, Motor Depan
+Motor motor2(PB_6, PB_12, PA_7);   // pwm, fwd, rev, Motor Belakang
 
 /* Deklarasi Motor Launcher */
-Motor motorld(PA_8, PC_1, PC_2);   // pwm, fwd, rev
-Motor motorlb(PA_9, PA_4, PC_15 ); // pwm, fwd, rev
+//Motor motorld(PA_8, PC_1, PC_2);   // pwm, fwd, rev
+//Motor motorlb(PA_0, PA_4, PC_15 ); // pwm, fwd, rev
 
 /* Deklarasi Servo Launcher */
-Servo servoS(PB_2);
-Servo servoB(PA_5);
+//Servo servoS(PB_2);
+//Servo servoB(PA_5);
 
 /**
  *  posX dan posY berdasarkan arah robot
@@ -89,7 +89,8 @@
 float getTetha();                   // Fungsi mendapatkan jarak Tetha
 
 /* Inisialisasi  Pin TX-RX Joystik dan PC */
-joysticknucleo joystick(PA_0,PA_1);
+joysticknucleo joystick(PA_9,PA_10);
+Serial pc(USBTX,USBRX);
 
 /* Variabel Stick */
 char case_ger;
@@ -99,7 +100,7 @@
 /*         Deklarasi Fungsi dan Procedure           */
 /****************************************************/
 int case_gerak(){
-/****************************************************/
+/****************************************************
  **  Gerak Motor Base
  **  Case 1  : Pivot kanan
  **  Case 2  : Pivot Kiri
@@ -120,10 +121,12 @@
     else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri))  {   
         // Kanan
         casegerak = 3;
+        pc.printf("kanan");
     } 
     else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {   
         // Kiri
-        casegerak = 4;       
+        casegerak = 4;
+        pc.printf("kiri");       
     } 
     else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {  
         // Break
@@ -135,7 +138,7 @@
 void aktuator(){
 /* Fungsi untuk menggerakkan servo */
     // Servo
-    if (servoGo){
+/*    if (servoGo){
         servoS.position(20);
         wait_ms(500);
         servoS.position(-28);
@@ -163,7 +166,7 @@
             motorld.speed(0);
             motorlb.speed(0);
     }
-    
+*/    
     // MOTOR Bawah
     switch (case_ger) {
         case (1): {       
@@ -215,14 +218,14 @@
     float vt;
     
     errT = Ta-getTetha();
-    Vt = kp_tetha*errT;
+    vt = kp_tetha*errT;
     
-    if (vt>speed1)
-    { vt = speed1; }
-    else if (vt<-speed1)
-    { vt = -speed1; }
+    if (vt>speedT)
+    { vt = speedT; }
+    else if (vt<-speedT)
+    { vt = -speedT; }
     
-    if ((((errT > 0.05) || (errT<-0.05))){ 
+    if (((errT > 0.05) || (errT<-0.05))){ 
         vpid = vt;    
     }
     else{
@@ -253,10 +256,11 @@
 int main (void){
     /* Set baud rate - 115200 */
     joystick.setup();
+    pc.baud(115200);
     wait_ms(1000);
     setCenter();
     wait_ms(500);
-    
+    pc.printf("ready....");
     /* Posisi Awal */
     Tetha = 0;