//===================================================================================================
// Setup
//====================================================================================================
void setup() {
// Lets initialize the Commander
Serial.begin();
if (!command.begin("/dev/ttyXBEE", B38400))
{
printf("Commander Begin failed\n");
return;
}
// Next initialize the Bioloid
bioloid.poseSize = CNT_SERVOS;
// Read in the current positions...
printf("Before readPose\n");
bioloid.readPose();
printf("After readPose\n");
for (int i=1; i <= CNT_SERVOS; i++) {
Serial.println(dxl_read_word(i, AX_PRESENT_POSITION_L), DEC);
}
// Start off to put arm to sleep...
Serial.println("Kurt's Arm");
PutArmToSleep();
MSound(3, 60, 2000, 80, 2250, 100, 2500);
}
void CheckVoltages(void)
{
#ifdef AVS_PIN
uint16_t wVS = analogRead(AVS_PIN);
#ifdef DEBUG
if (g_fDebugOutput && (wVS != g_wVSPrev)) {
Serial.print("VS: ");
Serial.println(wVS, DEC);
g_wVSPrev = wVS;
}
#endif
if ((wVS < (uint16_t)AVAL_MIN) && g_fServosInit) {
// detach any servos we may have...
MSound( 3, 100, 2500, 100, 2500, 100, 2500);
}
#elif defined(PACKET_MODE)
// Using Roboclaw in packet mode, so we can ask it for the
// motor voltages... Note: This requires us to send and receive
// stuff serially, so probably only do this every so often.
if ((millis()-g_ulCVTimeLast) > 1000)
{
// more than a second since we last tested
uint16_t wVS = RClaw.ReadMainBatteryVoltage(PACKETS_ADDRESS);
#ifdef DEBUG
if (g_fDebugOutput && (wVS != g_wVSPrev)) {
Serial.print("VS: ");
Serial.println(wVS, DEC);
g_wVSPrev = wVS;
}
#endif
if ((wVS < (uint16_t)VOLTAGE_MIN1) && g_fServosInit) {
// detach any servos we may have...
MSound( 3, 100, 2500, 100, 2500, 100, 2500);
}
g_ulCVTimeLast = millis(); // remember last we did it.
}
#endif
}
//==============================================================================
// 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();
}
}
uint8_t doArmIK(boolean fCartesian, int sIKX, int sIKY, int sIKZ, int sIKGA)
{
int t;
int sol0;
uint8_t bRet = IKS_SUCCESS; // assume success
#ifdef DEBUG
if (g_fDebugOutput) {
Serial.print("(");
Serial.print(sIKX, DEC);
Serial.print(",");
Serial.print(sIKY, DEC);
Serial.print(",");
Serial.print(sIKZ, DEC);
Serial.print(",");
Serial.print(sIKGA, DEC);
Serial.print(")=");
}
#endif
if (fCartesian) {
// first, make this a 2DOF problem... by solving base
sol0 = radToServo(atan2(sIKX,sIKY));
// remove gripper offset from base
t = sqrt(sq((long)sIKX)+sq((long)sIKY));
// BUGBUG... Not sure about G here
#define G 30
sol0 -= radToServo(atan2((G/2)-G_OFFSET,t));
}
else {
// We are in cylindrical mode, probably simply set t to the y we passed in...
t = sIKY;
#ifdef DEBUG
sol0 = 0;
#endif
}
// convert to sIKX/sIKZ plane, remove wrist, prepare to solve other DOF
float flGripRad = (float)(sIKGA)*3.14159/180.0;
long trueX = t - (long)((float)WristLength*cos(flGripRad));
long trueZ = sIKZ - BaseHeight - (long)((float)WristLength*sin(flGripRad));
long im = sqrt(sq(trueX)+sq(trueZ)); // length of imaginary arm
float q1 = atan2(trueZ,trueX); // angle between im and X axis
long d1 = sq(ShoulderLength) - sq(ElbowLength) + sq((long)im);
long d2 = 2*ShoulderLength*im;
float q2 = acos((float)d1/float(d2));
q1 = q1 + q2;
int sol1 = radToServo(q1-1.57);
d1 = sq(ShoulderLength)-sq((long)im)+sq(ElbowLength);
d2 = 2*ElbowLength*ShoulderLength;
q2 = acos((float)d1/(float)d2);
int sol2 = radToServo(3.14-q2);
// solve for wrist rotate
int sol3 = radToServo(3.2 + flGripRad - q1 - q2 );
#ifdef DEBUG
if (g_fDebugOutput) {
Serial.print("<");
Serial.print(sol0, DEC);
Serial.print(",");
Serial.print(trueX, DEC);
Serial.print(",");
Serial.print(trueZ, DEC);
Serial.print(",");
Serial.print(sol1, DEC);
Serial.print(",");
Serial.print(sol2, DEC);
Serial.print(",");
Serial.print(sol3, DEC);
Serial.print(">");
}
#endif
// Lets calculate the actual servo values.
if (fCartesian) {
sBase = min(max(512 - sol0, BASE_MIN), BASE_MAX);
}
sShoulder = min(max(512 - sol1, SHOULDER_MIN), SHOULDER_MAX);
// Magic Number 819???
sElbow = min(max(819 - sol2, SHOULDER_MIN), SHOULDER_MAX);
#define Wrist_Offset 512
sWrist = min(max(Wrist_Offset + sol3, WRIST_MIN), WRIST_MAX);
// Remember our current IK positions
g_sIKX = sIKX;
g_sIKY = sIKY;
g_sIKZ = sIKZ;
g_sIKGA = sIKGA;
// Simple test im can not exceed the length of the Shoulder+Elbow joints...
if (im > (ShoulderLength + ElbowLength)) {
if (g_bIKStatus != IKS_ERROR) {
#ifdef DEBUG
if (g_fDebugOutput) {
Serial.println("IK Error");
}
#endif
MSound(2, 50, 3000, 50, 3000);
}
bRet = IKS_ERROR;
}
else if(im > (ShoulderLength + ElbowLength-IK_FUDGE)) {
if (g_bIKStatus != IKS_WARNING) {
#ifdef DEBUG
if (g_fDebugOutput) {
Serial.println("IK Warning");
}
#endif
MSound(1, 75, 2500);
}
bRet = IKS_WARNING;
}
return bRet;
}
//===================================================================================================
// 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();
}
}
}
}
//==============================================================================
// 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 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
}
//==============================================================================
// 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);
}
}
}
}
//--------------------------------------------------------------------------
// Main: the main function.
//--------------------------------------------------------------------------
int main(int argc, char *argv[])
{
// 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);
char abT[40]; // give a nice large buffer.
uint8_t cbRead;
printf("Start\n");
if (argc > 1)
{
for (int i=1; i < argc; i++)
{
printf("%d - %s\n", i, argv[i]);
}
}
char *pszDevice;
if (!RClaw.begin(pszDevice = (argc > 1? argv[1] : szRClawDevice), B38400))
{
printf("RClaw (%s) Begin failed\n", pszDevice);
return 0;
}
if (!command.begin(pszDevice = (argc > 2? argv[2] : szCommanderDevice), B38400))
{
printf("Commander (%s) Begin failed\n", pszDevice);
return 0;
}
int error;
delay(250);
Serial.begin(/*57600*/);
// Try to load the Rover Configuration Data
rcd.Load();
g_MotorsDriver.Init();
Serial.println("Kurt's Rover Program Startup\n");
g_fDebugOutput = false; // start with it off!
g_fShowDebugPrompt = true;
g_fRoverActive = false;
g_fRoverActivePrev = false;
g_fServosInit = false;
g_bGear = 3; // We init in 3rd gear.
g_bSteeringMode = ONE_STICK_MODE;
// Initialize our pan and tilt servos
InitializeServos(); // Make sure the servos are active
for(;;)
{
//--------------------------------------------------------------------------
// Loop: the main arduino main Loop function
//--------------------------------------------------------------------------
// We also have a simple debug monitor that allows us to
// check things. call it here..
if (TerminalMonitor())
continue;
CheckVoltages(); // check voltages - if we get too low shut down the servos...
// Lets get the PS2 data...
ControlInput();
// Drive the rover
if (g_fRoverActive) {
if (g_bSteeringMode == TANK_MODE) {
sRDrivePWM = LStickY; //RStickY; // BUGBUG - appears like wrong ones doing each...
sLDrivePWM = RStickY; // LStickY;
} else { // One stick driving
if ((RStickY >=0) && (RStickX >= 0)) { // Quadrant 1
sRDrivePWM = RStickY - RStickX;
sLDrivePWM = max(RStickX, RStickY);
} else if ((RStickY<0) && (RStickX>=0)) { //Quadrant 2
sRDrivePWM = (RStickY + RStickX);
sLDrivePWM = min (-RStickX, RStickY);
} else if ((RStickY<0) && (RStickX<0)) { //Quadrant 3
sRDrivePWM = min (RStickX, RStickY);
sLDrivePWM = (RStickY - RStickX);
} else if ((RStickY>=0) && (RStickX<0)) { // Quadrant 4
sRDrivePWM = max(-RStickX, RStickY);
sLDrivePWM = (RStickY + RStickX);
} else {
sRDrivePWM = 0;
sLDrivePWM = 0;
}
}
// Lets output the appropriate stuff to the motor controller
// ok lets figure out our speeds to output to the two motors. two different commands
// depending on going forward or backward.
// Scale the two values for the motors.
sRDrivePWM = max(min((sRDrivePWM * g_bGear) / 4, 127), -127); // This should keep up in the -127 to +127 range and scale it depending on what gear we are in.
sLDrivePWM = max(min((sLDrivePWM * g_bGear) / 4, 127), -127);
#ifdef DEBUG
if (g_fDebugOutput) {
if ((RStickY != RStickYPrev) || (RStickX != RStickXPrev) ||
(LStickY != LStickYPrev) || (LStickX != LStickXPrev) ||
(sRDrivePWM != sRDrivePWMPrev) || (sLDrivePWM != sLDrivePWMPrev)) {
Serial.print(LStickY, DEC);
Serial.print(",");
Serial.print(LStickX, DEC);
Serial.print(" ");
Serial.print(RStickY, DEC);
Serial.print(",");
Serial.print(RStickX, DEC);
Serial.print(" - ");
Serial.print(sLDrivePWM, DEC);
Serial.print(",");
Serial.println(sRDrivePWM, DEC);
LStickYPrev = LStickY;
LStickXPrev = LStickX;
RStickYPrev = RStickY;
RStickXPrev = RStickX;
sRDrivePWMPrev = sRDrivePWM;
sLDrivePWMPrev = sLDrivePWM;
}
}
#endif
// Call our motors driver code which may change depending on how we talk to the motors...
g_MotorsDriver.RDrive(sRDrivePWM);
g_MotorsDriver.LDrive(sLDrivePWM);
// Also if we have a pan/tilt lets update that as well..
#ifdef BBB_SERVO_SUPPORT
if (g_bSteeringMode != TANK_MODE) {
if (LStickX ) {
if (command.buttons & BUT_L6) { //modify which thing we are controlling depending on if L6 is down or not.
w = max(min(g_wRot + LStickX/8, rcd.aServos[RoverConfigData::ROTATE].wMax), rcd.aServos[RoverConfigData::ROTATE].wMin);
if (w != g_wRot) {
pinRot.SetDutyUS(w);
g_wRot = w;
}
} else {
w = max(min(g_wPan + LStickX/8, rcd.aServos[RoverConfigData::PAN].wMax), rcd.aServos[RoverConfigData::PAN].wMin);
if (w != g_wPan) {
pinPan.SetDutyUS(w);
g_wPan = w;
}
}
}
if (LStickY) {
w = max(min(g_wTilt + LStickY/8, rcd.aServos[RoverConfigData::TILT].wMax), rcd.aServos[RoverConfigData::TILT].wMin);
if (w != g_wTilt) {
pinTilt.SetDutyUS(w);
g_wTilt = w;
}
}
}
#endif
delay (10);
} else {
if (g_fRoverActivePrev) {
MSound( 3, 100, 2500, 80, 2250, 60, 2000);
g_MotorsDriver.RDrive(0);
g_MotorsDriver.LDrive(0);
}
delay (10);
}
g_fRoverActivePrev = g_fRoverActive;
}
}
