Exemplo n.º 1
0
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;
}
Exemplo n.º 2
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;
}
Exemplo n.º 3
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();
            }
        }
    }
}
Exemplo n.º 5
0
//==============================================================================
// 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);
            }
        }
    }

}