Diff: main.cpp

Revision:

7:4d6a73d924ff

Parent:

6:0e159860e5c6

Child:

8:3cc68df2bebf

--- a/main.cpp	Fri Feb 19 14:18:01 2016 +0000
+++ b/main.cpp	Fri Mar 18 22:45:08 2016 +0000
@@ -11,304 +11,58 @@
 #include "Servo.h"
 
 
+
 /*********************************************************************************************/
 /**     DEKLARASI INPUT OUTPUT                                                              **/
 /*********************************************************************************************/
 // serial pc
 Serial pc(USBTX,USBRX);
 
-// LCD 20x4
-//TextLCD lcd(PC_5, PB_1, PA_7, PC_4, PA_5, PA_6, TextLCD::LCD20x4); //(rs,e,d4,d5,d6,d7)
-
 // joystick PS2
-PS_PAD ps2(PB_15,PB_14,PB_13, PB_12); //(mosi, miso, sck, ss)
-
-
-// tambahan power
-// inisialisasi pwm awal servo
-float pwm = 0.00;
-
-// inisialisasi sudut awal
-int sudut = 0;
-
-//membatasi nilai brushless pada edf
-float min=0;
-float max=0.50;
-
-// PID sensor garis 
-PID PID(0.992,0.000,0.81,0.001); //(P,I,D, time sampling)
+PS_PAD ps2(PB_15,PB_14,PB_13, PB_12); //(mosi, miso, sck, ss) default board lama
 
 // 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, 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);
-DigitalOut pnuematik_atas(PD_2);
-DigitalOut pnuematik_bawah(PC_12);
-
-Servo servo_gripper(PB_9);
-//Deklarasi Input Limit Switch
-
-
-// Sensor
-DigitalIn S1(PC_0);
-DigitalIn S2(PC_1);
-DigitalIn S3(PC_2);
-DigitalIn S4(PC_3);
-DigitalIn S5(PA_0);
-DigitalIn S6(PA_1);
-DigitalIn S7(PA_4);
-DigitalIn S8(PB_0);
-DigitalIn S9(PB_2);
-DigitalIn S10(PB_10);
-DigitalIn S11(PA_10);
-DigitalIn S12(PA_11);
-DigitalIn S13(PA_12);
-DigitalOut calibrate(PA_15);
-
-
-//DigitalIn button(USER_BUTTON);
-
-
-//bool sensor[13];
-
-//DigitalIn limit_switch1(A0);
-//DigitalIn limit_switch2(A1);
-
-
-// Multitasker
-//Ticker timer;
+Motor gripper(PC_6, PC_9, PC_8); //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
 
 
 /*********************************************************************************************/
 /**     DEKLARASI VARIABEL GLOBAL                                                           **/
 /*********************************************************************************************/
-float gMax_speed=0.7; //nilai maksimum kecepatan motor
+float gMax_speed=0.80; //nilai maksimum kecepatan motor
 float gMin_speed=-0.05;  //nilai minimum kecepatan motor
-float gTuning = 0.16;
+float gTuning = 0.09;
 
-float gripper_up = 1;
-float gripper_down = 0.2;
-
-unsigned char gMode=0;  //variabel mode driving (manual = 0 otomatis = 1)
-unsigned char gCase=0;  //variabel keadaan proses
+// tambahan power
+// inisialisasi pwm awal servo
+float pwm = 0.00;
+ 
+// inisialisasi sudut awal
+int sudut = 0;
+//membatasi nilai brushless pada edf
+float min=0;
+float max=0.70;
 
 unsigned char i; // variabel iterasi
 int over=0;
 int lapangan = 1;
 
-bool state_atas = 0;
-bool state_bawah = 0;
+unsigned char gMode=0;  //variabel mode driving (manual = 0 otomatis = 1)
+unsigned char gCase=0;  //variabel keadaan proses
+
+
   
 /*********************************************************************************************/
 /**     DEKLARASI PROSEDUR DAN FUNGSI                                                        **/
 /*********************************************************************************************/     
-void init_sensor()
-{
-    if(ps2.read(PS_PAD::PAD_SQUARE)==1)
-    {
-        calibrate=0;
-    }
-    else
-    {
-            calibrate=1;
-    }
-     
-    sensor[0]=(S1.read()==1);
-    sensor[1]=(S2.read()==1);
-    sensor[2]=(S3.read()==1);
-    sensor[3]=(S4.read()==1);
-    sensor[4]=(S5.read()==1);
-    sensor[5]=(S6.read()==1);
-    sensor[6]=(S7.read()==1);
-    sensor[7]=(S8.read()==1);
-    sensor[8]=(S9.read()==1);
-    sensor[9]=(S10.read()==1);
-    sensor[10]=(S11.read()==1);
-    sensor[11]=(S12.read()==1);
-    sensor[12]=(S13.read()==1);
-}
-
-void PIDrunning()   //menjalankan perintah untuk line follower
-{  
-    int pv;
-    int over=0;
-    float speedR,speedL;
-    
-    //init_sensor();
-    //////////////////logic dari PV (present Value)/////////////////////////
-    if(sensor[0]){
-        pv = -12;
-        over=1;
-    }
-    else if(sensor[12]){
-        pv = 12;
-        over=2;
-    }
-    else if(sensor[0] && sensor[1]){
-        pv = -10;
-    }
-    else if(sensor[11] && sensor[12]){
-        pv = 10;
-    }
-    else if(sensor[1]){
-        pv = -9;
-    }
-    else if(sensor[11]){
-        pv = 9;
-    }
-    else if(sensor[1] && sensor[2]){
-        pv = -8;
-    }
-    else if(sensor[10] && sensor[11]){
-        pv = 8;
-    }
-    else if(sensor[2]){
-        pv = -7;
-    }
-    else if(sensor[10]){
-        pv = 7;
-    }
-    else if(sensor[2] && sensor[3]){
-        pv = -6;
-    }
-    else if(sensor[9] && sensor[10]){
-        pv = 6;
-    }
-    else if(sensor[3]){
-        pv = -5;
-    }
-    else if(sensor[9]){
-        pv = 5;
-    }
-    else if(sensor[3] && sensor[4]){
-        pv = -4;
-    }
-    else if(sensor[8] && sensor[9]){
-        pv = 4;  
-    }
-    else if(sensor[4]){
-        pv = -3;
-    }
-    else if(sensor[8]){
-        pv = 3;
-    }
-    else if(sensor[4] && sensor[5]){
-        pv = -2;
-    }
-    else if(sensor[7] && sensor[8]){
-        pv = 2;
-    }
-    else if(sensor[5]){
-        pv = -1;
-    }  
-     else if(sensor[7]){
-        pv = 1;
-    }
-    else if(sensor[5] && sensor[6]){
-        pv = -0.5;    
-    }
-    else if(sensor[6] && sensor[7]){
-        pv = 0.5;
-    } 
-    else if (sensor[6]){
-        pv = 0;
-    }
-    ///////////////// 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 = gMax_speed - PID.compute();
-    if(speedR > gMax_speed){
-        speedR = gMax_speed;
-        }
-    else if(speedR < gMin_speed)
-        speedR = gMin_speed;
-    motor2.speed(speedR);
-
-    speedL = gMax_speed + PID.compute();
-    if(speedL > gMax_speed)
-        speedL = gMax_speed;
-    else if(speedL < gMin_speed)
-        speedL = gMin_speed;
-    motor1.speed(speedL);
-}
 
 /*********************************************************************************************/
 /*********************************************************************************************/
 /**     PROGRAM UTAMA                                                                       **/
 /*********************************************************************************************/
 /*********************************************************************************************/
-
-    
-void ambil(int c){
-    switch (c)
-    {
-        case 1:    
-            servo_gripper.position(170);
-            pc.printf("grip lost");
-            wait_ms(25);
-            pnuematik_lengan=0;
-            wait_ms(500);
-            
-        break;
-        case 2:
-            pnuematik_gripper=0;
-            wait_ms(600);
-            servo_gripper.position(50);
-            pc.printf("grip get");
-            wait_ms(25);
-            wait_ms(1000); 
-            pnuematik_gripper=1;
-            wait_ms(800);
-            pnuematik_lengan=1;
-            wait_ms(200);
-        break;
-    }
-}
-
 void init_servo(int i){
     if (i){
         if (sudut>90){
@@ -324,10 +78,9 @@
         }
         if (sudut<-90){
             sudut=-90;
-        }    
+         }    
     }
 }
-
 void init_pwm (){
     if (pwm>max){
         pwm = max;
@@ -337,195 +90,160 @@
         pwm = min;
     }
 }
-
+ 
 int count=1;
 
 int main(void){
     //inisialisasi joystick
-    ps2.init();
-    
-    //inisialisasi PID
-    PID.setInputLimits(-15,15);
-    PID.setOutputLimits(-1.0,1.0);
-    PID.setMode(1.0);
-    PID.setBias(0.0);
-    PID.reset();
-    
-    
+    ps2.init();   
     //tambahan power
-    //ESC edf(PB_9,20); //p wm esc pin PC_7
-    Servo myservo(PB_8); //pwm servo pin PA_8
+    ESC edf(PB_8,20); //pwm esc
+    Servo myservo(PB_9); //pwm servo
     //set inisialisasi servo pada posisi 0 
     myservo.position(sudut);
     //set edf pada posisi bukan kalibrasi, yaitu set edf 0
-    //edf.setThrottle(pwm);
-    //edf.pulse();
+    edf.setThrottle(pwm);
+    edf.pulse();
+    
+    float k;
     
     pc.baud(115200);
     float speed;
     
-    servo_gripper.position(140);
-    pnuematik_lengan=1;
-    pnuematik_gripper=1;
-    pnuematik_atas = state_atas;
-    pnuematik_bawah = state_bawah;
-    
     while(1)
     {  
-    ps2.poll();
-    
-        if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==0)){
-            //MOTOR DEPAN
-                    speed = gMax_speed;  
-                    
-                    motor1.speed(speed-gTuning);
-                    motor2.speed(speed);
-                    pc.printf("maju \n");
+        ps2.poll();
+        //init_sensor();
+        if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_L1)==1)){
+            speed = gMax_speed;                     
+            motor1.brake(1);
+            motor2.speed(speed-0.05);            
+            pc.printf("maju serong kiri\n");
+            
+        }
+        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);
+            pc.printf("maju serong kanan\n");
+            
+        }
+        else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_L1)==1)){
+            speed = gMax_speed;                     
+            motor2.brake(1);
+            motor1.speed(-(speed-gTuning-0.2));
+            pc.printf("mundur serong kanan\n");
+            
         }
-        else if((ps2.read(PS_PAD::PAD_X)==1)&&(ps2.read(PS_PAD::PAD_TOP)==1)){
-                    PIDrunning();
-                    pc.printf("PID \t %f \t ",PID.compute());
-                    
-                    for(i=0;i<13;i++)
-                    {
-                        pc.printf("%i \t",sensor[i]);
-                    }
-                    pc.printf("\n");
-        }        
+        else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_R1)==1)){
+            speed = gMax_speed;                     
+            motor2.speed(-(speed-0.2));
+            motor1.brake(1);
+            pc.printf("mundur serong kiri\n");
+            
+        }
+       else if((ps2.read(PS_PAD::PAD_TOP)==1) && (ps2.read(PS_PAD::PAD_BOTTOM)==0)){
+            //MOTOR DEPAN
+            
+            speed = k;
+
+            if (k >= gMax_speed){
+                k = gMax_speed;
+            }
+            
+            motor1.speed(speed-gTuning);
+            motor2.speed(speed);
+            pc.printf("maju \n");
+            
+            k += 0.1;
+        }
         else if((ps2.read(PS_PAD::PAD_BOTTOM)==1) && (ps2.read(PS_PAD::PAD_TOP)==0)){
             //MOTOR BELAKANG
-            speed = gMax_speed;
-                        
+            speed = k;
+
+            if (k >= gMax_speed){
+                k = gMax_speed;
+            }
+                                
             motor1.speed(-speed);
             motor2.speed(-speed);
             pc.printf("mundur \n");
+            
+            k += 0.1;
         }
         else if((ps2.read(PS_PAD::PAD_L1)==1) && (ps2.read(PS_PAD::PAD_R1)==0)){
             //pivot kiri
-            speed = gMax_speed;
-                        
+            speed = gMax_speed;                        
             motor1.speed(-(speed*0.9-gTuning));
             motor2.speed(speed*0.9);
-            pc.printf("kiri \n");
+            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;
-                        
+            speed = gMax_speed;                        
             motor1.speed(speed*0.9-gTuning);
             motor2.speed(-speed*0.9    );
-            pc.printf("kanan \n");
+            pc.printf("pivot kanan \n");
+          
         }
-        else if ((ps2.read(PS_PAD::PAD_CIRCLE)==1))
-        {
-            //mekanisme ambil gripper
-            pc.printf("mekanisme gripper");
-            if (count==1){
-                pc.printf("ambil 1");
-                ambil(1);
-                count=2;
-                wait_ms(400);
-            }
-            else
-            {
-                ambil(2);
-                count=1;
-                pc.printf("ambil 2");
-                wait_ms(400);
-            }
-        }
-        else 
-        {
+        else{
             motor1.brake(1);
             motor2.brake(1);
-        }
-        
-        if(ps2.read(PS_PAD::ANALOG_LX)<=-100){
-            //SLIDER KIRI
-            pc.printf("slide kiri \n");
+            
+            k = 0.6;
         }
-        else if(ps2.read(PS_PAD::ANALOG_LX)>=100){
-            //SLIDER KANAN
-            pc.printf("slide kanan \n");
-        }
-        else
-        {
-            pc.printf("slide diam \n");
-        }
+            
         
-        if(ps2.read(PS_PAD::ANALOG_LY)<=-100){
+        if((ps2.read(PS_PAD::PAD_CIRCLE)==0) && (ps2.read(PS_PAD::PAD_TRIANGLE)==1)){
             //POWER WINDOW ATAS
             
-            gripper.speed(gripper_up);
+            gripper.speed(1);
             pc.printf("up \n");
         }
-        else if(ps2.read(PS_PAD::ANALOG_LY)>=100){
+        else if((ps2.read(PS_PAD::PAD_CIRCLE)==1) && (ps2.read(PS_PAD::PAD_TRIANGLE)==0)){
             //POWER WINDOW BAWAH 
-            if ((state_atas==1) || (state_bawah==1))
-            {
-                gripper_down = 1;
-            }
-            else
-            {
-                gripper_down = 0.2;
-            }
+            
                        
-            gripper.speed(-gripper_down);
+            gripper.speed(-1);
             pc.printf("down \n");
         }
-        else
-        {
+        else{
             gripper.brake(1);
-            pc.printf("power diam \n");
-        }
+
+            pc.printf("diam \n");
+            
+        }           
         
-        if(ps2.read(PS_PAD::ANALOG_RY)<=-100){
+        if(ps2.read(PS_PAD::PAD_X)==1){
             //PWM ++
             pwm += 0.01;
             pc.printf("gaspol \n");
         }
-        
-        if(ps2.read(PS_PAD::ANALOG_RY)>=100){
+        else if(ps2.read(PS_PAD::PAD_SQUARE)==1){
             //PWM--
             pwm -= 0.01;
             pc.printf("rem ndeng \n");
         }
         
-        if(ps2.read(PS_PAD::ANALOG_RX)<=-100){
+        if(ps2.read(PS_PAD::PAD_L2)==1){
             //SERVO --
             sudut -= 3;
             pc.printf("servo min \n");
         }
-        
-        if(ps2.read(PS_PAD::ANALOG_RX)>=100){
+        else if(ps2.read(PS_PAD::PAD_R2)==1){
             //SERVO ++
             sudut += 3;
             pc.printf("servo max \n");
         }
         
-        //gripper pole
-        
-        if ((ps2.read(PS_PAD::PAD_L2)==1))
-        {
-                
-                state_atas = !state_atas;
-                pnuematik_atas= state_atas;
-                wait_ms(300);
-        }
-        else if ((ps2.read(PS_PAD::PAD_R2)==1))
-        {
-                state_bawah = !state_bawah;
-                pnuematik_bawah = state_bawah;
-                wait_ms(300);
-        }
-        
-                        
         init_servo(lapangan);
         init_pwm();
+        edf.setThrottle(pwm);
+        edf.pulse();
         myservo.position(sudut);
         wait_ms(25);
-        //edf.setThrottle(pwm);
-        //edf.pulse();
-        //wait_ms(25); 
+        
+                           
+ 
     }
 }
\ No newline at end of file