//==============================================================================
// 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();
}
}
}
}
//==============================================================================
// 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 code the checks for and processes input from the DIY XBee receiver
// work
//==============================================================================
void DIYXBeeController::ControlInput(void)
{
byte iNumButton;
// Then try to receive a packet of information from the XBEE.
// It will return true if it has a valid packet
if (ReceiveXBeePacket(&g_diyp)) {
if (memcmp((void*)&g_diyp, (void*)&g_diypPrev, sizeof(g_diyp)) != 0) {
#ifdef XBEE_NEW_DATA_ONLY
if (g_diystate.fPacketForced)
SendXbeeNewDataOnlyPacket(1);
#endif
#ifdef DEBUG
// setup to output back to our USB port
if (g_fDebugOutput) {
DBGPrintf("%x - %d %d - %d %d - %d %d\n", g_diyp.s.wButtons, g_diyp.s.bRJoyLR, g_diyp.s.bRJoyUD,
g_diyp.s.bLJoyLR, g_diyp.s.bLJoyUD, g_diyp.s.bRSlider, g_diyp.s.bLSlider);
}
#endif
}
// OK lets try "0" button for Start.
if ((g_diyp.s.wButtons & (1<<0)) && ((g_diypPrev.s.wButtons & (1<<0)) == 0)) { //Start Button (0 on keypad) test
if(g_InControlState.fHexOn) {
//Turn off
g_InControlState.BodyPos.x = 0;
g_InControlState.BodyPos.y = 0;
g_InControlState.BodyPos.z = 0;
g_InControlState.BodyRot1.x = 0;
g_InControlState.BodyRot1.y = 0;
g_InControlState.BodyRot1.z = 0;
g_InControlState.TravelLength.x = 0;
g_InControlState.TravelLength.z = 0;
g_InControlState.TravelLength.y = 0;
g_BodyYOffset = 0;
g_BodyYSift = 0;
g_InControlState.SelectedLeg = 255;
g_InControlState.fHexOn = 0;
}
else {
//Turn on
g_InControlState.fHexOn = 1;
}
}
if (g_InControlState.fHexOn) {
if ((g_diyp.s.wButtons & (1<<0xa)) && ((g_diypPrev.s.wButtons & (1<<0xa)) == 0)) { // A button test
MSound(1, 50, 2000);
XBeePlaySounds(1, 50, 2000);
bXBeeControlMode = WALKMODE;
XBeeOutputStringF(F("Walking"));
}
if ((g_diyp.s.wButtons & (1<<0xb)) && ((g_diypPrev.s.wButtons & (1<<0xb)) == 0)) { // B button test
MSound(1, 50, 2000);
XBeePlaySounds(1, 50, 2000);
bXBeeControlMode = TRANSLATEMODE;
XBeeOutputStringF(F("Body Translate"));
}
if ((g_diyp.s.wButtons & (1<<0xc)) && ((g_diypPrev.s.wButtons & (1<<0xc)) == 0)) { // C button test
MSound(1, 50, 2000);
bXBeeControlMode = ROTATEMODE;
XBeeOutputStringF(F("Body Rotate"));
}
if ((g_diyp.s.wButtons & (1<<0xD)) && ((g_diypPrev.s.wButtons & (1<<0xd)) == 0)) { // D button test - Single Leg
MSound(1, 50, 2000);
if (g_InControlState.SelectedLeg==255) // none
g_InControlState.SelectedLeg=cRF;
else if (bXBeeControlMode==SINGLELEGMODE) //Double press to turn all legs down
g_InControlState.SelectedLeg=255; //none
bXBeeControlMode=SINGLELEGMODE;
XBeeOutputStringF (F("Single Leg"));
}
if ((g_diyp.s.wButtons & (1<<0xe)) && ((g_diypPrev.s.wButtons & (1<<0xe)) == 0)) { // E button test - Balance mode
if (!g_InControlState.BalanceMode) {
g_InControlState.BalanceMode = 1;
MSound( 2, 100, 2000, 50, 4000);
XBeePlaySounds(2, 100, 2000, 50, 4000);
XBeeOutputStringF(F("Balance On"));
}
else {
g_InControlState.BalanceMode = 0;
MSound( 1, 250, 1500);
XBeePlaySounds(1, 50, 1500);
XBeeOutputStringF(F("Balance Off"));
}
}
#ifdef OPT_GPPLAYER
if ((g_diyp.s.wButtons & (1<<0xf)) && ((g_diypPrev.s.wButtons & (1<<0xf)) == 0)) { // F button test - GP Player
if (g_ServoDriver.FIsGPEnabled()) { //F Button GP Player Mode Mode on/off -- SSC supports this mode
XBeeOutputStringF(F("Run Sequence"));
MSound(1, 50, 2000);
g_InControlState.BodyPos.x = 0;
g_InControlState.BodyPos.z = 0;
g_InControlState.BodyRot1.x = 0;
g_InControlState.BodyRot1.y = 0;
g_InControlState.BodyRot1.z = 0;
g_InControlState.TravelLength.x = 0;
g_InControlState.TravelLength.z = 0;
g_InControlState.TravelLength.y = 0;
g_InControlState.SelectedLeg=255; //none
g_InControlState.fSLHold=0;
bXBeeControlMode = GPPLAYERMODE;
}
else {
XBeeOutputStringF(F("Seq Disabled"));
MSound(1, 50, 2000);
}
}
#endif
//Hack there are several places that use the 1-N buttons to select a number as an index
// so lets convert our bitmap of which key may be pressed to a number...
// BUGBUG:: There is probably a cleaner way to convert... Will extract buttons 1-9
iNumButton = 0; // assume no button
if ((g_diyp.s.wButtons & 0x3fe) && ((g_diypPrev.s.wButtons & 0x3fe) == 0)) { // buttons 1-9
word w = g_diyp.s.wButtons & 0x3fe;
while ((w & 0x1) == 0) {
w >>= 1;
iNumButton++;
}
}
// BUGBUG:: we are using all keys now, may want to reserve some...
//Switch gait
// We will do slightly different here than the RC version as we have a bit per button
if ((bXBeeControlMode==WALKMODE) && iNumButton && (iNumButton <= NUM_GAITS)) { //1-8 Button Gait select
if ( abs(g_InControlState.TravelLength.x)<cTravelDeadZone && abs(g_InControlState.TravelLength.z)<cTravelDeadZone &&
abs(g_InControlState.TravelLength.y*2)<cTravelDeadZone) {
//Switch Gait type
MSound( 1, 50, 2000); //Sound P9, [50\4000]
g_InControlState.GaitType = iNumButton-1;
#ifdef DEBUG
DBGPrintf("New Gate: %d\n\r", g_InControlState.GaitType);
#endif
GaitSelect();
#ifdef DEBUG
DBGPrintf("Output Gate Named\n\r");
#endif
XBeeOutputStringF((const __FlashStringHelper *)pgm_read_word(&s_asGateNames[g_InControlState.GaitType]));
}
}
//Switch single leg
if (bXBeeControlMode==SINGLELEGMODE) {
if (iNumButton>=1 && iNumButton<=6) {
MSound( 1, 50, 2000); //Sound P9, [50\4000]
g_InControlState.SelectedLeg = iNumButton-1;
g_InControlState.fSLHold=0;
}
if (iNumButton == 9) { //Switch Directcontrol
MSound( 1, 50, 2000); //Sound P9, [50\4000]
g_InControlState.fSLHold ^= 1; //Toggle g_InControlState.fSLHold
}
}
else if (bXBeeControlMode==WALKMODE) {
g_InControlState.SelectedLeg=255; // none
g_InControlState.fSLHold=0;
}
//Body Height - Control depends on how big our packet was (ie which controller we are using)
if (g_diystate.cbPacketSize > PKT_MSLIDER)
g_InControlState.BodyPos.y = SmoothControl((g_diyp.s.bMSlider / 2), g_InControlState.BodyPos.y, SmDiv);
else if (g_diystate.cbPacketSize > PKT_LPOT)
g_InControlState.BodyPos.y = SmoothControl((g_diyp.s.bLPot*2/3), g_InControlState.BodyPos.y, SmDiv);//Zenta test
else
g_InControlState.BodyPos.y = SmoothControl((g_diyp.s.bLJoyUD / 2), g_InControlState.BodyPos.y, SmDiv);
//Leg lift height - Right slider has value 0-255 translate to 30-93
g_InControlState.LegLiftHeight = 30 + g_diyp.s.bRSlider/3;//Zenta trying 3 instead of 4
//---------------------------------------------------------------------------------------------------
//Walk mode
//---------------------------------------------------------------------------------------------------
if (bXBeeControlMode==WALKMODE) { // Kurt's Arduino version
if (g_diystate.cbPacketSize > PKT_MSLIDER) {
g_InControlState.TravelLength.x = -(g_diyp.s.bLJoyLR - 128);
g_InControlState.TravelLength.z = -(g_diyp.s.bLJoyUD - 128) ;
g_InControlState.TravelLength.y = -(g_diyp.s.bRJoyLR - 128)/3;
//g_InControlState.BodyRot1.x = SmoothControl(((bPacket(PKT_RJOYUD)-128)*2), g_InControlState.BodyRot1.x, 2);
//g_InControlState.BodyRot1.z = SmoothControl((-(bPacket(PKT_RPOT)-128)*2), g_InControlState.BodyRot1.z, 2);
g_InControlState.InputTimeDelay = 128 - max( max( abs(g_diyp.s.bLJoyLR-128), abs(g_diyp.s.bLJoyUD-128)), abs(g_diyp.s.bRJoyLR-128)) + (128 -(g_diyp.s.bLSlider)/2);
}
else if (g_diystate.cbPacketSize > PKT_LPOT) { // Case for original DIY XBee with extra pots
g_InControlState.TravelLength.x = -(g_diyp.s.bLJoyLR - 128);
g_InControlState.TravelLength.z = -(g_diyp.s.bLJoyUD - 128) ;
g_InControlState.TravelLength.y = -(g_diyp.s.bRJoyLR - 128)/3;
g_InControlState.BodyRot1.z = SmoothControl((-(g_diyp.s.bRPot-128)*2), g_InControlState.BodyRot1.z, SmDiv);
g_InControlState.InputTimeDelay = 128 - max( max( abs(g_diyp.s.bLJoyLR-128), abs(g_diyp.s.bLJoyUD-128)), abs(g_diyp.s.bRJoyLR-128)) + (128 -(g_diyp.s.bLSlider)/2);
}
else { // original DIY XBee
g_InControlState.TravelLength.x = -(g_diyp.s.bRJoyLR - 128);
g_InControlState.TravelLength.z = -(g_diyp.s.bRJoyUD - 128) ;
g_InControlState.TravelLength.y = -(g_diyp.s.bLJoyLR - 128)/3;
g_InControlState.InputTimeDelay = 128 - max( max( abs(g_diyp.s.bRJoyLR-128), abs(g_diyp.s.bRJoyUD-128)), abs(g_diyp.s.bLJoyLR-128)) + (128 -(g_diyp.s.bLSlider)/2);
}
}
//---------------------------------------------------------------------------------------------------
//Body move
//---------------------------------------------------------------------------------------------------
if (bXBeeControlMode==TRANSLATEMODE) {
if (g_diystate.cbPacketSize > PKT_LPOT) {
g_InControlState.BodyPos.x = SmoothControl(((g_diyp.s.bRJoyLR-128)*2/3), g_InControlState.BodyPos.x, SmDiv);
g_InControlState.BodyPos.z = SmoothControl(((g_diyp.s.bRJoyUD-128)*2/3), g_InControlState.BodyPos.z, SmDiv);
g_InControlState.BodyRot1.y = SmoothControl(((g_diyp.s.bLJoyLR-128)*2), g_InControlState.BodyRot1.y, SmDiv);
}
else {
g_InControlState.BodyPos.x = SmoothControl(((g_diyp.s.bRJoyLR-128)*2/3), g_InControlState.BodyPos.x, SmDiv);
g_InControlState.BodyPos.z = SmoothControl(((g_diyp.s.bRJoyUD-128)*2/3), g_InControlState.BodyPos.z, SmDiv);
g_InControlState.BodyRot1.y = SmoothControl(((g_diyp.s.bLJoyLR-128)*2), g_InControlState.BodyRot1.y, SmDiv);
}
g_InControlState.InputTimeDelay = 128 - abs(g_diyp.s.bLJoyUD-128) + (128 -(g_diyp.s.bLSlider)/2);
}
//---------------------------------------------------------------------------------------------------
//Body rotate
//---------------------------------------------------------------------------------------------------
if (bXBeeControlMode==ROTATEMODE) {
if (iNumButton &&(iNumButton <=3)) {
LjoyUDFunction = iNumButton -1;
MSound( 1, 20, 2000);
XBeeOutputStringF((const __FlashStringHelper *)pgm_read_word(&s_asLJoyUDNames[LjoyUDFunction]));
}
if (iNumButton == 4) { // Toogle Left Joystick left/Right function
LeftJoyLRmode = !LeftJoyLRmode;
if (LeftJoyLRmode) {
XBeeOutputStringF(F("LJOYLR trans"));
MSound( 1, 20, 1000);
}
else {
XBeeOutputStringF(F("LJOYLR rotate"));
MSound( 1, 20, 2000);
}
}
#ifdef DISP_VOLTAGE
if (iNumButton == 8) { // Toogle g_fDisplayLiPo
g_fDisplayLiPo = !g_fDisplayLiPo;
MSound( 1, 20, 1500+500*g_fDisplayLiPo);
}
#endif
if (iNumButton == 9) { // Toogle LockFunction
LockFunction = !LockFunction;
if (LockFunction) {
XBeeOutputStringF(F("Lock ON"));
MSound( 1, 20, 1500);
}
else {
XBeeOutputStringF(F("Lock OFF"));
MSound( 1, 20, 2500);
}
}
// BranchLJoyUDFunction in basic
switch (LjoyUDFunction) {
case 0:
g_InControlState.TravelLength.z = -(g_diyp.s.bLJoyUD-128); // dito need to update
break;
case 1:
g_InControlState.BodyPos.z = SmoothControl(((g_diyp.s.bLJoyUD-128)*2/3), g_InControlState.BodyPos.z, SmDiv);
break;
default:
if (!LockFunction) {
g_InControlState.BodyRotOffset.z = (g_diyp.s.bLJoyUD - 128);
g_InControlState.BodyRotOffset.y = (g_diyp.s.bRPot - 128);
}
}
if (LeftJoyLRmode) {//;Do X translation:
g_InControlState.BodyPos.x = SmoothControl(((g_diyp.s.bLJoyLR-128)*2/3), g_InControlState.BodyPos.x, SmDiv);
}
else {
g_InControlState.BodyRot1.y = SmoothControl (((g_diyp.s.bLJoyLR-128)*2), g_InControlState.BodyRot1.y, SmDiv);
}
g_InControlState.BodyRot1.x = SmoothControl(((g_diyp.s.bRJoyUD-128)*2), g_InControlState.BodyRot1.x, SmDiv);
g_InControlState.BodyRot1.z = SmoothControl((-(g_diyp.s.bRJoyLR-128)*2), g_InControlState.BodyRot1.z , SmDiv);
g_InControlState.InputTimeDelay = 128 - abs(g_diyp.s.bLJoyUD-128) + (128 -(g_diyp.s.bLSlider)/2);
}
//---------------------------------------------------------------------------------------------------
//Single Leg Mode
//---------------------------------------------------------------------------------------------------
if (bXBeeControlMode == SINGLELEGMODE) {
g_InControlState.SLLeg.x = SmoothControl(((g_diyp.s.bRJoyLR-128)), g_InControlState.SLLeg.x , SmDiv);//;
g_InControlState.SLLeg.z = SmoothControl((-(g_diyp.s.bRJoyUD-128)), g_InControlState.SLLeg.z , SmDiv);//;
g_InControlState.SLLeg.y = SmoothControl((-(g_diyp.s.bLJoyUD-128)), g_InControlState.SLLeg.y , SmDiv);//Need to check packetsize..
g_InControlState.InputTimeDelay = 128 - max( max( abs(g_diyp.s.bRJoyLR-128), abs(g_diyp.s.bRJoyUD-128)), abs(g_diyp.s.bLJoyLR-128)) + (128 -(g_diyp.s.bLSlider)/2);
}
//---------------------------------------------------------------------------------------------------
// Sequence General Player Mode
//---------------------------------------------------------------------------------------------------
#ifdef OPT_GPPLAYER
if (bXBeeControlMode == 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)
|| (g_diyp.s.bRJoyUD > (128+16)) || (g_diyp.s.bRJoyUD < (128-16)))
{
// We are in speed modify mode...
short sNewGPSM = map(g_diyp.s.bRJoyUD, 0, 255, -200, 200);
if (sNewGPSM != g_sGPSMController) {
g_sGPSMController = sNewGPSM;
g_ServoDriver.GPSetSpeedMultiplyer(g_sGPSMController);
}
}
// See what step we are on, if it changed then output the step to the user
byte bCurStep = g_ServoDriver.GPCurStep();
if (bCurStep != g_bGPCurStepPrev) {
g_bGPCurStepPrev = bCurStep;
// Lets build a quick and dirty string to output
char szTemp[20];
sprintf(szTemp, "cs: %d SM: %d", bCurStep, (g_sGPSMController == 32767) ? 100 : g_sGPSMController);
XBeeOutputString(szTemp);
}
}
if (iNumButton>=1 && iNumButton<=9) { //1-9 Button Play GP Seq
word wGPSeqPtr;
if (!g_ServoDriver.FIsGPSeqActive() ) {
uint8_t GPSeq = iNumButton-1;
if ( g_ServoDriver.FIsGPSeqDefined(GPSeq)) {
XBeeOutputStringF(F("Start Sequence")); // Tell user sequence started.
g_ServoDriver.GPStartSeq(GPSeq);
g_sGPSMController = 32767; // Say we are not in modifiy speed modifier mode yet...
g_bGPCurStepPrev = 0xff;
}
else {
XBeeOutputStringF(F("Seq Not defined")); // that sequence was not defined...
}
}
else {
// Cancel the current one
g_ServoDriver.GPStartSeq(0xff); // tell the GP system to abort if possible...
MSound (2, 50, 2000, 50, 2000);
}
}
}
#endif
//Calculate walking time delay
}
g_diypPrev = g_diyp; // remember the last packet
}
