void Controller::updateController(){
ps2x.read_gamepad(false, vibrate);
MyStartButton = ps2x.Button(PSB_START);
MyLStickY = ps2x.Analog(PSS_LY);
MyLStickX = ps2x.Analog(PSS_LX);
MyRStickY = ps2x.Analog(PSS_RY);
MyRStickX = ps2x.Analog(PSS_RX);
dMyXButton = ps2x.NewButtonState(PSB_BLUE);
if(dMyXButton == 1){
if(MylastXButton == 1){
MyXButton = 0;
}
if(MylastXButton == 0){
MyXButton = 1;
}
}
MylastXButton = MyXButton;
}
// If both PS2 and XBee are defined then we will become secondary to the xbee
void InputController::Init(void)
{
int error;
//error = ps2x.config_gamepad(57, 55, 56, 54); // Setup gamepad (clock, command, attention, data) pins
error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT); // Setup gamepad (clock, command, attention, data) pins
g_BodyYOffset = 0;
g_BodyYShift = 0;
g_sPS2ErrorCnt = 0; // error count
ControlMode = WALKMODE;
DoubleHeightOn = false;
DoubleTravelOn = false;
WalkMethod = false;
g_InControlState.SpeedControl = 100; // Sort of migrate stuff in from Devon.
}
void Controller::setupController(){
delay(100);
error = ps2x.config_gamepad(pressures, rumble);
if(error == 0){
Serial.println("Found Controller, configured successful ");
}
else if(error == 1){
Serial.println("No controller found");
}
else{
Serial.println("nothing is happening");
delay(10000);
}
Serial.println("Neeeeeeext...");
delay(200);
MyXButton = 0;
}
//==============================================================================
// This is The main code to input function to read inputs from the PS2 and then
//process any commands.
//==============================================================================
void InputController::ControlInput(void)
{
boolean fAdjustLegPositions = false;
// Then try to receive a packet of information from the PS2.
// Then try to receive a packet of information from the PS2.
ps2x.read_gamepad(); //read controller and set large motor to spin at 'vibrate' speed
// Wish the library had a valid way to verify that the read_gamepad succeeded... Will hack for now
if ((ps2x.Analog(1) & 0xf0) == 0x70) {
// In an analog mode so should be OK...
g_sPS2ErrorCnt = 0; // clear out error count...
if (ps2x.ButtonPressed(PSB_START)) {// OK lets try "0" button for Start.
if (g_InControlState.fHexOn) {
PS2TurnRobotOff();
} else {
//Turn on
g_InControlState.fHexOn = 1;
fAdjustLegPositions = true;
}
}
if (g_InControlState.fHexOn) {
// [SWITCH MODES]
//Translate mode
if (ps2x.ButtonPressed(PSB_L1)) {// L1 Button Test
MSound(SOUND_PIN, 1, 50, 2000); //sound SOUND_PIN, [50\4000]
if (ControlMode != TRANSLATEMODE )
ControlMode = TRANSLATEMODE;
else {
if (g_InControlState.SelectedLeg==255)
ControlMode = WALKMODE;
else
ControlMode = SINGLELEGMODE;
}
}
//Rotate mode
if (ps2x.ButtonPressed(PSB_L2)) { // L2 Button Test
MSound(SOUND_PIN, 1, 50, 2000); //sound SOUND_PIN, [50\4000]
if (ControlMode != ROTATEMODE)
ControlMode = ROTATEMODE;
else {
if (g_InControlState.SelectedLeg == 255)
ControlMode = WALKMODE;
else
ControlMode = SINGLELEGMODE;
}
}
//Single leg mode fNO
if (ps2x.ButtonPressed(PSB_CIRCLE)) {// O - Circle Button Test
if (abs(g_InControlState.TravelLength.x)<cTravelDeadZone && abs(g_InControlState.TravelLength.z)<cTravelDeadZone
&& abs(g_InControlState.TravelLength.y*2)<cTravelDeadZone ) {
//Sound SOUND_PIN,[50\4000]
if (ControlMode != SINGLELEGMODE) {
ControlMode = SINGLELEGMODE;
if (g_InControlState.SelectedLeg == 255) //Select leg if none is selected
g_InControlState.SelectedLeg=cRF; //Startleg
} else {
ControlMode = WALKMODE;
g_InControlState.SelectedLeg=255;
}
}
}
#ifdef OPT_GPPLAYER
// GP Player Mode X
if (ps2x.ButtonPressed(PSB_CROSS)) { // X - Cross Button Test
MSound(SOUND_PIN, 1, 50, 2000); //sound SOUND_PIN, [50\4000]
if (ControlMode != GPPLAYERMODE) {
ControlMode = GPPLAYERMODE;
GPSeq=0;
} else
ControlMode = WALKMODE;
}
#endif // OPT_GPPLAYER
//[Common functions]
//Switch Balance mode on/off
if (ps2x.ButtonPressed(PSB_SQUARE)) { // Square Button Test
g_InControlState.BalanceMode = !g_InControlState.BalanceMode;
if (g_InControlState.BalanceMode) {
MSound(SOUND_PIN, 1, 250, 1500); //sound SOUND_PIN, [250\3000]
} else {
MSound(SOUND_PIN, 2, 100, 2000, 50, 4000);
}
}
//Stand up, sit down
if (ps2x.ButtonPressed(PSB_TRIANGLE)) { // Triangle - Button Test
if (g_BodyYOffset>0)
g_BodyYOffset = 0;
else
g_BodyYOffset = 35;
fAdjustLegPositions = true;
}
if (ps2x.ButtonPressed(PSB_PAD_UP)) {// D-Up - Button Test
g_BodyYOffset += 10;
// And see if the legs should adjust...
fAdjustLegPositions = true;
if (g_BodyYOffset > MAX_BODY_Y)
g_BodyYOffset = MAX_BODY_Y;
}
if (ps2x.ButtonPressed(PSB_PAD_DOWN) && g_BodyYOffset) {// D-Down - Button Test
if (g_BodyYOffset > 10)
g_BodyYOffset -= 10;
else
g_BodyYOffset = 0; // constrain don't go less than zero.
// And see if the legs should adjust...
fAdjustLegPositions = true;
}
if (ps2x.ButtonPressed(PSB_PAD_RIGHT)) { // D-Right - Button Test
if (g_InControlState.SpeedControl>0) {
g_InControlState.SpeedControl = g_InControlState.SpeedControl - 50;
MSound(SOUND_PIN, 1, 50, 2000); //sound SOUND_PIN, [50\4000]
}
}
if (ps2x.ButtonPressed(PSB_PAD_LEFT)) { // D-Left - Button Test
if (g_InControlState.SpeedControl<2000 ) {
g_InControlState.SpeedControl = g_InControlState.SpeedControl + 50;
MSound(SOUND_PIN, 1, 50, 2000); //sound SOUND_PIN, [50\4000]
}
}
//[Walk functions]
if (ControlMode == WALKMODE) {
//Switch gates
if (ps2x.ButtonPressed(PSB_SELECT) // Select Button Test
&& abs(g_InControlState.TravelLength.x)<cTravelDeadZone //No movement
&& abs(g_InControlState.TravelLength.z)<cTravelDeadZone
&& abs(g_InControlState.TravelLength.y*2)<cTravelDeadZone ) {
g_InControlState.GaitType = g_InControlState.GaitType+1; // Go to the next gait...
if (g_InControlState.GaitType<NUM_GAITS) { // Make sure we did not exceed number of gaits...
MSound(SOUND_PIN, 1, 50, 2000); //sound SOUND_PIN, [50\4000]
} else {
MSound (SOUND_PIN, 2, 50, 2000, 50, 2250);
g_InControlState.GaitType = 0;
}
GaitSelect();
}
//Double leg lift height
if (ps2x.ButtonPressed(PSB_R1)) { // R1 Button Test
MSound(SOUND_PIN, 1, 50, 2000); //sound SOUND_PIN, [50\4000]
DoubleHeightOn = !DoubleHeightOn;
if (DoubleHeightOn)
g_InControlState.LegLiftHeight = 80;
else
g_InControlState.LegLiftHeight = 50;
}
//Double Travel Length
if (ps2x.ButtonPressed(PSB_R2)) {// R2 Button Test
MSound (SOUND_PIN, 1, 50, 2000); //sound SOUND_PIN, [50\4000]
DoubleTravelOn = !DoubleTravelOn;
}
// Switch between Walk method 1 && Walk method 2
if (ps2x.ButtonPressed(PSB_R3)) { // R3 Button Test
MSound (SOUND_PIN, 1, 50, 2000); //sound SOUND_PIN, [50\4000]
WalkMethod = !WalkMethod;
}
//Walking
if (WalkMethod) //(Walk Methode)
g_InControlState.TravelLength.z = (ps2x.Analog(PSS_RY)-128); //Right Stick Up/Down
else {
g_InControlState.TravelLength.x = -(ps2x.Analog(PSS_LX) - 128);
g_InControlState.TravelLength.z = (ps2x.Analog(PSS_LY) - 128);
}
if (!DoubleTravelOn) { //(Double travel length)
g_InControlState.TravelLength.x = g_InControlState.TravelLength.x/2;
g_InControlState.TravelLength.z = g_InControlState.TravelLength.z/2;
}
g_InControlState.TravelLength.y = -(ps2x.Analog(PSS_RX) - 128)/4; //Right Stick Left/Right
}
//[Translate functions]
g_BodyYShift = 0;
if (ControlMode == TRANSLATEMODE) {
g_InControlState.BodyPos.x = (ps2x.Analog(PSS_LX) - 128)/2;
g_InControlState.BodyPos.z = -(ps2x.Analog(PSS_LY) - 128)/3;
g_InControlState.BodyRot1.y = (ps2x.Analog(PSS_RX) - 128)*2;
g_BodyYShift = (-(ps2x.Analog(PSS_RY) - 128)/2);
}
//[Rotate functions]
if (ControlMode == ROTATEMODE) {
g_InControlState.BodyRot1.x = (ps2x.Analog(PSS_LY) - 128);
g_InControlState.BodyRot1.y = (ps2x.Analog(PSS_RX) - 128)*2;
g_InControlState.BodyRot1.z = (ps2x.Analog(PSS_LX) - 128);
g_BodyYShift = (-(ps2x.Analog(PSS_RY) - 128)/2);
}
//[Single leg functions]
if (ControlMode == SINGLELEGMODE) {
//Switch leg for single leg control
if (ps2x.ButtonPressed(PSB_SELECT)) { // Select Button Test
MSound (SOUND_PIN, 1, 50, 2000); //sound SOUND_PIN, [50\4000]
if (g_InControlState.SelectedLeg<5)
g_InControlState.SelectedLeg = g_InControlState.SelectedLeg+1;
else
g_InControlState.SelectedLeg=0;
}
g_InControlState.SLLeg.x= (ps2x.Analog(PSS_LX) - 128)/2; //Left Stick Right/Left
g_InControlState.SLLeg.y= (ps2x.Analog(PSS_RY) - 128)/10; //Right Stick Up/Down
g_InControlState.SLLeg.z = (ps2x.Analog(PSS_LY) - 128)/2; //Left Stick Up/Down
// Hold single leg in place
if (ps2x.ButtonPressed(PSB_R2)) { // R2 Button Test
MSound (SOUND_PIN, 1, 50, 2000); //sound SOUND_PIN, [50\4000]
g_InControlState.fSLHold = !g_InControlState.fSLHold;
}
}
#ifdef OPT_GPPLAYER
//[GPPlayer functions]
if (ControlMode == GPPLAYERMODE) {
//Switch between sequences
if (ps2x.ButtonPressed(PSB_SELECT)) { // Select Button Test
if (!g_ServoDriver.FIsGPSeqActive() ) {
if (GPSeq < 5) { //Max sequence
MSound (SOUND_PIN, 1, 50, 1500); //sound SOUND_PIN, [50\3000]
GPSeq = GPSeq+1;
} else {
MSound (SOUND_PIN, 2, 50, 2000, 50, 2250);//Sound SOUND_PIN,[50\4000, 50\4500]
GPSeq=0;
}
}
}
//Start Sequence
if (ps2x.ButtonPressed(PSB_R2))// R2 Button Test
g_ServoDriver.GPStartSeq(GPSeq);
}
#endif // OPT_GPPLAYER
//Calculate walking time delay
g_InControlState.InputTimeDelay = 128 - max(max(abs(ps2x.Analog(PSS_LX) - 128), abs(ps2x.Analog(PSS_LY) - 128)), abs(ps2x.Analog(PSS_RX) - 128));
}
//Calculate g_InControlState.BodyPos.y
g_InControlState.BodyPos.y = min(max(g_BodyYOffset + g_BodyYShift, 0), MAX_BODY_Y);
if (fAdjustLegPositions)
AdjustLegPositionsToBodyHeight(); // Put main workings into main program file
} else {
// We may have lost the PS2... See what we can do to recover...
if (g_sPS2ErrorCnt < MAXPS2ERRORCNT)
g_sPS2ErrorCnt++; // Increment the error count and if to many errors, turn off the robot.
else if (g_InControlState.fHexOn)
PS2TurnRobotOff();
ps2x.reconfig_gamepad();
}
}