Diff: main.cpp

Revision:

12:e07c59c28c29

Parent:

10:f0f0dc3904e0

Child:

13:8ab42383a2ca

--- a/main.cpp	Tue Nov 29 15:26:09 2016 +0000
+++ b/main.cpp	Sat Dec 03 05:43:10 2016 +0000
@@ -54,7 +54,8 @@
 float speed2=0.6;
 float speed3=0.6;
 float speed4=0.6;
-float speedL=0.6;
+float speedB=0.43 ;
+float speedL=0.4;
 
 float KpX=0.5, KpY=0.5, Kp_tetha=0.03;
 
@@ -105,9 +106,6 @@
 float getX();
 float getTetha();
 
-
-
-
 // Inisialisasi  Pin TX-RX Joystik dan PC
 joysticknucleo joystick(PA_0,PA_1);
 Serial pc(USBTX,USBRX);
@@ -233,13 +231,13 @@
 {
     //Servo
     if (ServoGo){
-        servoS.position(10);
+        servoS.position(20);
         wait_ms(500);
-        servoS.position(-70);
+        servoS.position(-28);
         wait_ms(500);
-        servoS.position(10);
+        servoS.position(20);
         wait_ms(500);
-        for (int i = 0; i<=90; i++){
+        for (int i = -0; i<=70; i++){
         servoB.position(i);
         wait_ms(10);
         }
@@ -248,7 +246,7 @@
         ServoGo = false;
         
     }else{
-        servoS.position(10);
+        servoS.position(20);
         servoB.position(0);
     
     }
@@ -256,7 +254,7 @@
     // Motor Atas
     if (Launcher) {
             motorld.speed(speedL);
-            motorlb.speed(speedL);
+            motorlb.speed(speedB);
     }else{
             motorld.speed(0);
             motorlb.speed(0);
@@ -508,13 +506,19 @@
 
 void speedLauncher()
 {
-    if (joystick.R3_click and speedL < 0.7){
-        speedL = speedL + 0.1;}
+    if (joystick.R3_click and speedL < 0.8){
+        speedL = speedL + 0.01;}
     if (joystick.L3_click and speedL > 0.1){
-        speedL = speedL - 0.1;}    
-
+        speedL = speedL - 0.01;} 
+   /* if (joystick.R2>0 and speedB < 0.8){
+        speedB = speedB + 0.01;}
+    if (joystick.L2>0 and speedB > 0.1){
+        speedB = speedB - 0.01;}*/    
+    pc.printf("Pwm depan = %.3f\n ", speedL);
 }
-
+   
+    
+    
 int main (void)
 {
     // Set baud rate - 115200
@@ -530,8 +534,6 @@
     Tetha = 0;
     pc.printf("Ready...\n");
     kalibrasi();
-    servoS.position(90);
-    servoB.position(0);
     waktu.start();
     while(1)
     {
@@ -542,11 +544,12 @@
             joystick.baca_data();
             // Panggil fungsi pengolahan data joystik
             joystick.olah_data();
+            //pc.printf("%3d %3d\n\r",joystick.R2, joystick.L2);
             //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());
+            //pc.printf("bacaS = %.2f\tbacaB = %.2f\n",servoS.read(), servoB.read());
             
             //kalibrasi
             if (joystick.START){
@@ -565,6 +568,7 @@
                 pc.printf("x..\n");
                 }
             speedLauncher();
+         
         } else {
             joystick.idle();