//===================================================================================================
// 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);
}
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;
}
// If both PS2 and XBee are defined then we will become secondary to the xbee
void CommanderInputController::Init(void)
{
// DBGSerial.println("Init Commander Start");
g_BodyYOffset = 0;
g_BodyYShift = 0;
command.begin(szDevice, B38400);
GPSeq = 0; // init to something...
ControlMode = WALKMODE;
HeightSpeedMode = NORM_NORM;
// DoubleHeightOn = false;
DoubleTravelOn = false;
WalkMethod = false;
// DBGSerial.println("Init Commander End");
SpeakStr("Init");
}
//--------------------------------------------------------------------------
// 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;
}
}