static void recv_thread_entry(void* parameter)
{
rt_uint32_t ev =0;
rt_err_t ret = RT_EOK;
rt_uint16_t recv_len = 0;
while(1)
{
ret = rt_event_recv(cmd.recv_event,0x01,RT_EVENT_FLAG_AND|RT_EVENT_FLAG_CLEAR,RT_WAITING_FOREVER,&ev);
if(ret == RT_EOK)
{
recv_len = rt_device_read(cmd.dev,0,cmd.recv_buf,1024);
if (recv_len > 0)
{
int index =0;
while(index <(recv_len-4))
{
if((cmd.recv_buf[index]==0xaa)&&(cmd.recv_buf[index+1]==0xaf)&&(cmd.recv_buf[index+2]>0)&&(cmd.recv_buf[index+2]<0xf1))
{
if(cmd.recv_buf[index+3]<50)
{
uint8_t cmd_len = cmd.recv_buf[index+3];
if(cmd_len+index<recv_len)
{
cmd.Data_Receive_Anl( &cmd.recv_buf[index],cmd_len+5);
index+=cmd_len+5;
}
else
{
index++;
}
}
else
{
index+=4;
}
}
else
{
index++;
}
}
}
}
}
}
// 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");
}
//--------------------------------------------------------------------------
// SignalHandler - Try to free up things like servos if we abort.
//--------------------------------------------------------------------------
void SignalHandler(int sig){
printf("Caught signal %d\n", sig);
// Stop motors if they are active
if (g_fRoverActive) {
g_MotorsDriver.RDrive(0);
g_MotorsDriver.LDrive(0);
}
#ifdef BBB_SERVO_SUPPORT
printf("Free Servos\n");
for (int i=0; i < sizeof(g_aServos)/sizeof(g_aServos[0]); i++) {
g_aServos[i]->Disable();
}
#endif
command.end(); // tell the commander to release stuff...
exit(1);
}
//===================================================================================================
// 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();
}
}
}
}
int main(int argc, char** argv){
Commander commander = Commander();
commander.readInstructions();
printf(commander.executeInstructions( ).c_str( ));
return 0;
}
/*#################### MAIN FUNCTION #######################*/
void player_ai(const PMap &map, const PPlayerInfo &info, PCommand &cmd) {
#ifdef LOG
logger << "====Loading Observer====" << endl;
#endif
console = new Console(map, info, cmd);
// 获取比赛信息
Observer *observer = new Observer();
observer->observeGame();
observer->storeObserverInfo();
#ifdef LOG
logger << "====Successfully load observer====" << endl;
logger << "camp: " << CAMP << endl;
logger << "====Loading Commander====" << endl;
#endif
// 分析比赛局势
Commander *commander = new Commander();
commander->handleGame();
commander->storeCmdInfo();
#ifdef LOG
logger << "====Successfully load Commander====" << endl;
logger << "====Loading Hero====" << endl;
logger << "... Creating Hero List" << endl;
#endif
// make hero list
vector<Hero> heroes; // fixme debug
for (int i = 0; i < current_friends.size(); ++i) {
Hero temp(nullptr);
temp.setPtr(current_friends[i]);
heroes.push_back(temp);
}
#ifdef LOG
logger << "@Hero info" << endl;
printHeroList(heroes);
logger << "@Enemy info" << endl;
printUnit(vi_enemies);
logger << "... Heroes do their actions" << endl;
#endif
// heroes do actions and store info
for (int j = 0; j < heroes.size(); ++j) {
heroes[j].go();
heroes[j].storeMe();
}
#ifdef LOG
logger << "====Successfully handle all Heroes!====" << endl;
logger << "====Clear excess info====" << endl;
#endif
// clear vector and release pointers
clearExcessData();
releaseVector(current_friends);
releaseVector(vi_enemies);
delete observer;
delete commander;
delete console;
#ifdef LOG
logger << "====Perfect end====" << endl;
logger << "====Stored Info===" << endl;
logger << "@Stored Economy" << endl;
printString(stored_money);
logger << "@Stored Friends" << endl;
printString(stored_friends);
logger << "@Stored Tactic" << endl;
printString(stored_tactic);
logger << "@Present Miner" << endl;
for (int k = 0; k < MINE_NUM; ++k) {
logger << miner[k] << " ";
}
logger << endl;
logger << "@Present Baser" << endl;
logger << baser[0] << " " << baser[1] << endl;
logger << "@Stored Situation" << endl;
printString(stored_mine_situation);
logger << endl;
logger << endl;
#endif
}
int main(int argc, char *argv[])
{
bool RunMode; // 0 = daemon, 1 = manual
int LogMode = 0; /* 0 = Whatever RunMode says, 1 = force logging,
2 = force to stdout */
/* Parameter Processing */
if(argc != 2)
{
ParamError();
return 1;
}
for(int i = 1; i < argc; i++)
{
if(argv[i][0] == '-') // Make sure parameter is valid.
{
if(argv[i][1] == '-') // Parameter is in long-form.
{
string Parameter;
for(int k = 0; argv[i][k] != '\0'; k++) // Read argv[i] into a string
{
Parameter += argv[i][k];
}
if(Parameter == "--daemon")
{
RunMode = 0;
#ifdef DEBUG
cout << "Run mode set to daemon." << endl;
#endif
}
else if(Parameter == "--manual")
{
RunMode = 1;
#ifdef DEBUG
cout << "Run mode set to manual." << endl;
#endif
}
else if(Parameter == "--force-log")
{
LogMode = 1;
}
else if(Parameter == "--force-output")
{
LogMode = 2;
}
/* Add more long-form
options here. */
else if(Parameter == "--version")
{
cout << "garden " << version << endl;
cout << "Copyright (C) 2012 Joshua Schell" << endl;
cout << "License GPLv3+: GNU GPL version 3 or later <http://gnu.org/licenses/gpl.html>" << endl;
cout << "This is free software: you are free to change and redistribute it." << endl;
cout << "There is NO WARRANTY, to the extent permitted by law." << endl;
cout << endl;
cout << "Report bugs at: <https://github.com/Jailfindery/garden/issues>" << endl;
cout << "garden homepage: <https://github.com/Jailfindery/garden>" << endl;
cout << "DotSlashGarden Devel Blog: <http://dotslashgarden.blogspot.ca>" << endl;
return 0;
}
else if(Parameter == "--help")
{
cout << "garden " << version << endl;
cout << "Usage: garden [options]" << endl;
cout << " Options:" << endl;
cout << " -d, --daemon Runs program in daemon mode" << endl;
cout << " -m, --manual Runs program in manual mode" << endl;
cout << " --version Prints version information" << endl;
cout << " --help Prints this message" << endl;
return 0;
}
else
{
ParamError();
return 1;
}
}
else if(argv[i][2] == '\0') // Parameter is in short-form
{
switch(argv[i][1]) {
case 'd':
RunMode = 0;
#ifdef DEBUG
cout << "Run mode set to daemon." << endl;
#endif
break; // TRADITION!
case 'm':
RunMode = 1;
#ifdef DEBUG
cout << "Run mode set to manual." << endl;
#endif
break;
case 'L':
LogMode = 1;
break;
case 'O':
LogMode = 2;
break;
/* Add more short-form
options here. */
default:
ParamError();
return 1;
break;
}
}
else
{
ParamError();
return 1;
}
}
else // Parameter is some random text
{
ParamError();
return 1;
}
}
/* Program Runtime */
bool PassToReporter;
if(LogMode = 1)
{
PassToReporter = 0;
}
else if(LogMode = 2)
{
PassToReporter = 1;
}
else
{
PassToReporter = RunMode;
}
if(RunMode == 1)
{
Commander* MainCommander = new Commander;
Interface* MainInterface = MainCommander->GetInterface();
Reporter* MainReporter = new Reporter(PassToReporter, MainInterface);
bool Continue = 1;
bool& Active = Continue;
while(Active)
{
MainCommander->MainMenu(Active); // Runs the main menu.
}
delete MainCommander;
delete MainReporter;
MainCommander = 0;
MainReporter = 0;
}
else if(RunMode == 0)
{
Scheduler* MainScheduler = new Scheduler;
Interface* MainInterface = MainScheduler->GetInterface();
Reporter* MainReporter = new Reporter(PassToReporter, MainInterface);
while(true)
{
MainScheduler->InterpretTime(); // Checks for tasks to run.
sleep(5);
}
delete MainScheduler;
delete MainReporter;
MainScheduler = 0;
MainReporter = 0;
}
return 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);
}
}
}
}
//--------------------------------------------------------------------------
// 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;
}
}