Revision 46:85169ae8659b, committed 2017-04-24

Comitter:

Najib_irvani

Date:

Mon Apr 24 14:12:08 2017 +0000

Parent:

45:964ae71a30e3

Commit message:

paling baru

Changed in this revision

diff -r 964ae71a30e3 -r 85169ae8659b PS3Arduino.txt
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PS3Arduino.txt	Mon Apr 24 14:12:08 2017 +0000
@@ -0,0 +1,299 @@
+/*********************************************************************************************/
+/**                                                                                         **/
+/** PROGRAM KOMUNIKASI DATA JOYSTIK PS3                                                     **/
+/**                                                                                         **/
+/** Joystik PS3 -> Arduino -> STM32Nucleo                                                   **/
+/**                                                                                         **/
+/** Fanny Achmad Hindrarta                                                                  **/
+/** EL'12 - 13212076                                                                        **/
+/**                                                                                         **/
+/** Last Update : 01 Februar1 2015, 20.30                                                   **/
+/*********************************************************************************************/
+ 
+#include <PS3BT.h>
+#include <usbhub.h>
+//#include <PS3USB.h>
+#include "Arduino.h"
+// Satisfy IDE, which only needs to see the include statment in the ino.
+#ifdef dobogusinclude
+#include <spi4teensy3.h>
+#endif
+ 
+ 
+USB Usb;
+ 
+// Bluetooth
+// You can create the instance of the class in two ways 
+BTD Btd(&Usb); // You have to create the Bluetooth Dongle instance like so
+//You can create the instance of the class in two ways 
+PS3BT PS3(&Btd); // This will just create the instance
+//PS3BT PS3(&Btd, 0x00, 0x15, 0x83, 0x3D, 0x0A, 0x57); // This will also store the bluetooth address - this can be obtained from the dongle when running the sketch
+/*
+// USB
+// You can create the instance of the class in two ways
+PS3USB PS3(&Usb); // This will just create the instance
+//PS3USB PS3(&Btd, 0x00, 0x15, 0x83, 0x3D, 0x0A, 0x57); // This will also store the bluetooth address - this can be obtained from the dongle when running the sketch
+*/
+ 
+boolean printAngle;
+uint8_t state = 0;
+ 
+// Deklarasi tombol
+boolean kiri_click=0, kanan_click=0, atas_click=0, bawah_click=0;
+boolean segitiga_click=0, lingkaran_click=0, kotak_click=0, silang_click=0;
+boolean L1_click=0, R1_click=0, L3_click=0, R3_click=0;
+boolean START_click=0, SELECT_click=0, PS_click=0;
+ 
+// Deklarasi variabel tombol analog
+unsigned char LX, LY, RX, RY, aL2, aR2;
+ 
+// Deklarasi varibel data yang dikirim
+unsigned char button;
+unsigned char RL;
+unsigned int button_click;
+unsigned int RL_click;
+ 
+void setup_joystik()
+{
+    while (!Serial) // Wait for serial port to connect - used on Leonardo, Teensy and other boards with built-in USB CDC serial connection
+        {
+                Serial.write(0x88);
+        Serial.write(0x09);
+        }
+    if (Usb.Init() == -1) {
+        //Serial.print(F("\r\nOSC did not start"));
+        while (1) //halt
+                {
+                         Serial.write(0x88);
+                 Serial.write(0x09);
+                }
+    }
+    //Serial.print(F("\r\nPS3 USB Library Started"));
+}
+ 
+/*********************************************************************************************/
+/**                                                                                         **/
+/** FUNGSI PENGIRIMAN DATA                                                                  **/
+/** -   Data yang akan dikirim adalah paket data 8-bit dengan urutan sebagai berikut        **/
+/** |------|------|--------|----|--------------|----------|----|----|----|----|----|----|   **/
+/** | 0x88 | 0x08 | button | RL | button_click | RL_click | R2 | L2 | RX | RY | LX | LY |   **/
+/** |------|------|--------|----|--------------|----------|----|----|----|----|----|----|   **/
+/**                                                                                         **/
+/*********************************************************************************************/
+ 
+void kirimdatajoystik()
+{
+    
+    Serial.write(0x88);
+    Serial.write(0x08);
+    Serial.write(button);
+    Serial.write(RL);
+    Serial.write(button_click);
+    Serial.write(RL_click);
+    Serial.write(aR2);
+    Serial.write(aL2);
+    Serial.write(RX);
+    Serial.write(RY);
+    Serial.write(LX);
+    Serial.write(LY);
+ 
+// Debug    
+ 
+/*
+    Serial.print(millis());
+    Serial.print("\t");
+    Serial.print(button);
+    Serial.print("\t");
+    Serial.print(RL);
+    Serial.print("\t");
+    Serial.print(button_click);
+    Serial.print("\t");
+    Serial.print(RL_click);
+    Serial.print("\t");
+    Serial.print(aR2);
+    Serial.print("\t");
+    Serial.print(aL2);
+    Serial.print("\t");
+    Serial.print(RX);
+    Serial.print("\t");
+    Serial.print(RY);
+    Serial.print("\t");
+    Serial.print(LX);
+    Serial.print("\t");
+    Serial.println(LY);
+*/  
+}
+ 
+/*****************************************************************************************/
+/** SETUP REGISTER dan INISIALISASI                                                     **/
+/** -   Setup Joystik                                                                   **/
+/** -   Baud Rate Serial 115200, 8-bit, 1 stop, 0 parity                                **/
+/*****************************************************************************************/
+ 
+void setup() {
+    Serial.begin(115200);
+ 
+    setup_joystik();
+}
+ 
+ 
+/*****************************************************************************************/
+/**                                                                                     **/
+/** FUNGSI PEMBACAAN DATA JOYSTIK                                                       **/
+/** -   Data dari Joystik dikirim melalui Bluetooth.                                    **/
+/**     Lihat Library "PS3BT.h"                                                         **/
+/** -   Data tombol dari joystik adalah data 1-bit. Pengiriman data melalui USART       **/
+/**     adalah 8-bit. Agar pengiriman data efisien, maka data 1-bit digabungkan         **/
+/**     agar terbentuk data 8-bit                                                       **/
+/** -   Penggabungan data segitiga, longkaran, silang, kotak, arah atas, arah kanan,    **/
+/**     arah bawah, arah kiri disimpan dalam variabel "button"                          **/
+/** -   Urutan data pada variabel "button" dan "button_click"                           **/
+/**     adalah sebagai berikut                                                          **/
+/**                                                                                     **/
+/**         |------|-------|-------|------|-------|--------|-----------|----------|     **/
+/** Bit Ke  |   7  |   6   |   5   |   4  |   3   |    2   |      1    |     0    |     **/
+/**         |------|-------|-------|------|-------|--------|-----------|----------|     **/
+/** Data    | kiri | bawah | kanan | atas | kotak | silang | lingkaran | segitiga |     **/
+/**         |------|-------|-------|------|-------|--------|-----------|----------|     **/
+/**                                                                                     **/
+/** -   Penggabungan data R1, R2, L1, L2, R3, L3, START, dan SELECT disimpan dalam      **/
+/**     variabel "RL"                                                                   **/
+/** -   Urutan data pada variabel "RL" dan "RL_click" adalah                            **/
+/**     sebagai berikut                                                                 **/
+/**                                                                                     **/
+/**         |----|--------|-------|----|----|----|----|                                 **/
+/** Bit Ke  |  6 |    5   |   4   |  3 |  2 |  1 |  0 |                                 **/
+/**         |----|--------|-------|----|----|----|----|                                 **/
+/** Data    | PS | SELECT | START | L3 | L1 | R3 | R1 |                                 **/
+/**         |----|--------|-------|----|----|----|----|                                 **/
+/**                                                                                     **/
+/*****************************************************************************************/
+ 
+void loop() {
+    button = 0;
+    RL = 0;
+        button_click=0;
+        RL_click=0;
+    Usb.Task();
+ 
+    // Pembacaan data joystik dilakukan jika PS3 tersambung ke Arduino
+    if (PS3.PS3Connected || PS3.PS3NavigationConnected) {
+        // Pembacaan dan penggabungan data segitiga, lingkaran, silang, kotak, atas, kanan, bawah, kiri
+        // Data bernilai '1' jika tombol ditekan
+        if(PS3.getButtonPress(TRIANGLE)){
+            button = button + (0x1 << 0);
+        }
+        if(PS3.getButtonPress(CIRCLE)){
+            button = button + (0x1 << 1);
+        }
+        if(PS3.getButtonPress(CROSS)){
+            button = button + (0x1 << 2);
+        }
+        if(PS3.getButtonPress(SQUARE)){
+            button = button + (0x1 << 3);
+        }
+        if(PS3.getButtonPress(UP)){
+            button = button + (0x1 << 4);
+        }
+        if(PS3.getButtonPress(RIGHT)){
+            button = button + (0x1 << 5);
+        }
+        if(PS3.getButtonPress(DOWN)){
+            button = button + (0x1 << 6);
+        }
+        if(PS3.getButtonPress(LEFT)){
+            button = button + (0x1 << 7);
+        }
+ 
+        // Pembacaan dan penggabungan data R1, R3, L1, L3, START, SELECT, dan PS
+        // Data bernilai '1' jika tombol ditekan
+        if(PS3.getButtonPress(R1)){
+            RL = RL + (0x1 << 0);
+        }
+        if(PS3.getButtonPress(R3)){
+            RL = RL + (0x1 << 1);
+        }
+        if(PS3.getButtonPress(L1)){
+            RL = RL + (0x1 << 2);
+        }
+        if(PS3.getButtonPress(L3)){
+            RL = RL + (0x1 << 3);
+        }
+        if(PS3.getButtonPress(START)){
+            RL = RL + (0x1 << 4);
+        }
+        if(PS3.getButtonPress(SELECT)){
+            RL = RL + (0x1 << 5);
+        }
+        if(PS3.getButtonPress(PS)){
+            RL = RL + (0x1 << 6);
+        }
+ 
+        // Pembacaan dan penggabungan data segitiga, lingkaran, silang, kotak, atas, kanan, bawah, kiri
+        // Data bernilai '1' hanya saat tombol pertama kali ditekan
+        if(PS3.getButtonClick(TRIANGLE)){
+            button_click = button_click + (0x1 << 0);
+        }
+        if(PS3.getButtonClick(CIRCLE)){
+            button_click = button_click + (0x1 << 1);
+        }
+        if(PS3.getButtonClick(CROSS)){
+            button_click = button_click + (0x1 << 2);
+        }
+        if(PS3.getButtonClick(SQUARE)){
+            button_click = button_click + (0x1 << 3);
+        }
+        if(PS3.getButtonClick(UP)){
+            button_click = button_click + (0x1 << 4);
+        }
+        if(PS3.getButtonClick(RIGHT)){
+            button_click = button_click + (0x1 << 5);
+        }
+        if(PS3.getButtonClick(DOWN)){
+            button_click = button_click + (0x1 << 6);
+        }
+        if(PS3.getButtonClick(LEFT)){
+            button_click = button_click + (0x1 << 7);
+        }
+ 
+        if(PS3.getButtonClick(R1)){
+            RL_click = RL_click + (0x1 << 0);
+        }
+        if(PS3.getButtonClick(R3)){
+            RL_click = RL_click + (0x1 << 1);
+        }
+        if(PS3.getButtonClick(L1)){
+            RL_click = RL_click + (0x1 << 2);
+        }
+        if(PS3.getButtonClick(L3)){
+            RL_click = RL_click + (0x1 << 3);
+        }
+        if(PS3.getButtonClick(START)){
+            RL_click = RL_click + (0x1 << 4);
+        }
+        if(PS3.getButtonClick(SELECT)){
+            RL_click = RL_click + (0x1 << 5);
+        }
+        if(PS3.getButtonClick(PS)){
+            RL_click = RL_click + (0x1 << 6);
+        }
+ 
+        // Tombol Analog
+        LX = PS3.getAnalogHat(LeftHatX);
+        LY = PS3.getAnalogHat(LeftHatY);
+        RX = PS3.getAnalogHat(RightHatX);
+        RY = PS3.getAnalogHat(RightHatY);
+        
+        aL2 = PS3.getAnalogButton(L2);
+        aR2 = PS3.getAnalogButton(R2);
+            
+                kirimdatajoystik();
+    } else {
+        // PS3 Disconnected
+        Serial.write(0x88);
+        Serial.write(0x09);
+    }
+    delay(3);
+}
+ 
+      
\ No newline at end of file

diff -r 964ae71a30e3 -r 85169ae8659b main.cpp
--- a/main.cpp	Wed Apr 05 16:04:08 2017 +0000
+++ b/main.cpp	Mon Apr 24 14:12:08 2017 +0000
@@ -2,7 +2,7 @@
 /****************************************************************************/
 /*                  PROGRAM UNTUK PID CLOSED LOOP                           */
 /*                                                                          */
-/*                  Last Update : 11 Maret 2017                          */
+/*                  Last Update : 16 April 2017                          */
 /*                                                                          */
 /*  - Digunakan encoder autonics                                            */
 /*  - Konfigurasi Motor dan Encoder sbb :                                   */
@@ -53,38 +53,48 @@
 #define D_ENCODER 10        // Diameter Roda Encoder
 #define D_ROBOT 80          // Diameter Roda Robot
 
-// Variable Atas
+// Variable Atas 
+// indek 2 untuk motor depan, 1 untuk motor belakang
 double speed, speed2;
-const double maxSpeed = 0.95, minSpeed = 0.0;
-const double kpA=0.6757, kdA=0.7757, kiA=0.00003757;
-double p,i,d;
-double p2,i2,d2;
-double last_error = 0, current_error, sum_error = 0;
-double last_error2 = 0, current_error2, sum_error2 = 0;
+const double maxSpeed = 0.95, minSpeed = 0.0, Ts = 12.5;
+const double kpA1=0.1478, kdA1=0.9295, kiA1=0.0004226;
+const double kpA2=0.1293, kdA2=1.007, kiA2=0.0002986;
+double a1,b1,c1;
+double a2,b2,c2;
+double current_error1, previous_error1_1 = 0, previous_error1_2 = 0;
+double current_error2, previous_error2_1 = 0, previous_error2_2 = 0;
+double previous_speed1 = 0;
+double previous_speed2 = 0;
+
 float rpm, rpm2;
-float target_rpm = 17.0, target_rpm2 = 17.0; // selisih 4 bagus, sama bagus
-const float maxRPM = 28, minRPM = 0; // Limit 25 atau 27
+double target_rpm = 17.0, target_rpm2 = 17.0; // selisih 4 bagus, sama bagus
+const float maxRPM = 35, minRPM = 0; // Limit 25 atau 27
 
 const float pwmPowerUp        = 1.0;
 const float pwmPowerDown      = -1.0;
 
-float jarak_ping=0;
- 
+double jarak_ping=0;
+double ping_target = 12;
+double ping_current_error, ping_previous_error1 = 0, ping_sum_error=0;
+double ping_Kp = -0.13, ping_Kd =-0.049, ping_Ts=10;
+double ping_pwm, ping_previous_pwm = 0; 
+
 // Variable Bawah
 float Vt;
 float keliling_enc      = PI*D_ENCODER;
 float keliling_robot    = PI*D_ROBOT;
 float speedT            = 0.2;
 float PIVOT             = 0.17;      // PWM Pivot Kanan, Pivot Kiri
-float tuneDpn           = 0.80;     // Tunning PWM motor Depan
-float tuneBlk           = 0.80;     // Tunning PWM motor belakang
+float tuneDpn           = 1.0;     // Tunning PWM motor Depan
+float tuneBlk           = 1.0;     // Tunning PWM motor belakang
 float tuneAksel         = 0.6;
 int aksel               = 0;
 float tuneAkselBlk      = 0.4;
-float tuneR             = 0.78;
-float tuneL             = 0.72;
+float tuneR             = 1.0;
+float tuneL             = 1.0;
 float serong            = 0.4;
-float rasio             = 1.4545;
+float rasio             = 3.0;
+float t_new             = 0.25;
 
 /* variable tunning */
 float tunning_L;
@@ -141,7 +151,7 @@
 //Motor motorBlk(PB_6, PC_14, PC_13);  //(PB_6, PB_1, PB_12); (PC_7, PC_14, PC_13); 
 Motor motorBlk(PB_2, PB_15, PB_1);
 Motor motorL  (PB_9, PA_12, PA_6);  
-Motor motorR  (PB_8, PC_5, PC_6);   //(PC_6, PB_4, PB_5);
+Motor motorR  (PB_8, PC_6, PC_5);   //(PC_6, PB_4, PB_5);
 
 /* Deklarasi Motor Launcher */
 Motor launcherDpn(PA_5,PB_12,PA_11);    // pwm ,fwd, rev
@@ -494,36 +504,36 @@
         {
             // Serong kanan maju
             motorDpn.speed(-serong);
-            motorL.speed(-serong);
+            motorL.speed(-serong-t_new);
             motorBlk.speed(serong);
-            motorR.speed(serong);
+            motorR.speed(serong+t_new);
             break;
         }
         case (14) : 
         {
             // Serong kiri maju
             motorDpn.speed(serong);
-            motorR.speed(serong);
+            motorR.speed(serong+t_new);
             motorBlk.speed(-serong);
-            motorL.speed(-serong);
+            motorL.speed(-serong-t_new);
             break;
         }
         case (15) : 
         {
             // Serong kanan mundur 
             motorDpn.speed(-serong);
-            motorR.speed(-serong);
+            motorR.speed(-serong-t_new);
             motorBlk.speed(serong);
-            motorL.speed(serong);
+            motorL.speed(serong+t_new);
             break;
         }
         case (16) : 
         {
             // Serong kiri mundur
             motorDpn.speed(serong);
-            motorL.speed(serong);
+            motorL.speed(serong+t_new);
             motorBlk.speed(-serong);
-            motorR.speed(-serong);
+            motorR.speed(-serong-t_new);
             break;
         }
         case (3) : 
@@ -559,8 +569,8 @@
             if (aksel>300)
                 tuneAksel = 0.9;
             
-            motorR.speed(tuneAksel);
-            motorL.speed(-tuneAksel);
+            motorR.speed(tuneAksel+t_new);
+            motorL.speed(-tuneAksel-t_new);
             motorDpn.brake(1);
             motorBlk.brake(1);
             break;
@@ -572,8 +582,8 @@
             if (aksel>300)
                 tuneAksel = 0.9;
             
-            motorR.speed(-tuneAksel);
-            motorL.speed(tuneAksel);
+            motorR.speed(-tuneAksel-t_new);
+            motorL.speed(tuneAksel+t_new);
             motorDpn.brake(1);
             motorBlk.brake(1);
             break;
@@ -631,8 +641,8 @@
         case (32) : 
         {
             // select
-            target_rpm2 = 10;
-            target_rpm = 10;
+            target_rpm2 = 11;
+            target_rpm = 11;
             init_rpm();
             break;
         }
@@ -676,18 +686,27 @@
         else if(!limitTengah){
             isReload = true;
         }
-        else if((jarak_ping > 6.5) && !flag_Pneu){
-            powerScrew.speed(pwmPowerUp);
-            ready = false;
+        else if(!flag_Pneu){
+            //pc.printf("%.2f\n", ping_pwm);
+            ping_current_error =  (double) (ping_target-jarak_ping);
+               
+            ping_sum_error += ping_current_error*ping_Ts;
+            ping_pwm = (double) ping_Kp*ping_current_error + ping_Kd*(ping_current_error-ping_previous_error1)/ping_Ts;   
+            
+            if (ping_pwm>1) ping_pwm=1;
+            if (ping_pwm>0.049 && ping_pwm<0.5) ping_pwm = 0.5;
+            if (ping_pwm<-0.049 && ping_pwm>-0.5) ping_pwm = -0.5;
+            if (ping_pwm<-1) ping_pwm=-1;
+            
+            powerScrew.speed(ping_pwm);
+            
+            ping_previous_error1 = ping_current_error;
         }
-        else if((jarak_ping < 6.0) && !flag_Pneu) {
-            powerScrew.speed(-0.85);
+        if ((jarak_ping>(ping_target-3))&&(jarak_ping<(ping_target+3))){
+            ready = true;
+        }else{
             ready = false;
         }
-        else{
-            powerScrew.brake(1);
-            ready = true;
-        }
     }
     else{
         powerScrew.brake(1);
@@ -702,16 +721,15 @@
         currentMillis  = millis();
         currentMillis2 = millis();
         
-        // PID Launcher Depan
-        if (currentMillis-previousMillis>=12.5)
+        // PID Launcher Belakang
+        if (currentMillis-previousMillis>=Ts)
         {
             rpm = (float)encLauncherBlk.getPulses();    
-            current_error = target_rpm - rpm;
-            sum_error = sum_error + current_error*12.5;
-            p = current_error*kpA;
-            d = (current_error-last_error)*kdA/12.5;
-            i = sum_error*kiA;
-            speed = p + d + i;
+            current_error1 = target_rpm - rpm;
+            a1 = kpA1 + kiA1*Ts/2 + kdA1/Ts;
+            b1 = -kpA1 + kiA1*Ts/2 - 2*kdA1/Ts;
+            c1 = kdA1/Ts;
+            speed = previous_speed1 + a1*current_error1 + b1*previous_error1_1 + c1*previous_error1_2;
             //init_speed();
             if(speed > maxSpeed){
                 launcherBlk.speed(maxSpeed);
@@ -722,31 +740,35 @@
             else {
                 launcherBlk.speed(speed);
             }
-            last_error = current_error;
+            previous_speed1 = speed;
+            previous_error1_2 = previous_error1_1;
+            previous_error1_1 = current_error1;
             encLauncherBlk.reset();
-          //pc.printf("%.04lf\n",rpm);
             previousMillis = currentMillis;
+            
         }
-        if (currentMillis2-previousMillis2>=12.5)
+        // PID Launcher Depan
+        if (currentMillis2-previousMillis2>=Ts)
         {
             rpm2 = (float)encLauncherDpn.getPulses();    
             current_error2 = target_rpm2 - rpm2;
-            sum_error2 = sum_error2 + current_error2*12.5;
-            p2 = current_error2*kpA;
-            d2 = (current_error2-last_error2)*kdA/12.5;
-            i2 = sum_error2*kiA;
-            speed2 = p2 + d2 + i2;
+            a2 = kpA2 + kiA2*Ts/2 + kdA2/Ts;
+            b2 = -kpA2 + kiA2*Ts/2 - 2*kdA2/Ts;
+            c2 = kdA2/Ts;
+            speed2 = previous_speed2 + a2*current_error2 + b2*previous_error2_1 + c2*previous_error2_2;
             //init_speed();
             if (speed2 > maxSpeed){
                 launcherDpn.speed(maxSpeed);
             }
-            else if ( speed < minSpeed){
+            else if ( speed2 < minSpeed){
                 launcherDpn.speed(minSpeed);
             }
             else{
                 launcherDpn.speed(speed2);
             }
-            last_error2 = current_error2;
+            previous_speed2 = speed2;
+            previous_error2_2 = previous_error2_1;
+            previous_error2_1 = current_error2;
             encLauncherDpn.reset();
             previousMillis2 = currentMillis2;
         }
@@ -756,12 +778,13 @@
     {
         launcherDpn.brake(1);
         launcherBlk.brake(1);
-        sum_error = 0;
-        sum_error2 = 0;
-        current_error = 0;
-        current_error2 = 0;
-        last_error = 0;
-        last_error2 = 0;
+        
+        previous_error1_1 = 0;
+        previous_error1_2 = 0;
+        previous_error2_1 = 0;
+        previous_error2_2 = 0;
+        previous_speed1 = 0;
+        previous_speed2 = 0;
     }     
 }
   
@@ -791,8 +814,8 @@
     while(1)
     {   
         // interupsi pembacaan PING setiap 30 ms
-        if(millis() - previousMillis4 >= 5){    //30
-            jarak_ping = (float)pingAtas.Read_cm()/2;
+        if(millis() - previousMillis4 >= 10){    //30
+            jarak_ping = (float)pingAtas.Read_cm();
             
             pingAtas.Send();
             previousMillis4 = millis();