Diff: main.cpp

Revision:

27:68efb1622985

Parent:

26:256160a1a82d

Child:

28:2d0746dc2d7d

--- a/main.cpp	Fri Feb 10 13:22:02 2017 +0000
+++ b/main.cpp	Fri Feb 10 17:59:57 2017 +0000
@@ -54,20 +54,23 @@
 #define PERPINDAHAN 1       // Perpindahan ke kanan dan kiri
 
 // Variable Atas
-double speed, maxSpeed = 0.8, minSpeed = -0.8;
+double speed,speed2, maxSpeed = 0.8, minSpeed = 0;
 double kpA=0.6757, kdA=0.6757, kiA=0.00006757;
 double p,i,d;
+double p2,i2,d2;
 double last_error = 0, current_error, sum_error = 0;
-double rpm, target_rpm = 9.0;
+double last_error2 = 0, current_error2, sum_error2 = 0;
+float rpm, target_rpm = 2.0;
+float rpm2, target_rpm2 = 4.0;
  
 // Variable Bawah
 float Vt;
 float k_enc     = PI*D_ENCODER;
 float k_robot   = PI*D_ROBOT;
 float speedT    = 0.2;
-float speedB    = 0.43;
-float speedL    = 0.4;
-float vpid = 0;
+float vpid      = 0;
+float pwmP      = 0.5;
+float pwmT      = -0.8;
 
 
 /* Deklarasi Encoder Base */
@@ -75,19 +78,25 @@
 
 /* Deklarasi Encoder Launcher */
 encoderKRAI encoderAtas( PB_13, PB_14, 14, encoderKRAI::X2_ENCODING);
+encoderKRAI encoderAtas2( PB_15, PC_4, 14, encoderKRAI::X2_ENCODING);
 
 /* Deklarasi Motor Base */
 Motor motor1(PB_9, PC_5, PA_12);    // pwm, fwd, rev, Motor Depan
 Motor motor2(PB_6, PB_1, PB_12);    // pwm, fwd, rev, Motor Belakang
 
 /* Deklarasi Motor Launcher */
-Motor motor3(PA_8,PC_1,PC_2);    // pwm ,fwd, rev, Motor Atas
-//Motor motorld(PA_8, PC_1, PC_2);   // pwm, fwd, rev
-//Motor motorlb(PA_0, PA_4, PC_15 ); // pwm, fwd, rev
+Motor motorL1(PA_8,PC_1,PC_2);    // pwm ,fwd, rev, Motor Launcher1
+Motor motorL3(PA_10, PC_0, PC_3); // pwm, fwd, rev, Motor Launcher2 
+Motor motorP(PC_7, PC_14, PC_13); // pwm, fwd, rev, Motor Powerscrew                
 
 /* Deklarasi Penumatik Launcher */
 DigitalOut pneumatik(PB_2, 0);
 
+/*Dekalrasi Limit Switch */
+DigitalIn limitA (PC_9, PullUp);
+DigitalIn limitT(PB_10, PullUp);
+DigitalIn limitB (PC_8, PullUp);
+
 /* Deklarasi Servo */
 //Servo servoS(PB_2);
 //Servo servoB(PA_5);
@@ -95,7 +104,7 @@
 /**
  *  posX dan posY berdasarkan arah robot
  *  encoder Depan & Belakang sejajar sumbu Y
- *  encoder Kanan & Kiri sejajar sumbu X 
+ *  encoder Kanan & Kirim sejajar sumbu X 
  **/
 
 /* Variabel Encoder */
@@ -112,11 +121,13 @@
 /* Deklarasi Variable Milis */
 unsigned long int previousMillis = 0;
 unsigned long int currentMillis;
+unsigned long int previousMillis2 = 0;
+unsigned long int currentMillis2;
 
 /* Variabel Stick */
 char case_ger;
 bool launcher = false;
-//bool pneumatikGo = false;
+bool reload = false;
 
 /****************************************************/
 /*         Deklarasi Fungsi dan Procedure           */
@@ -152,12 +163,12 @@
     else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri))  {   
         // Kanan
         casegerak = 3;
-        pc.printf("kanan");
+//        pc.printf("kanan");
     } 
     else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {   
         // Kiri
         casegerak = 4;
-        pc.printf("kiri");       
+//        pc.printf("kiri");       
     } 
     else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {  
         // Break
@@ -167,35 +178,34 @@
 }
 
 void aktuator(){
-/* Fungsi untuk menggerakkan Penumatik */
-    // Pneumatik
-/*    if (pneumatikGo){
-        servoS.position(20);
-        wait_ms(500);
-        servoS.position(-28);
-        wait_ms(500);
-        servoS.position(20);
-        wait_ms(500);
-        for (int i = -0; i<=70; i++){
-        servoB.position(i);
-        wait_ms(10);
+/* Fungsi untuk menggerakkan Motor PowerScrew */
+    // PowerScrew
+    if (reload){
+        if (!limitA){
+            motorP.brake (1);
         }
-        wait_ms(500);
-        servoB.position(0);
-        servoGo = false;
+        else
+        {
+            motorP.speed(pwmP);
+            if (!limitT)
+            {
+                while (limitB) 
+                {
+                    motorP.speed(pwmT);
+                }
+                motorP.brake(1);
+            }
+        }
+        reload = !reload;
     }
-    else{
-        servoS.position(20);
-        servoB.position(0);
-    }
-*/    
-    //Motor Atas
+/*Motor Atas*/
     if (launcher) {
         startMillis();
         currentMillis = millis();
+        /*Launcher Depan*/
         if (currentMillis-previousMillis>=10)
         {
-            rpm = (double)encoderAtas.getPulses();    
+            rpm = (float)encoderAtas.getPulses();    
             current_error = target_rpm - rpm;
             sum_error = sum_error + current_error;
             p = current_error*kpA;
@@ -203,7 +213,7 @@
             i = sum_error*kiA*10.0;
             speed = p + d + i;
             init_speed();
-            motor3.speed(speed);
+            motorL1.speed(speed);
             last_error = current_error;
             encoderAtas.reset();
           //pc.printf("%.04lf\n",rpm);
@@ -211,7 +221,27 @@
         }
         else
         {
-            motor3.speed(0);
+            encoderAtas.getPulses();
+        }      
+        if (currentMillis2-previousMillis2>=10)
+        {
+            rpm2 = (float)encoderAtas2.getPulses();    
+            current_error2 = target_rpm2 - rpm2;
+            sum_error2 = sum_error2 + current_error2;
+            p2 = current_error2*kpA;
+            d2 = (current_error2-last_error2)*kdA/10.0;
+            i2 = sum_error2*kiA*10.0;
+            speed2 = p2 + d2 + i2;
+            init_speed();
+            motorL3.speed(speed2);
+            last_error2 = current_error2;
+            encoderAtas2.reset();
+          //pc.printf("%.04lf\n",rpm2);
+            previousMillis2 = currentMillis2;
+        }
+        else
+        {
+            encoderAtas2.getPulses();
         }
     }
     // MOTOR Bawah
@@ -249,7 +279,6 @@
         default : {
             motor1.brake(1);
             motor2.brake(1);
-            break;
             }   
     } // End Switch 
 }
@@ -270,19 +299,19 @@
 
 void speedKalibrasiMotor(){
 /* Fungsi untuk speed launcher */
-    if (joystick.R3_click and speedL < 0.8){
-        speedL = speedL + 0.01; // PWM++ Motor Belakang
+    if (joystick.R3_click and target_rpm < 14){
+        target_rpm = target_rpm + 1; // RPM++ Motor Belakang
     }
-    if (joystick.L3_click and speedL > 0.1){
-        speedL = speedL - 0.01; // PWM-- Motor Belakang
+    if (joystick.L3_click and target_rpm > 1){
+        target_rpm = target_rpm - 1; // RPM-- Motor Belakang
     } 
-    if (joystick.R2_click and speedB < 0.8 ){
-        speedB = speedB + 0.01; // PWM++ Motor Depan
+    if (joystick.R2_click and target_rpm2 < 14){
+        target_rpm2 = target_rpm2 + 1; // RPM++ Motor Depan
     }  
-    if (joystick.L2_click and speedB > 0.1 ){
-        speedB = speedB - 0.01; // PWM-- Motor Depan
+    if (joystick.L2_click and target_rpm2 > 1 ){
+        target_rpm2 = target_rpm2 - 1; // RPM-- Motor Depan
     }
-    // pc.printf("Pwm depan = %.3f\t  Pwm belakang = %.3f\n", speedL, speedB);    
+    // pc.printf("Rpm depan = %.4f\t  Rpm belakang = %.4f\n", target_rpm, target_rpm2);    
 }
 
 void init_speed (){
@@ -293,6 +322,13 @@
     if (speed<minSpeed){
         speed = minSpeed;
     }
+    if (speed2>maxSpeed){
+        speed2 = maxSpeed;
+    }
+    
+    if (speed2<minSpeed){
+        speed2 = minSpeed;
+    }
 }  
 /*********************************************************/
 /*                  Main Function                        */
@@ -329,7 +365,8 @@
                 wait_ms(500);
                 pneumatik = 0;
             }
-            speedKalibrasiMotor();  
+            speedKalibrasiMotor();
+            if (joystick.silang_click) reload = !reload;  
         }
         else {
             joystick.idle();