Diff: main.cpp

Revision:

8:3cc68df2bebf

Parent:

7:4d6a73d924ff

--- a/main.cpp	Fri Mar 18 22:45:08 2016 +0000
+++ b/main.cpp	Mon Mar 21 12:10:27 2016 +0000
@@ -9,6 +9,7 @@
 #include "millis.h"
 #include "esc.h"
 #include "Servo.h"
+#include "Ping.h"
 
 
 
@@ -18,22 +19,39 @@
 // serial pc
 Serial pc(USBTX,USBRX);
 
-// joystick PS2
-PS_PAD ps2(PB_15,PB_14,PB_13, PB_12); //(mosi, miso, sck, ss) default board lama
+//sensor ping
+Ping pinger(PA_1);
+//read jarak
+int read_ping(){
+    pinger.Send();
+    wait_ms(30);
+    return ((pinger.Read_cm())/2);
+}
 
+// joystick PS2
+PS_PAD ps2(PB_15,PB_14,PB_13, PC_4); //(mosi, miso, sck, ss) default board lama pb_12
+/*
 // Motor(pwm, fwd, rev)
-Motor gripper(PC_6, PC_9, PC_8); //PB_6, PB_8, PB_9
+Motor gripper(PC_6, PC_8, PC_9); //PB_6, PB_8, PB_9
 //Motor slider(PC_6, PC_9, PC_8);
 Motor motor2(PB_3, PB_4, PB_5); //kanan
 Motor motor1(PA_8, PC_7, PA_9); //kiri
+*/
+Motor gripper(PA_10, PB_3, PB_5); //PB_6, PB_8, PB_9
+//Motor slider(PC_6, PC_9, PC_8);
+Motor motor2(PA_11, PB_12, PA_7); //kanan
+Motor motor1(PA_8, PB_4, PB_1); //kiri
+
+DigitalOut limit0(PC_0,PullUp);
+DigitalOut limit1(PC_1,PullUp);
 
 
 /*********************************************************************************************/
 /**     DEKLARASI VARIABEL GLOBAL                                                           **/
 /*********************************************************************************************/
-float gMax_speed=0.80; //nilai maksimum kecepatan motor
+float gMax_speed=0.83; //nilai maksimum kecepatan motor
 float gMin_speed=-0.05;  //nilai minimum kecepatan motor
-float gTuning = 0.09;
+float gTuning = 0.14;
 
 // tambahan power
 // inisialisasi pwm awal servo
@@ -47,7 +65,7 @@
 
 unsigned char i; // variabel iterasi
 int over=0;
-int lapangan = 1;
+int lapangan = 0;
 
 unsigned char gMode=0;  //variabel mode driving (manual = 0 otomatis = 1)
 unsigned char gCase=0;  //variabel keadaan proses
@@ -65,19 +83,19 @@
 /*********************************************************************************************/
 void init_servo(int i){
     if (i){
-        if (sudut>90){
-            sudut=90;
+        if (sudut>60){
+            sudut=60;
             }
-        if (sudut<0){
-            sudut=0;
+        if (sudut<-60){
+            sudut=-60;
             }
     } else {
             
-        if (sudut>0){
-            sudut=0;
+        if (sudut>60){
+            sudut=60;
         }
-        if (sudut<-90){
-            sudut=-90;
+        if (sudut<-60){
+            sudut=-60;
          }    
     }
 }
@@ -97,10 +115,11 @@
     //inisialisasi joystick
     ps2.init();   
     //tambahan power
-    ESC edf(PB_8,20); //pwm esc
-    Servo myservo(PB_9); //pwm servo
+    ESC edf(PC_6,20); //pwm esc PB_8
+    Servo myservo(PC_8); //pwm servo PB_9
     //set inisialisasi servo pada posisi 0 
-    myservo.position(sudut);
+    myservo.position(0);
+
     //set edf pada posisi bukan kalibrasi, yaitu set edf 0
     edf.setThrottle(pwm);
     edf.pulse();
@@ -116,7 +135,7 @@
         //init_sensor();
         if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_L1)==1)){
             speed = gMax_speed;                     
-            motor1.brake(1);
+            motor1.brake(0.2);
             motor2.speed(speed-0.05);            
             pc.printf("maju serong kiri\n");
             
@@ -124,7 +143,7 @@
         else if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_R1)==1)){
             speed = gMax_speed;                     
             motor1.speed(speed-gTuning-0.05);
-            motor2.brake(1);
+            motor2.brake(0.2);
             pc.printf("maju serong kanan\n");
             
         }
@@ -174,15 +193,15 @@
         else if((ps2.read(PS_PAD::PAD_L1)==1) && (ps2.read(PS_PAD::PAD_R1)==0)){
             //pivot kiri
             speed = gMax_speed;                        
-            motor1.speed(-(speed*0.9-gTuning));
-            motor2.speed(speed*0.9);
+            motor1.speed(-(speed*0.95-gTuning));
+            motor2.speed(speed*0.95);
             pc.printf("pivot 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.9-gTuning);
-            motor2.speed(-speed*0.9    );
+            motor1.speed(speed*0.95-gTuning);
+            motor2.speed(-speed*0.95    );
             pc.printf("pivot kanan \n");
           
         }
@@ -196,15 +215,22 @@
         
         if((ps2.read(PS_PAD::PAD_CIRCLE)==0) && (ps2.read(PS_PAD::PAD_TRIANGLE)==1)){
             //POWER WINDOW ATAS
+            gripper.speed(1);
             
-            gripper.speed(1);
+            if (limit0 == 0){
+                gripper.brake(1);
+            }
             pc.printf("up \n");
         }
         else if((ps2.read(PS_PAD::PAD_CIRCLE)==1) && (ps2.read(PS_PAD::PAD_TRIANGLE)==0)){
             //POWER WINDOW BAWAH 
-            
                        
             gripper.speed(-1);
+            
+            if (limit1 ==0){
+                gripper.brake(1);
+            }
+            
             pc.printf("down \n");
         }
         else{
@@ -227,12 +253,12 @@
         
         if(ps2.read(PS_PAD::PAD_L2)==1){
             //SERVO --
-            sudut -= 3;
+            sudut += 3;
             pc.printf("servo min \n");
         }
         else if(ps2.read(PS_PAD::PAD_R2)==1){
             //SERVO ++
-            sudut += 3;
+            sudut -= 3;
             pc.printf("servo max \n");
         }
         
@@ -240,10 +266,8 @@
         init_pwm();
         edf.setThrottle(pwm);
         edf.pulse();
+        wait_ms(25);
         myservo.position(sudut);
-        wait_ms(25);
         
-                           
- 
     }
 }
\ No newline at end of file