RC Car 4WD Stick PS2 Wireless

Program Utama

=================================Source Code================================

 #include "Wire.h"
 #include "PS2X_lib.h"
 #define PS2_DAT 13 // data
 #define PS2_CMD 11 //command
 #define PS2_SEL 10 //attention
 #define PS2_CLK 12 //clock
 #define pressures false
 #define rumble false
 PS2X ps2x; //
 int error = 0;
 byte type = 0;
 byte vibrate = 0;
 int temp1;
 int temp2;
 int temp3;
 int temp4;
 int temp33;
 int temp44;
 
 void setup()
 { 
     pinMode(3, OUTPUT);
     pinMode(5, OUTPUT);
     pinMode(6, OUTPUT);
     pinMode(9, OUTPUT);
     Serial.begin(57600);
 
     delay(300);
     error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT, pressures, rumble);
     if (error == 0) {
         Serial.print("Found controller ");
         Serial.print("pressures = ");
         if (pressures)
             Serial.println("true ");
         else
             Serial.println("false");
         Serial.print("rumble = ");
         if (rumble)
             Serial.println("true)");
         else
             Serial.println("false");
         Serial.println("Try all button;");
         Serial.println("KEEP L1 or R1 will print analog value.");
     }
     else if (error == 1)
         Serial.println("Cant connect, pls try again...");
 
     else if (error == 2)
         Serial.println("Found controller however cannot received cmd");
 
     else if (error == 3)
         Serial.println("Controller rejected Pressures ");
 
     type = ps2x.readType();
     switch (type) {
     case 0:
         Serial.print("Controller not suitable ");
         break;
     case 1:
         Serial.print("Found DualShock ");
         break;
     case 2:
         Serial.print("Found GuitarHero ");
         break;
     case 3:
         Serial.print("Found wifi Sony DualShock");
         break;
     }
 }
 
 void loop()
 {
     temp1 = 0;
     temp2 = 0;
     temp3 = 0;
     temp4 = 0;
     if (error == 1)
         return;
     if (type == 2) {
         ps2x.read_gamepad();
     }
     else { 
         ps2x.read_gamepad(false, vibrate); 
 
         if (ps2x.Analog(PSS_LY) > 128) {
             temp1 = (ps2x.Analog(PSS_LY) - 128) * 2;
         }
         else if (ps2x.Analog(PSS_LY) < 128) {
             temp2 = abs(ps2x.Analog(PSS_LY) - 128) * 2;
             if (temp2 > 255)
                 temp2 = 255;
         }
         else {
             temp2 = 0;
         }
         if (ps2x.Analog(PSS_RX) > 128) {
             temp3 = (ps2x.Analog(PSS_RX) - 128) * 2;
         }
         else if (ps2x.Analog(PSS_RX) < 128) {
             temp4 = abs(ps2x.Analog(PSS_RX) - 128) * 2;
             if (temp2 > 255)
                 temp4 = 255;
         }
         else {
             temp4 = 0;
         }
       
     }
  
     if ((temp2 > 0) && temp3 == 0 && temp4 == 0) {
         analogWrite(3, temp2);
         digitalWrite(5, LOW);
         analogWrite(6, temp2);
         digitalWrite(9, LOW);
     }
     else {
         if ((temp1 > 0) && temp3 == 0 && temp4 == 0) {
             digitalWrite(3, LOW);
             analogWrite(5, temp1);
             digitalWrite(6, LOW);
             analogWrite(9, temp1);
         }
         else {
             if ((temp2 > 0) && temp4 > 0) {
                 temp44 = (temp2 - temp4);
                 if (temp44 < 0) {
                     temp44 = 0;
                 }
                 analogWrite(3, temp44);
                 digitalWrite(5, LOW);
                 analogWrite(6, temp2);
                 digitalWrite(9, LOW);
             }
             else {
                 if ((temp2 > 0) && temp3 > 0) {
                     temp33 = (temp2 - temp3);
                     if (temp33 < 0) {
                         temp33 = 0;
                     }
                     analogWrite(3, temp2);
                     digitalWrite(5, LOW);
                     analogWrite(6, temp33);
                     digitalWrite(9, LOW);
                 }
                 else {
                     if ((temp1 > 0) && temp4 > 0) {
                         temp44 = (temp1 - temp4);
                         if (temp44 < 0) {
                             temp44 = 0;
                         }
                         analogWrite(3, LOW);
                         digitalWrite(5, temp44);
                         analogWrite(6, LOW);
                         digitalWrite(9, temp1);
                     }
                     else {
                         if ((temp1 > 0) && temp3 > 0) {
                             temp33 = (temp1 - temp3);
                             if (temp33 < 0) {
                                 temp33 = 0;
                             }
                             analogWrite(3, LOW);
                             digitalWrite(5, temp33);
                             analogWrite(6, LOW);
                             digitalWrite(9, temp1);
                         }
                         else {
                             digitalWrite(3, LOW);
                             digitalWrite(5, LOW);
                             digitalWrite(6, LOW);
                             digitalWrite(9, LOW);
                         }
                     }
                 }
             }
         }
     }
 }