int main()
{
printf("Arbotox Commander XBee Test!\n");
// Lets try to open the XBee device...
command.begin(szDevice, B38400);
printf("After Begin!\n");
// loop simply echo what we receive from xbee to terminal
// Now lets try to get data from the commander.
for (;;)
{
if (command.ReadMsgs())
{
// We have data. see if anything has changed before
if ((command.rightV != rightV) || (command.rightH != rightH) ||
(command.leftV != leftV) || (command.leftH != leftH) ||
(command.buttons != buttons) || (command.ext != ext))
{
// Something changed so print it out
rightV = command.rightV;
rightH = command.rightH;
leftV = command.leftV;
leftH = command.leftH;
buttons = command.buttons;
ext = command.ext;
printf("%x %x - %d %d %d %d\n", buttons, ext, rightV, rightH, leftV, leftH);
}
}
usleep(100);
}
return 0;
}
int main()
{
printf("Arbotox Commander XBee Test!\n");
// Install signal handler to allow us to do some cleanup...
struct sigaction sigIntHandler;
sigIntHandler.sa_handler = SignalHandler;
sigemptyset(&sigIntHandler.sa_mask);
sigIntHandler.sa_flags = 0;
sigaction(SIGINT, &sigIntHandler, NULL);
mraa_uart_context uart;
mraa_init();
uart = mraa_uart_init(0);
if (uart == NULL) {
printf("MRAA UART failed to setup\n");
}
// Lets try to open the XBee device...
command.begin(szDevice, B38400);
printf("After Begin!\n");
// loop simply echo what we receive from xbee to terminal
// Now lets try to get data from the commander.
while (fRunning)
{
if (command.ReadMsgs())
{
// We have data. see if anything has changed before
if ((command.rightV != rightV) || (command.rightH != rightH) ||
(command.leftV != leftV) || (command.leftH != leftH) ||
(command.buttons != buttons) || (command.ext != ext))
{
// Something changed so print it out
rightV = command.rightV;
rightH = command.rightH;
leftV = command.leftV;
leftH = command.leftH;
buttons = command.buttons;
ext = command.ext;
printf("%x %x - %d %d %d %d\n", buttons, ext, rightV, rightH, leftV, leftH);
}
}
usleep(100);
}
return 0;
}
//===================================================================================================
// loop: Our main Loop!
//===================================================================================================
void loop() {
boolean fChanged = false;
if (command.ReadMsgs()) {
// See if the Arm is active yet...
if (g_fArmActive) {
sBase = g_sBase;
sShoulder = g_sShoulder;
sElbow = g_sElbow;
sWrist = g_sWrist;
sGrip = g_sGrip;
sWristRot = g_sWristRot;
if ((command.buttons & BUT_R1) && !(buttonsPrev & BUT_R1)) {
if (++g_bIKMode > IKM_BACKHOE)
g_bIKMode = 0;
// For now lets always move arm to the home position of the new input method...
// Later maybe we will get the current position and covert to the coordinate system
// of the current input method.
MoveArmToHome();
}
else if ((command.buttons & BUT_R2) && !(buttonsPrev & BUT_R2)) {
MoveArmToHome();
}
#ifdef DEBUG
if ((command.buttons & BUT_R3) && !(buttonsPrev & BUT_R3)) {
g_fDebugOutput = !g_fDebugOutput;
MSound( 1, 45, g_fDebugOutput? 3000 : 2000);
}
#endif
// Going to use L6 in combination with the right joystick to control both the gripper and the
// wrist rotate...
else if (command.buttons & BUT_L6) {
sGrip = min(max(sGrip + command.rightV/2, GRIP_MIN), GRIP_MAX);
sWristRot = min(max(g_sWristRot + command.rightH/6, WROT_MIN), WROT_MAX);
fChanged = (sGrip != g_sGrip) || (sWristRot != g_sWristRot);
}
else {
switch (g_bIKMode) {
case IKM_IK3D_CARTESIAN:
fChanged |= ProcessUserInput3D();
break;
case IKM_CYLINDRICAL:
fChanged |= ProcessUserInputCylindrical();
break;
case IKM_BACKHOE:
fChanged |= ProcessUserInputBackHoe();
break;
}
}
// If something changed and we are not in an error condition
if (fChanged && (g_bIKStatus != IKS_ERROR)) {
MoveArmTo(sBase, sShoulder, sElbow, sWrist, sWristRot, sGrip, 100, true);
}
else if (bioloid.interpolating() > 0) {
bioloid.interpolateStep();
}
}
else {
g_fArmActive = true;
MoveArmToHome();
}
buttonsPrev = command.buttons;
ulLastMsgTime = millis(); // remember when we last got a message...
}
else {
if (bioloid.interpolating() > 0) {
bioloid.interpolateStep();
}
// error see if we exceeded a timeout
if (g_fArmActive && ((millis() - ulLastMsgTime) > ARBOTIX_TO)) {
PutArmToSleep();
}
}
}
//==============================================================================
// 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();
}
}
}
}
//==============================================================================
// ControlInput -This is code the checks for and processes input from
// the Commander controller.
//==============================================================================
void ControlInput(void)
{
boolean fPacketChanged;
short JoyStickValueIn;
if(command.ReadMsgs() > 0)
{
if (!g_fRoverActive)
{
//Turn on
MSound( 3, 60, 2000, 80, 2250, 100, 2500);
g_fRoverActive = 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_fControllerInUse = command.buttons
|| (abs(command.rightH) >= cTravelDeadZone)
|| (abs(command.rightV) >= cTravelDeadZone)
|| (abs(command.leftH) >= cTravelDeadZone)
|| (abs(command.leftV) >= cTravelDeadZone);
#ifdef BBB_SERVO_SUPPORT
// If we have a pan and tilt servos use LT to reset them to zero.
if ((command.buttons & BUT_LT) && !(g_bButtonsPrev & BUT_LT))
{
g_wPan = rcd.aServos[RoverConfigData::PAN].wCenter;
g_wTilt = rcd.aServos[RoverConfigData::TILT].wCenter;
g_wRot = rcd.aServos[RoverConfigData::ROTATE].wCenter;
// Could do as one move but good enough
pinPan.SetDutyUS(g_wPan);
pinTilt.SetDutyUS(g_wTilt);
pinRot.SetDutyUS(g_wRot);
}
#endif
// Check some buttons for to see if we should be changing state...
if ((command.buttons & BUT_L4) && !(g_bButtonsPrev & BUT_L4))
{
if (g_bGear < 4) {
g_bGear++;
MSound( 1, 50, 2000);
} else {
MSound( 2, 50, 2000, 50, 2500);
}
}
if ((command.buttons & BUT_L5) && !(g_bButtonsPrev & BUT_L5))
{
if (g_bGear > 1) {
g_bGear--;
MSound( 1, 50, 2000);
} else {
MSound( 2, 50, 2500, 50, 2000);
}
}
if ((command.buttons & BUT_RT) && !(g_bButtonsPrev & BUT_RT))
{
MSound( 1, 50, 2000);
if (g_bSteeringMode == ONE_STICK_MODE)
g_bSteeringMode = TANK_MODE;
else
g_bSteeringMode = ONE_STICK_MODE;
}
// Ok lets grab the current Stick values.
LStickY = NormalizeJoystickValue(command.leftV);
LStickX = NormalizeJoystickValue(command.leftH);
RStickY = NormalizeJoystickValue(command.rightV);
RStickX = NormalizeJoystickValue(command.rightH);
// Save away the buttons state as to not process the same press twice.
g_bButtonsPrev = command.buttons;
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_fRoverActive)
{
if ((millis() - g_ulLastMsgTime) > ARBOTIX_TO)
{
g_fRoverActive = false;
// Turn off...
MSound( 3, 60, 2500, 80, 2250, 100, 2000);
}
}
}
}