//==============================================================================
// This is The function that is called by the Main program to initialize
//the input controller, which in this case is a generic Linux Joystick object
// For a few of them we will try to detect which one it is and map the buttons
// and axes...
//==============================================================================
void JoystickInputController::Init(void)
{
g_BodyYOffset = 0;
g_BodyYShift = 0;
ljoy.begin(szDevice);
GPSeq = 0; // init to something...
ControlMode = WALKMODE;
HeightSpeedMode = NORM_NORM;
DoubleTravelOn = false;
WalkMethod = false;
SpeakStr("Init");
}
// If both PS2 and XBee are defined then we will become secondary to the xbee
void CommanderInputController::Init(void)
{
// DBGSerial.println("Init Commander Start");
g_BodyYOffset = 0;
g_BodyYShift = 0;
command.begin(szDevice, B38400);
GPSeq = 0; // init to something...
ControlMode = WALKMODE;
HeightSpeedMode = NORM_NORM;
// DoubleHeightOn = false;
DoubleTravelOn = false;
WalkMethod = false;
// DBGSerial.println("Init Commander End");
SpeakStr("Init");
}
//==============================================================================
// This is The main code to input function to read inputs from the Commander and then
//process any commands.
//==============================================================================
void CommanderInputController::ControlInput(void)
{
// See if we have a new command available...
if(command.ReadMsgs() > 0)
{
// If we receive a valid message than turn robot on...
g_InControlState.fRobotOn = true;
// Experimenting with trying to detect when IDLE. maybe set a state of
// of no button pressed and all joysticks are in the DEADBAND area...
g_InControlState.fControllerInUse = command.buttons
|| (abs(command.leftH) >= cTravelDeadZone)
|| (abs(command.leftV) >= cTravelDeadZone)
|| (abs(command.rightH) >= cTravelDeadZone)
|| (abs(command.rightV) >= cTravelDeadZone);
// [SWITCH MODES]
// Cycle through modes...
if ((command.buttons & BUT_LT) && !(buttonsPrev & BUT_LT))
{
if (++ControlMode >= MODECNT)
{
ControlMode = WALKMODE; // cycled back around...
MSound( 2, 50, 2000, 50, 3000);
}
else
{
MSound( 1, 50, 2000);
}
}
//[Common functions]
//Switch Balance mode on/off
if ((command.buttons & BUT_L4) && !(buttonsPrev & BUT_L4))
{
g_InControlState.BalanceMode = !g_InControlState.BalanceMode;
if (g_InControlState.BalanceMode)
{
MSound( 1, 250, 1500);
}
else
{
MSound( 2, 100, 2000, 50, 4000);
}
}
//Stand up, sit down
if ((command.buttons & BUT_L5) && !(buttonsPrev & BUT_L5))
{
if (g_BodyYOffset>0)
g_BodyYOffset = 0;
else
g_BodyYOffset = 35;
}
// We will use L6 with the Right joystick to control both body offset as well as Speed...
// We move each pass through this by a percentage of how far we are from center in each direction
// We get feedback with height by seeing the robot move up and down. For Speed, I put in sounds
// which give an idea, but only for those whoes robot has a speaker
if (command.buttons & BUT_L6 )
{
printf("L6 down\n\r");
// raise or lower the robot on the joystick up /down
// Maybe should have Min/Max
g_BodyYOffset += command.rightV/25;
if (abs(command.rightV)>= 25)
printf("%d %d", g_BodyYOffset, command.rightV);
// Likewise for Speed control
int dspeed = command.rightH / 16; //
if ((dspeed < 0) && g_InControlState.SpeedControl)
{
if ((word)(-dspeed) < g_InControlState.SpeedControl)
g_InControlState.SpeedControl += dspeed;
else
g_InControlState.SpeedControl = 0;
MSound( 1, 50, 1000+g_InControlState.SpeedControl);
}
if ((dspeed > 0) && (g_InControlState.SpeedControl < 2000))
{
g_InControlState.SpeedControl += dspeed;
if (g_InControlState.SpeedControl > 2000)
g_InControlState.SpeedControl = 2000;
MSound( 1, 50, 1000+g_InControlState.SpeedControl);
}
command.rightH = 0; // don't walk when adjusting the speed here...
}
//[Walk functions]
if (ControlMode == WALKMODE)
{
//Switch gates
if (((command.buttons & BUT_R1) && !(buttonsPrev & BUT_R1))
//No movement
&& abs(g_InControlState.TravelLength.x)<cTravelDeadZone
&& abs(g_InControlState.TravelLength.z)<cTravelDeadZone
&& abs(g_InControlState.TravelLength.y*2)<cTravelDeadZone )
{
// Go to the next gait...
g_InControlState.GaitType = g_InControlState.GaitType+1;
// Make sure we did not exceed number of gaits...
if (g_InControlState.GaitType<NUM_GAITS)
{
#ifndef OPT_ESPEAK
MSound( 1, 50, 2000);
#endif
}
else
{
#ifdef OPT_ESPEAK
MSound (2, 50, 2000, 50, 2250);
#endif
g_InControlState.GaitType = 0;
}
#ifdef OPT_ESPEAK
SpeakStr(s_asGateNames[g_InControlState.GaitType]);
#endif
GaitSelect();
}
//Double leg lift height
if ((command.buttons & BUT_RT) && !(buttonsPrev & BUT_RT))
{
MSound( 1, 50, 2000);
// wrap around mode
HeightSpeedMode = (HeightSpeedMode + 1) & 0x3;
DoubleTravelOn = HeightSpeedMode & 0x1;
if ( HeightSpeedMode & 0x2)
g_InControlState.LegLiftHeight = 80;
else
g_InControlState.LegLiftHeight = 50;
}
// Switch between Walk method 1 && Walk method 2
if ((command.buttons & BUT_R2) && !(buttonsPrev & BUT_R2))
{
MSound (1, 50, 2000);
WalkMethod = !WalkMethod;
}
//Walking
if (WalkMethod) //(Walk Methode)
//Right Stick Up/Down
g_InControlState.TravelLength.z = (command.rightV);
else
{
g_InControlState.TravelLength.x = -command.leftH;
g_InControlState.TravelLength.z = command.leftV;
}
if (!DoubleTravelOn) //(Double travel length)
{
g_InControlState.TravelLength.x = g_InControlState.TravelLength.x/2;
g_InControlState.TravelLength.z = g_InControlState.TravelLength.z/2;
}
//Right Stick Left/Right
g_InControlState.TravelLength.y = -(command.rightH)/4;
}
//[Translate functions]
g_BodyYShift = 0;
if (ControlMode == TRANSLATEMODE)
{
g_InControlState.BodyPos.x = SmoothControl(((command.leftH)*2/3), g_InControlState.BodyPos.x, SmDiv);
g_InControlState.BodyPos.z = SmoothControl(((command.leftV)*2/3), g_InControlState.BodyPos.z, SmDiv);
g_InControlState.BodyRot1.y = SmoothControl(((command.rightH)*2), g_InControlState.BodyRot1.y, SmDiv);
// g_InControlState.BodyPos.x = (command.leftH)/2;
// g_InControlState.BodyPos.z = -(command.leftV)/3;
// g_InControlState.BodyRot1.y = (command.rightH)*2;
g_BodyYShift = (-(command.rightV)/2);
}
//[Rotate functions]
if (ControlMode == ROTATEMODE)
{
g_InControlState.BodyRot1.x = (command.leftV);
g_InControlState.BodyRot1.y = (command.rightH)*2;
g_InControlState.BodyRot1.z = (command.leftH);
g_BodyYShift = (-(command.rightV)/2);
}
#ifdef OPT_GPPLAYER
//[GPPlayer functions]
if (ControlMode == GPPLAYERMODE)
{
// Lets try some speed control... Map all values if we have mapped some before
// or start mapping if we exceed some minimum delta from center
// Have to keep reminding myself that commander library already subtracted 128...
if (g_ServoDriver.FIsGPSeqActive() )
{
if ((g_sGPSMController != 32767)
|| (command.rightV > 16) || (command.rightV < -16))
{
// We are in speed modify mode...
if (command.rightV >= 0)
g_sGPSMController = map(command.rightV, 0, 127, 0, 200);
else
g_sGPSMController = map(command.rightV, -127, 0, -200, 0);
g_ServoDriver.GPSetSpeedMultiplyer(g_sGPSMController);
}
}
//Switch between sequences
if ((command.buttons & BUT_R1) && !(buttonsPrev & BUT_R1))
{
if (!g_ServoDriver.FIsGPSeqActive() )
{
if (GPSeq < 5) //Max sequence
{
MSound (1, 50, 1500);
GPSeq = GPSeq+1;
}
else
{
MSound (2, 50, 2000, 50, 2250);
GPSeq=0;
}
}
}
//Start Sequence
if ((command.buttons & BUT_R2) && !(buttonsPrev & BUT_R2))
{
if (!g_ServoDriver.FIsGPSeqActive() )
{
g_ServoDriver.GPStartSeq(GPSeq);
g_sGPSMController = 32767; // Say that we are not in Speed modify mode yet... valid ranges are 50-200 (both postive and negative...
}
else
{
// tell the GP system to abort if possible...
g_ServoDriver.GPStartSeq(0xff);
MSound (2, 50, 2000, 50, 2000);
}
}
}
#endif // OPT_GPPLAYER
//Calculate walking time delay
g_InControlState.InputTimeDelay = 128 - max(max(abs(command.leftH), abs(command.leftV)), abs(command.rightH));
//Calculate g_InControlState.BodyPos.y
g_InControlState.BodyPos.y = max(g_BodyYOffset + g_BodyYShift, 0);
// Save away the buttons state as to not process the same press twice.
buttonsPrev = command.buttons;
extPrev = command.ext;
g_ulLastMsgTime = millis();
}
else
{
// We did not receive a valid packet. check for a timeout to see if we should turn robot off...
if (g_InControlState.fRobotOn)
{
if ((millis() - g_ulLastMsgTime) > ARBOTIX_TO)
{
g_InControlState.fControllerInUse = true; // make sure bypass is not used.
CommanderTurnRobotOff();
}
}
}
}
//==============================================================================
// This is The main code to input function to read inputs from the Joystick and then
//process any commands.
//==============================================================================
void JoystickInputController::ControlInput(void)
{
// See if we have a new command available...
if(ljoy.readMsgs() > 0)
{
boolean fAdjustLegPositions = false;
short sLegInitXZAdjust = 0;
short sLegInitAngleAdjust = 0;
// See if this is the first message we have received
if (g_ulLastMsgTime == (unsigned long)-1)
{
// First valid message see if we can figure out what it is from...
printf("Firt JS0 even: %d axes %d buttons Name: %s\n", ljoy.joystickAxisCount(),
ljoy.joystickButtonCount(),
ljoy.JoystickName());
if ((ljoy.joystickAxisCount() == 27) && (ljoy.joystickButtonCount() == 19))
{
printf("PS3!\n");
}
else if ((ljoy.joystickAxisCount() == 8) && (ljoy.joystickButtonCount() == 14))
{
printf("DS4!\n");
}
}
// Save message time so we may detect timeouts.
g_ulLastMsgTime = millis();
// We have a message see if we have turned the robot on or not...
if (ljoy.buttonPressed(JOYSTICK_BUTTONS::START_SHARE))
{
if (!g_InControlState.fRobotOn)
{
// Turn it on
g_InControlState.fRobotOn = true;
fAdjustLegPositions = true;
printf("Turn Robot on: %d %d : %d %d\n",
ljoy.axis(JOYSTICK_AXES::LX), ljoy.axis(JOYSTICK_AXES::LY),
ljoy.axis(JOYSTICK_AXES::RX), ljoy.axis(JOYSTICK_AXES::RY));
}
else
{
ControllerTurnRobotOff();
}
}
// Few of the buttons we may process even when off...
if (ljoy.buttonPressed(JOYSTICK_BUTTONS::L3))
g_fDebugOutput = !g_fDebugOutput; // toggle debug on and off
// If robot is not on, lets bail from here...
if ( !g_InControlState.fRobotOn)
return;
// In some cases we update values as to keep other places to use the values
// So we create a local copy we use through this function.
int axes_rx = ljoy.axis(JOYSTICK_AXES::RX);
int axes_ry = ljoy.axis(JOYSTICK_AXES::RY);
int axes_lx = ljoy.axis(JOYSTICK_AXES::LX);
int axes_ly = ljoy.axis(JOYSTICK_AXES::LY);
// Experimenting with trying to detect when IDLE. maybe set a state of
// of no button pressed and all joysticks are in the DEADBAND area...
g_InControlState.fControllerInUse = ljoy.buttons()
|| (abs((axes_lx/256)) >= cTravelDeadZone)
|| (abs((axes_ly/256)) >= cTravelDeadZone)
|| (abs((axes_rx/256)) >= cTravelDeadZone)
|| (abs((axes_ry/256)) >= cTravelDeadZone);
// [SWITCH MODES]
// Cycle through modes...
// May break this up, like PS2...
if (ljoy.buttonPressed(JOYSTICK_BUTTONS::R1))
{
if (++ControlMode >= MODECNT)
{
ControlMode = WALKMODE; // cycled back around...
MSound( 2, 50, 2000, 50, 3000);
}
else
{
MSound( 1, 50, 2000);
}
}
// Hack make easy to get back to standard mode...
if (ljoy.buttonPressed(JOYSTICK_BUTTONS::PS))
{
ControlMode = WALKMODE; // cycled back around...
MSound( 2, 50, 2000, 50, 3000);
}
//[Common functions]
//Switch Balance mode on/off
if (ljoy.buttonPressed(JOYSTICK_BUTTONS::SQUARE))
{
g_InControlState.BalanceMode = !g_InControlState.BalanceMode;
if (g_InControlState.BalanceMode)
{
MSound( 1, 250, 1500);
}
else
{
MSound( 2, 100, 2000, 50, 4000);
}
}
//Stand up, sit down
if (ljoy.buttonPressed(JOYSTICK_BUTTONS::TRI))
{
if (g_BodyYOffset>0)
g_BodyYOffset = 0;
else
g_BodyYOffset = 35;
fAdjustLegPositions = true;
g_fDynamicLegXZLength = false;
}
// We will use L1 with the Right joystick to control both body offset as well as Speed...
// We move each pass through this by a percentage of how far we are from center in each direction
// We get feedback with height by seeing the robot move up and down. For Speed, I put in sounds
// which give an idea, but only for those whoes robot has a speaker
if (ljoy.button(JOYSTICK_BUTTONS::L1))
{
// raise or lower the robot on the joystick up /down
int delta = (axes_ry/256)/25;
if (delta) {
g_BodyYOffset = max(min(g_BodyYOffset + delta, MAX_BODY_Y), 0);
fAdjustLegPositions = true;
}
// Likewise for Speed control
int dspeed = (axes_rx/256) / 16; //
if ((dspeed < 0) && g_InControlState.SpeedControl)
{
if ((word)(-dspeed) < g_InControlState.SpeedControl)
g_InControlState.SpeedControl += dspeed;
else
g_InControlState.SpeedControl = 0;
MSound( 1, 50, 1000+g_InControlState.SpeedControl);
}
if ((dspeed > 0) && (g_InControlState.SpeedControl < 2000))
{
g_InControlState.SpeedControl += dspeed;
if (g_InControlState.SpeedControl > 2000)
g_InControlState.SpeedControl = 2000;
MSound( 1, 50, 1000+g_InControlState.SpeedControl);
}
// Likewise we use the left Joystick to control Postion of feet
sLegInitXZAdjust = axes_lx/(256*10); // play with this.
#ifdef ADJUSTABLE_LEG_ANGLES
sLegInitAngleAdjust = axes_ly/(256*8);
#endif
// Make sure other areas of code below does not use some of the joystick values
axes_lx = 0;
axes_ly = 0;
axes_rx = 0; // don't walk when adjusting the speed here...
}
//[Walk functions]
if (ControlMode == WALKMODE)
{
//Switch gates
if ((ljoy.buttonPressed(JOYSTICK_BUTTONS::SELECT_OPT))
//No movement
&& abs(g_InControlState.TravelLength.x)<cTravelDeadZone
&& abs(g_InControlState.TravelLength.z)<cTravelDeadZone
&& abs(g_InControlState.TravelLength.y*2)<cTravelDeadZone )
{
// Go to the next gait...
g_InControlState.GaitType = g_InControlState.GaitType+1;
// Make sure we did not exceed number of gaits...
if (g_InControlState.GaitType<NUM_GAITS)
{
#ifndef OPT_ESPEAK
MSound( 1, 50, 2000);
#endif
}
else
{
#ifdef OPT_ESPEAK
MSound (2, 50, 2000, 50, 2250);
#endif
g_InControlState.GaitType = 0;
}
#ifdef OPT_ESPEAK
SpeakStr(s_asGateNames[g_InControlState.GaitType]);
#endif
GaitSelect();
}
//Double leg lift height
if (ljoy.buttonPressed(JOYSTICK_BUTTONS::R2))
{
MSound( 1, 50, 2000);
// wrap around mode
HeightSpeedMode = (HeightSpeedMode + 1) & 0x3;
DoubleTravelOn = HeightSpeedMode & 0x1;
if ( HeightSpeedMode & 0x2)
g_InControlState.LegLiftHeight = 80;
else
g_InControlState.LegLiftHeight = 50;
}
// Switch between Walk method 1 && Walk method 2
if (ljoy.buttonPressed(JOYSTICK_BUTTONS::R3))
{
MSound (1, 50, 2000);
WalkMethod = !WalkMethod;
}
//Walking
if (WalkMethod) //(Walk Methode)
//Right Stick Up/Down
g_InControlState.TravelLength.z = ((axes_ry/256));
else
{
g_InControlState.TravelLength.x = -(axes_lx/256);
g_InControlState.TravelLength.z = (axes_ly/256);
}
if (!DoubleTravelOn) //(Double travel length)
{
g_InControlState.TravelLength.x = g_InControlState.TravelLength.x/2;
g_InControlState.TravelLength.z = g_InControlState.TravelLength.z/2;
}
//Right Stick Left/Right
g_InControlState.TravelLength.y = -((axes_rx/256))/4;
}
//[Translate functions]
g_BodyYShift = 0;
if (ControlMode == TRANSLATEMODE)
{
g_InControlState.BodyPos.x = SmoothControl((((axes_lx/256))*2/3), g_InControlState.BodyPos.x, SmDiv);
g_InControlState.BodyPos.z = SmoothControl((((axes_ly/256))*2/3), g_InControlState.BodyPos.z, SmDiv);
g_InControlState.BodyRot1.y = SmoothControl((((axes_rx/256))*2), g_InControlState.BodyRot1.y, SmDiv);
g_BodyYShift = (-((axes_ry/256))/2);
}
//[Rotate functions]
if (ControlMode == ROTATEMODE)
{
g_InControlState.BodyRot1.x = ((axes_ly/256));
g_InControlState.BodyRot1.y = ((axes_rx/256))*2;
g_InControlState.BodyRot1.z = ((axes_lx/256));
g_BodyYShift = (-((axes_ry/256))/2);
}
//Calculate walking time delay
g_InControlState.InputTimeDelay = 128 - max(max(abs((axes_lx/256)), abs((axes_ly/256))), abs((axes_rx/256)));
//Calculate g_InControlState.BodyPos.y
g_InControlState.BodyPos.y = max(g_BodyYOffset + g_BodyYShift, 0);
// Add Dynamic leg position and body height code
if (sLegInitXZAdjust || sLegInitAngleAdjust)
{
// User asked for manual leg adjustment - only do when we have finished any previous adjustment
if (!g_InControlState.ForceGaitStepCnt)
{
if (sLegInitXZAdjust)
g_fDynamicLegXZLength = true;
sLegInitXZAdjust += GetLegsXZLength(); // Add on current length to our adjustment...
// Handle maybe change angles...
#ifdef ADJUSTABLE_LEG_ANGLES
if (sLegInitAngleAdjust)
RotateLegInitAngles(sLegInitAngleAdjust);
#endif
// Give system time to process previous calls
AdjustLegPositions(sLegInitXZAdjust);
}
}
if (fAdjustLegPositions && !g_fDynamicLegXZLength)
AdjustLegPositionsToBodyHeight(); // Put main workings into main program file
}
else
{
// We did not receive a valid packet. check for a timeout to see if we should turn robot off...
if (g_InControlState.fRobotOn)
{
if (! ljoy.connected()) // See if controller is still connected?
{
g_InControlState.fControllerInUse = true; // make sure bypass is not used.
ControllerTurnRobotOff();
}
}
}
}
