Diff: main.cpp

Revision:

3:d3708c3ed288

Parent:

2:2f7bed7fb055

Child:

4:65d65a108b68

--- a/main.cpp	Tue Feb 16 18:33:25 2016 +0000
+++ b/main.cpp	Wed Feb 17 17:21:18 2016 +0000
@@ -24,6 +24,7 @@
 #include "esc.h"
 #include "Servo.h"
 
+
 /*********************************************************************************************/
 /**     DEKLARASI INPUT OUTPUT                                                              **/
 /*********************************************************************************************/
@@ -52,12 +53,22 @@
 //PID PID(0.992,0.000,0.81,0.001); //(P,I,D, time sampling)
 
 // Motor(pwm, fwd, rev)
-Motor motor2(PC_6, PC_8, PC_9);  //gripper
-Motor motor1(PA_15, PA_14, PA_13); //motor3
-//Motor gripper(PB_5, PB_4, PB_3); 
+//Motor motor2(PC_6, PC_8, PC_9);  //gripper
+//Motor motor1(PA_15, PA_14, PA_13); //motor3
+//Motor gripper(PB_5, PA_11, PA_12); 
 //Motor motor2(PA_3, PC_15, PC_14); //kanan
 //Motor motor1(PA_1, PH_0, PH_1); //kiri
 
+Motor gripper(PA_10, PB_4, PB_5); // pwm, fwd, rev
+Motor motor2(PA_0, PA_6, PA_7); // pwm, fwd, rev
+//Motor motor3(PA_1, PH_1, PH_0); // pwm, fwd, rev
+Motor motor1(PA_1, PC_9, PC_8); // pwm, fwd, rev
+
+DigitalOut pnuematik_lengan(PC_10);
+DigitalOut pnuematik_gripper(PC_11);
+
+Servo servo_gripper(PC_7);
+
 /*
 // Sensor
 DigitalIn S1(PC_0);
@@ -92,9 +103,9 @@
 /*********************************************************************************************/
 /**     DEKLARASI VARIABEL GLOBAL                                                           **/
 /*********************************************************************************************/
-float gMax_speed=0.5; //nilai maksimum kecepatan motor
+float gMax_speed=0.7; //nilai maksimum kecepatan motor
 float gMin_speed=-0.05;  //nilai minimum kecepatan motor
-float gTuning = 0.34;
+float gTuning = 0.16;
 
 unsigned char gMode=0;  //variabel mode driving (manual = 0 otomatis = 1)
 unsigned char gCase=0;  //variabel keadaan proses
@@ -116,6 +127,28 @@
 /*********************************************************************************************/
 /*********************************************************************************************/
 
+void ambil(int c){
+    switch (c)
+    {
+        case 1:    
+            servo_gripper.position(140);
+            pnuematik_lengan=1;
+            wait_ms(500);
+            pnuematik_gripper=0;
+            wait_ms(100);
+        break;
+        case 2:
+            wait_ms(300);
+            servo_gripper.position(40);
+            wait_ms(1000); 
+            pnuematik_gripper=1;
+            wait_ms(800);
+            pnuematik_lengan=0;
+            wait_ms(200);
+        break;
+    }
+}
+
 void init_servo(int i){
     if (i){
         if (sudut>90){
@@ -145,6 +178,8 @@
     }
 }
 
+int count=1;
+
 int main(void){
     //inisialisasi joystick
     ps2.init();
@@ -161,6 +196,9 @@
     pc.baud(115200);
     float speed;
     
+    servo_gripper.position(140);
+    pnuematik_lengan=0;
+    
     while(1)
     {  
     ps2.poll();
@@ -169,7 +207,7 @@
             //MOTOR DEPAN
                     speed = gMax_speed;  
                     
-                    motor1.speed(speed);
+                    motor1.speed(speed-gTuning);
                     motor2.speed(speed);
                     pc.printf("maju \n");
         }
@@ -177,7 +215,7 @@
             //MOTOR BELAKANG
             speed = gMax_speed;
                         
-            motor1.speed(-speed);
+            motor1.speed(-(speed-gTuning));
             motor2.speed(-speed);
             pc.printf("mundur \n");
         }
@@ -185,18 +223,32 @@
             //pivot kiri
             speed = gMax_speed;
                         
-            motor1.speed(-speed*0.8);
-            motor2.speed(speed*0.8);
+            motor1.speed(-(speed*0.9-gTuning));
+            motor2.speed(speed*0.9);
             pc.printf("kiri \n");
         }
         else if((ps2.read(PS_PAD::PAD_R1)==1) && (ps2.read(PS_PAD::PAD_L1)==0)){
             //pivot kanan
             speed = gMax_speed;
                         
-            motor1.speed(speed*0.8);
-            motor2.speed(-speed*0.8);
+            motor1.speed(speed*0.9-gTuning);
+            motor2.speed(-speed*0.9    );
             pc.printf("kanan \n");
         }
+        else if ((ps2.read(PS_PAD::PAD_CIRCLE)==1))
+        {
+            if (count==1){
+                ambil(1);
+                count=2;
+                wait_ms(400);
+            }
+            else
+            {
+                ambil(2);
+                count=1;
+                wait_ms(400);
+            }
+        }
         else 
         {
             motor1.brake(1);
@@ -216,9 +268,9 @@
             pc.printf("slide diam \n");
         }
         
-        /*if(ps2.read(PS_PAD::ANALOG_LY)<=-100){
+        if(ps2.read(PS_PAD::ANALOG_LY)<=-100){
             //POWER WINDOW ATAS
-            gripper.speed(0.75);
+            gripper.speed(1);
             pc.printf("up \n");
         }
         else if(ps2.read(PS_PAD::ANALOG_LY)>=100){
@@ -230,7 +282,7 @@
         {
             gripper.brake(1);
             pc.printf("power diam \n");
-        }*/
+        }
         
         if(ps2.read(PS_PAD::ANALOG_RY)<=-100){
             //PWM ++
@@ -255,7 +307,7 @@
             sudut += 3;
             pc.printf("servo max \n");
         }
-        
+                
         init_servo(lapangan);
         init_pwm();
         myservo.position(sudut);