Ejemplo n.º 1
0
 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");

}
Ejemplo n.º 3
0
//--------------------------------------------------------------------------
// 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);

}
Ejemplo n.º 4
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();
            }
        }
    }
}
Ejemplo n.º 6
0
int main(int argc, char** argv){
    Commander commander = Commander();
    commander.readInstructions();
    printf(commander.executeInstructions(  ).c_str(  ));
    return 0;
}
Ejemplo n.º 7
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
}
Ejemplo n.º 8
0
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;
}
Ejemplo n.º 9
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);
            }
        }
    }

}
Ejemplo n.º 10
0
//--------------------------------------------------------------------------
// 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;
    }
}