Diff: main.cpp

Revision:

9:226d1137453e

Parent:

8:3cc68df2bebf

--- a/main.cpp	Mon Mar 21 12:10:27 2016 +0000
+++ b/main.cpp	Mon Apr 18 05:14:51 2016 +0000
@@ -18,25 +18,14 @@
 /*********************************************************************************************/
 // serial pc
 Serial pc(USBTX,USBRX);
-
-//sensor ping
-Ping pinger(PA_1);
-//read jarak
-int read_ping(){
-    pinger.Send();
-    wait_ms(30);
-    return ((pinger.Read_cm())/2);
-}
+Serial sensor(PA_0,PA_1);
 
 // 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_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
-*/
+
+// PID sensor garis 
+PID PID(0.992,0.000,0.81,0.001); //(P,I,D, time sampling)
+
 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
@@ -45,6 +34,13 @@
 DigitalOut limit0(PC_0,PullUp);
 DigitalOut limit1(PC_1,PullUp);
 
+DigitalOut pnuematik_lengan(PC_12);
+DigitalOut pnuematik_gripper(PC_11);
+
+ESC edf(PC_6,20); //pwm esc PB_8
+Servo myservo(PC_8); //pwm servo PB_9
+
+Timer timer;
 
 /*********************************************************************************************/
 /**     DEKLARASI VARIABEL GLOBAL                                                           **/
@@ -70,17 +66,20 @@
 unsigned char gMode=0;  //variabel mode driving (manual = 0 otomatis = 1)
 unsigned char gCase=0;  //variabel keadaan proses
 
+int c=0;    //motor gripper auto
+int g=2;    //pnuematik kondisi gripper
+int count=1;
 
-  
+float k;
+float speed;
+
+int datasensor[6];
+
 /*********************************************************************************************/
 /**     DEKLARASI PROSEDUR DAN FUNGSI                                                        **/
 /*********************************************************************************************/     
 
 /*********************************************************************************************/
-/*********************************************************************************************/
-/**     PROGRAM UTAMA                                                                       **/
-/*********************************************************************************************/
-/*********************************************************************************************/
 void init_servo(int i){
     if (i){
         if (sudut>60){
@@ -99,6 +98,7 @@
          }    
     }
 }
+
 void init_pwm (){
     if (pwm>max){
         pwm = max;
@@ -108,32 +108,10 @@
         pwm = min;
     }
 }
- 
-int count=1;
 
-int main(void){
-    //inisialisasi joystick
-    ps2.init();   
-    //tambahan power
-    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(0);
 
-    //set edf pada posisi bukan kalibrasi, yaitu set edf 0
-    edf.setThrottle(pwm);
-    edf.pulse();
-    
-    float k;
-    
-    pc.baud(115200);
-    float speed;
-    
-    while(1)
-    {  
-        ps2.poll();
-        //init_sensor();
-        if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_L1)==1)){
+void motor_base(void){
+    if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_L1)==1)){
             speed = gMax_speed;                     
             motor1.brake(0.2);
             motor2.speed(speed-0.05);            
@@ -211,36 +189,69 @@
             
             k = 0.6;
         }
-            
-        
+}
+
+void motor_gripper(){
         if((ps2.read(PS_PAD::PAD_CIRCLE)==0) && (ps2.read(PS_PAD::PAD_TRIANGLE)==1)){
             //POWER WINDOW ATAS
-            gripper.speed(1);
+        /*  do{
+                motor_base();
+                gripper.speed(1);
+            }while((limit0 != 0) && (c > 7));
+          */
+            gripper.speed(0.9);
             
             if (limit0 == 0){
                 gripper.brake(1);
             }
+            
+            
             pc.printf("up \n");
+            c++;
         }
         else if((ps2.read(PS_PAD::PAD_CIRCLE)==1) && (ps2.read(PS_PAD::PAD_TRIANGLE)==0)){
             //POWER WINDOW BAWAH 
+           /*do{
+                motor_base();
+                gripper.speed(-0.8);
+            }while((limit1 != 0) && (c > 7));
+            */
                        
-            gripper.speed(-1);
+            gripper.speed(-0.7);
             
             if (limit1 ==0){
                 gripper.brake(1);
             }
             
             pc.printf("down \n");
+            c--;        
         }
         else{
             gripper.brake(1);
-
-            pc.printf("diam \n");
+            if ((c <= 7) && (c>=-7)){
+                c=0;
+            }
             
+            pc.printf("diam \n");
         }           
-        
-        if(ps2.read(PS_PAD::PAD_X)==1){
+        if((c > 7) && (limit0 == 0)){
+            c = 0;
+            gripper.brake(1);
+        }
+        else if((c < -7) && (limit1 == 0)){
+            c = 0;
+            gripper.brake(1);
+        }
+        else if( (c > 7) && (limit0 != 0)){
+            gripper.speed(1);
+        }
+        else if ((c<-7) && (limit1 != 0)){
+            gripper.speed(-0.9);
+        }
+}
+
+void servo_edf(){
+    if(ps2.read(PS_PAD::PAD_X)==1){
             //PWM ++
             pwm += 0.01;
             pc.printf("gaspol \n");
@@ -262,12 +273,211 @@
             pc.printf("servo max \n");
         }
         
+        if(ps2.read(PS_PAD::PAD_START)==1){
+            
+            sudut = -60;
+            pwm = 0.18;
+        }
+        
         init_servo(lapangan);
         init_pwm();
         edf.setThrottle(pwm);
         edf.pulse();
         wait_ms(25);
         myservo.position(sudut);
+}
+
+void pnuematik_running(){
+    if ((ps2.read(PS_PAD::PAD_SELECT)==1))
+        {
+            //mekanisme ambil gripper
+            pc.printf("mekanisme gripper");
+            if (g==1){
+                pc.printf("ambil 1");
+                pnuematik_lengan = 1;
+                pnuematik_gripper = 1;
+                g=2;
+                wait_ms(400);
+            }
+            else
+            {
+                pnuematik_gripper = 0;
+                wait_ms(400);
+                pnuematik_lengan = 0;
+                
+                g=1;
+                pc.printf("ambil 2");
+                wait_ms(400);
+            }
+        }
+}
+
+void line_follower(float speed){    
+    int pv;  
+    float speedR,speedL;
+         
+        //////////////////logic dari PV (present Value)/////////////////////////
+        if(datasensor[0]){
+            pv = -5;
+            over=1;
+        }
+        else if(datasensor[5]){
+            pv = 5;
+            over=2;
+        }
+        else if(datasensor[0] && datasensor[1]){
+            pv = -4;
+        }
+        else if(datasensor[4] && datasensor[5]){
+            pv = 4;
+        }
+        else if(datasensor[1]){
+            pv = -3;
+        }
+        else if(datasensor[4]){
+            pv = 3;
+        }
+        else if(datasensor[1] && datasensor[2]){
+            pv = -2;
+        }
+        else if(datasensor[3] && datasensor[4]){
+            pv = 2;
+        }
+        else if(datasensor[2]){
+            pv = -1;
+        }
+        else if(datasensor[3]){
+            pv = 1;
+        }
+        else if(datasensor[2] && datasensor[3]){
+            pv = 0;
+        }
+        else
+        {
+            ///////////////// robot bergerak keluar dari sensor/////////////////////
+            if(over==1){
+                /*if(speed_ka > 0){
+                    wait_ms(10);
+                    motor2.speed(-speed_ka);
+                    wait_ms(10);
+                    }
+                else{
+                    motor2.speed(speed_ka);
+                    }
+                wait_ms(10);*/
+                
+                motor1.brake(1);
+                //wait_ms(100);
+                
+            }
+            else if(over==2){
+                /*if(speed_ki > 0){
+                    wait_ms(10);
+                    motor1.speed(-speed_ki);
+                    wait_ms(10);
+                    }
+                else{
+                    wait_ms(10);
+                    motor1.speed(speed_ki);
+                    wait_ms(10);
+                    }
+                wait_ms(10);*/
+                motor2.brake(1);
+                //wait_ms(100);
+            }
+        } 
+        PID.setProcessValue(pv);
+        PID.setSetPoint(0);
         
+        // memulai perhitungan PID
+    
+        speedR = speed - PID.compute();
+        if(speedR > speed){
+            speedR = speed;
+            }
+        else if(speedR < gMin_speed)
+            speedR = gMin_speed;
+        motor2.speed(speedR);
+    
+        speedL = speed + PID.compute();
+        if(speedL > speed)
+            speedL = speed;
+        else if(speedL < gMin_speed)
+            speedL = gMin_speed;
+        motor1.speed(speedL);
+}
+
+void init_sensor(){
+    char data;
+    if(sensor.readable()){  
+        data = sensor.getc();
+        
+        for(int i=0; i<6; i++){
+           datasensor[i] = (data >> i) & 1;
+       }
     }
+}
+
+/*********************************************************************************************/
+/**     PROGRAM UTAMA                                                                       **/
+/*********************************************************************************************/
+/*********************************************************************************************/ 
+int main(void){
+    //inisialisasi serial
+    pc.baud(115200);
+    sensor.baud(115200);
+    
+    //inisialisasi joystick
+    ps2.init();   
+
+    //set inisialisasi servo pada posisi 0 
+    myservo.position(0);
+
+    //set edf pada posisi bukan kalibrasi, yaitu set edf 0
+    edf.setThrottle(pwm);
+    edf.pulse();
+    
+    //inisialisasi gripper propeller
+    pnuematik_lengan = 0;
+    pnuematik_gripper = 0;
+    
+    //inisialisasi PID sensor    
+    PID.setInputLimits(-15,15);
+    PID.setOutputLimits(-1.0,1.0);
+    PID.setMode(1.0);
+    PID.setBias(0.0);
+    PID.reset();
+    
+    bool manual=true;
+   
+   while(manual)
+    {
+        ps2.poll();          
+        
+        motor_base();
+        
+        motor_gripper();
+             
+        pnuematik_running();
+                
+        servo_edf();
+ 
+        if(ps2.read(PS_PAD::ANALOG_RIGHT)==1){
+            manual = false;
+        }
+    }
+    
+    timer.start();
+    while(1)
+    {
+        init_sensor();
+    
+        //line_follower(0.4);
+            
+        for(int i=0; i<6; i++){
+            pc.printf("%d  ",datasensor[i]);
+        }
+        pc.printf("\n");
+    }
+        
 }
\ No newline at end of file