Diff: main.cpp

Revision:

7:d138c56dab20

Parent:

6:68293bed71ea

Child:

8:0711dea61312

--- a/main.cpp	Sun Nov 27 09:38:37 2016 +0000
+++ b/main.cpp	Mon Nov 28 10:31:15 2016 +0000
@@ -44,6 +44,7 @@
 #define PPR 1000
 #define diaRobot 0.64
 #define pinservo1 PC_9
+//PC 9
 #define pinservo2 PC_8
 
 float K_enc = pi*diaEncoder;
@@ -53,6 +54,7 @@
 float speed2=0.6;
 float speed3=0.6;
 float speed4=0.6;
+float speedL=0.2;
 
 float KpX=0.5, KpY=0.5, Kp_tetha=0.03;
 
@@ -76,7 +78,7 @@
 
 //Motor Atas
 Motor motorld(PA_8, PB_0 ,PC_15); // pwm, fwd, rev
-Motor motorlb(PA_11, PA_6 ,PA_5); // pwm, fwd, rev
+Motor motorlb(PA_11, PA_5 ,PA_6); // pwm, fwd, rev
 
 //Servo Atas
 Servo servoS(pinservo1);
@@ -230,30 +232,53 @@
 void aktuator()
 {
     //Servo
-    /*if (ServoGo){
-        servoS.position(90);
-        servoB.position(-90);
-        if (waktu.read_ms()>=15){
+    if (ServoGo){
+  //      servoS.position(90);
+    //    servoB.position(-90);
+      /*  if (waktu.read()<=3){
             pc.printf("Servo samping...");
-        }else if ( waktu.read()<10) {
+            servoS.position(90);
+        }else if ( waktu.read()>8) {
+        // Delay
+        
+        ServoGo = false;
+         } else {
         // Delay
         servoB.position(90);
         pc.printf("Servo belakang...");
-        } else {
-        // Delay
-        ServoGo = false;
+       
         }        
+        */
+        switch (waktu.read)
+    {
+    case (1):
+            {
+                pc.printf("Servo samping...");
+                servoS.position(90);
+            }    
+    case ()
         
+        
+        
+        
+        
+        
+        
+    }    
+    
+    
+    
+    
     }else{
         servoS.position(90);
         servoB.position(0);
     
-    }*/
+    }
     
     // Motor Atas
     if (Launcher) {
-            motorld.speed(0.2);
-            motorlb.speed(0.2);
+            motorld.speed(speedL);
+            motorlb.speed(speedL);
     }else{
             motorld.speed(0);
             motorlb.speed(0);
@@ -503,6 +528,14 @@
 }
 //End Encoder
 
+void speedLauncher()
+{
+    if (joystick.R3_click and speedL < 0.7){
+        speedL = speedL + 0.1;}
+    if (joystick.L3_click and speedL > 0.1){
+        speedL = speedL - 0.1;}    
+
+}
 
 int main (void)
 {
@@ -519,8 +552,8 @@
     Tetha = 0;
     pc.printf("Ready...\n");
     kalibrasi();
-    //servoS.position(90);
-    //servoB.position(0);
+    servoS.position(90);
+    servoB.position(0);
     waktu.start();
     while(1)
     {
@@ -553,7 +586,7 @@
                 Tetha = 0;
                 pc.printf("x..\n");
                 }
-            
+            speedLauncher();
         } else {
             joystick.idle();