06 Januari 2012

Wireless PlayStation 2 Controller dengan FSRB

/*
* Program utk Famosa Studio Robotik Kit ini diadaptasi dari
* program yang dibuat oleh
* George Frick (george.frick@gmail.com)
* dan dipublikasikan di sini:
* http://georgefrick.blogspot.com/2010/02/arduino-robot-2-wandering-tank.html
*
* Famosa Studio Robotik Kit (FSRB) adalah produk robotik dari Toko Online Famosa Studio.
* Dilengkapi Arduino Uno, Motor Shield 1A, Standard Servo, Micro Servo dan Sharp GP2D12.
*
* Tambahan kode untuk penggunaan PS2 Wireless Controller dengan FSRB
*/
#include //PS2X Library

//PS2X - Start
PS2X ps2x; // create PS2 Controller Class

int error = 0;
byte type = 0;
byte vibrate = 0;
//PS2X - End

// Constants for robot wheel.
const int RIGHTSPEED = 255;  //right motor speed
const int LEFTSPEED  = 255;  //left motor speed
int motorR = 5;       //right motor
int dirmotorR = 4;    //motor direction
int motorL = 6;       //left motor
int dirmotorL = 7;    //motor direction

/*
* Initializes everything. Is run once.
*/
void setup() {
//PS2X - Start
Serial.begin(57600);
//Serial.begin(9600);           // set up Serial library at 9600 bps
error = ps2x.config_gamepad(13,11,10,12, true, true);   //setup pins and settings:  GamePad(clock, command, attention, data, Pressures?, Rumble?) check for error

  if(error == 0)
{
Serial.println("Found Controller, configured successful");
Serial.println("Try out all the buttons, X will vibrate the controller, faster as you press harder;");
Serial.println("holding L1 or R1 will print out the analog stick values.");
Serial.println("Go to www.billporter.info for updates and to report bugs.");
} else if(error == 1)
{
Serial.println("No controller found, check wiring, see readme.txt to enable debug. visit www.billporter.info for troubleshooting tips");
} else if(error == 2)
{
Serial.println("Controller found but not accepting commands. see readme.txt to enable debug. Visit www.billporter.info for troubleshooting tips");
} else if(error == 3)
{
Serial.println("Controller refusing to enter Pressures mode, may not support it. ");
}

  //Serial.print(ps2x.Analog(1), HEX);

  type = ps2x.readType();
switch(type) {
case 0:
Serial.println("Unknown Controller type");
break;
case 1:
Serial.println("DualShock Controller Found");
break;
case 2:
Serial.println("GuitarHero Controller Found");
break;
}

  //PS2X - End

}  

/*
* Runs continuously.
* 1. Update Servo.
* 2. Check IR range sensor.
* 3. Move Robot
*/  

void loop() {
//PS2X - Start
if(error == 1) //skip loop if no controller found
return;
if(type == 2) //Guitar Hero Controller
return; //not used
else { //DualShock Controller
ps2x.read_gamepad(false, vibrate);          //read controller and set large motor to spin at 'vibrate' speed

    //if right analog stick is moved on Y axis forward-backward, or X axis right-left turn
if((ps2x.Analog(PSS_RY) < 64) && ((ps2x.Analog(PSS_RX) >= 64) && (ps2x.Analog(PSS_RX) <= 192)))
{
//move forward
Serial.println("FORWARD");
RMotor(RIGHTSPEED,true); // turn it on going forward
LMotor(LEFTSPEED,true);  // turn it on going forward
}
else if((ps2x.Analog(PSS_RY) > 192) && ((ps2x.Analog(PSS_RX) >= 64) && (ps2x.Analog(PSS_RX) <= 192)))
{
//move backward
Serial.println("BACKWARD");
RMotor(RIGHTSPEED,false); // turn it on going backward
LMotor(LEFTSPEED,false);  // turn it on going backward
}
else if((ps2x.Analog(PSS_RX) < 64) && ((ps2x.Analog(PSS_RY) >= 64) && (ps2x.Analog(PSS_RY) <= 192)))
{
//move left
Serial.println("LEFT");
RMotor(RIGHTSPEED,true); // turn it on going left
LMotor(LEFTSPEED,false); // turn it on going left
}
else if((ps2x.Analog(PSS_RX) > 192) && ((ps2x.Analog(PSS_RY) >= 64) && (ps2x.Analog(PSS_RY) <= 192)))
{
//move right
Serial.println("RIGHT");
RMotor(RIGHTSPEED,false); // turn it on going right
LMotor(LEFTSPEED,true);   // turn it on going right
}
//using button up, down, right and left for movement
else if(ps2x.Button(PSB_PAD_UP))
{
Serial.print("Up held this hard: ");
Serial.println(ps2x.Analog(PSAB_PAD_UP), DEC);
//move forward
Serial.println("FORWARD");
RMotor(RIGHTSPEED,true); // turn it on going forward
LMotor(LEFTSPEED,true);  // turn it on going forward
}
else if(ps2x.Button(PSB_PAD_RIGHT))
{
Serial.print("Right held this hard: ");
Serial.println(ps2x.Analog(PSAB_PAD_RIGHT), DEC);
//move right
Serial.println("RIGHT");
RMotor(RIGHTSPEED,false); // turn it on going right
LMotor(LEFTSPEED,true);   // turn it on going right
}
else if(ps2x.Button(PSB_PAD_LEFT))
{
Serial.print("LEFT held this hard: ");
Serial.println(ps2x.Analog(PSAB_PAD_LEFT), DEC);
//move left
Serial.println("LEFT");
RMotor(RIGHTSPEED,true); // turn it on going left
LMotor(LEFTSPEED,false); // turn it on going left
}
else if(ps2x.Button(PSB_PAD_DOWN))
{
Serial.print("DOWN held this hard: ");
Serial.println(ps2x.Analog(PSAB_PAD_DOWN), DEC);
//move backward
Serial.println("BACKWARD");
RMotor(RIGHTSPEED,false); // turn it on going backward
LMotor(LEFTSPEED,false);  // turn it on going backward
}
else
{
//stopped
Serial.println("Robot Stopped!");
RMotor(0,false); //stopped
LMotor(0,false); //stopped
}

  }  

  Serial.print("Stick Values:");
Serial.print(ps2x.Analog(PSS_LY), DEC); //Left stick, Y axis. Other options: LX, RY, RX
Serial.print(",");
Serial.print(ps2x.Analog(PSS_LX), DEC);
Serial.print(",");
Serial.print(ps2x.Analog(PSS_RY), DEC);
Serial.print(",");
Serial.println(ps2x.Analog(PSS_RX), DEC);
//PS2X - End

}  

// right motor
void RMotor(int pwmspeed, boolean forward) {
analogWrite(motorR, pwmspeed);
if (forward) {
digitalWrite(dirmotorR, HIGH);
}
else {
digitalWrite(dirmotorR, LOW);
}
}

//left motor
void LMotor(int pwmspeed, boolean forward) {
analogWrite(motorL, pwmspeed);
if (forward) {
digitalWrite(dirmotorL, HIGH);
}
else {
digitalWrite(dirmotorL, LOW);
}
}

/* EOF */


View the original article here

Ikuti Blog ini

Langganan

Mau dapet Update-an Blog ini lewat e-mail? Masukkin aja alamat Email kamu disini:

Dikirim Oleh FeedBurner