/**
* @brief Sets the team to which the player belongs
*
* @param team_index 0,1 or 2 for teams red, green and blue respectively
*
* @return
*/
void setTeamName(int team_index = 0 /*default value*/)
{
switch (team_index)
{
case 0:
setTeamName("red"); break;
case 1:
setTeamName("green"); break;
case 2:
setTeamName("blue"); break;
default:
cout << "wrong team index given. Cannot set team" << endl; break;
}
}
/*
* Runs the user operator control code. This function will be started in its own task with the
* default priority and stack size whenever the robot is enabled via the Field Management System
* or the VEX Competition Switch in the operator control mode. If the robot is disabled or
* communications is lost, the operator control task will be stopped by the kernel. Re-enabling
* the robot will restart the task, not resume it from where it left off.
*
* If no VEX Competition Switch or Field Management system is plugged in, the VEX Cortex will
* run the operator control task. Be warned that this will also occur if the VEX Cortex is
* tethered directly to a computer via the USB A to A cable without any VEX Joystick attached.
*
* Code running in this task can take almost any action, as the VEX Joystick is available and
* the scheduler is operational. However, proper use of delay() or taskDelayUntil() is highly
* recommended to give other tasks (including system tasks such as updating LCDs) time to run.
*
* This task should never exit; it should end with some kind of infinite loop, even if empty.
*/
void operatorControl() {
int leftPower,rightPower;
setTeamName("Team1");
while (1) {
if (isJoystickConnected(CONTROLLER)){
leftPower = joystickGetAnalog(CONTROLLER,3);
rightPower = joystickGetAnalog(CONTROLLER,2);
motorSet(LEFTMOTOR,-leftPower);
motorSet(RIGHTMOTOR,-rightPower);
motorSet(ARM,-30*(joystickGetDigital(CONTROLLER,5,JOY_UP)-joystickGetDigital(CONTROLLER,5,JOY_DOWN)));
motorSet(CLAW,45*(joystickGetDigital(CONTROLLER,6,JOY_UP)-joystickGetDigital(CONTROLLER,6,JOY_DOWN)));
}
}
}
/**
* @brief Constructor
*
* @param name player name
* @param team team name
*/
MyPlayer(string name, string team): Player(name)
{
setTeamName(team);
ros::NodeHandle node;
prevHuntName="";
prevPreyName="";
//Initialize teams
vector<string> myTeam_names, myHunters_names, myPreys_names;
string myTeamId, myHuntersId, myPreysId;
if (!team_info(node, myTeam_names, myHunters_names, myPreys_names, myTeamId, myHuntersId, myPreysId))
ROS_ERROR("Something went wrong reading teams");
my_team = (boost::shared_ptr<Team>) new Team(myTeamId, myTeam_names);
hunter_team = (boost::shared_ptr<Team>) new Team(myHuntersId, myHunters_names);
prey_team = (boost::shared_ptr<Team>) new Team(myPreysId, myPreys_names);
my_team->printTeamInfo();
hunter_team->printTeamInfo();
prey_team->printTeamInfo();
//Initialize position according to team
ros::Duration(0.5).sleep(); //sleep to make sure the time is correct
tf::Transform t;
struct timeval t1;
gettimeofday(&t1, NULL);
srand(t1.tv_usec);
double X=((((double)rand()/(double)RAND_MAX) ) * 2 -1) * 5 ;
double Y=((((double)rand()/(double)RAND_MAX) ) * 2 -1) * 5 ;
t.setOrigin( tf::Vector3(X, Y, 0.0) );
tf::Quaternion q; q.setRPY(0, 0, 0);
t.setRotation(q);
br.sendTransform(tf::StampedTransform(t, ros::Time::now(), "/map", name));
//initialize the subscriber
_sub = (boost::shared_ptr<ros::Subscriber>) new ros::Subscriber;
*_sub = node.subscribe("/game_move", 1, &MyPlayer::moveCallback, this);
}
/*
* Runs pre-initialization code. This function will be started in kernel mode one time while the
* VEX Cortex is starting up. As the scheduler is still paused, most API functions will fail.
*
* The purpose of this function is solely to set the default pin modes (pinMode()) and port
* states (digitalWrite()) of limit switches, push buttons, and solenoids. It can also safely
* configure a UART port (usartOpen()) but cannot set up an LCD (lcdInit()).
*/
void initializeIO() {
setTeamName("934");
}
void initialize() {
setTeamName("750E");
//int exit = 0;
//imeReinitialize();
lcdInit(uart2);
lcdClear(uart2);
// int count=0;
// while (!exit) {
// delay(20);
//
// // Selecting which auton program.
// if (joystickGetDigital(1,7,JOY_DOWN)|| joystickGetDigital(2,7,JOY_DOWN))
// {
// exit=1;
// }
// else
// {
// switch (count)
// {
// //Autonomous 1.
// case 0: lcdSetText(uart2,1, "Autonomous 1");
// lcdSetText(uart2,2, "< Select >");
// if (lcdReadButtons(uart2)==LCD_BTN_LEFT){
// count=3;
// }
// else if (lcdReadButtons(uart2)==LCD_BTN_RIGHT){
// count++;
// }
// //If selected, display...
// else if (lcdReadButtons(uart2)==LCD_BTN_CENTER){
// lcdSetText(uart2,1,"Autonomous 1");
// lcdSetText(uart2,2,"is running.");
// exit=1;
// break;
// }
// break;
// //Autonomous 2.
// case 1: lcdSetText(uart2,1,"Autonomous 2");
// lcdSetText(uart2,2,"< Select >");
// if (lcdReadButtons(uart2)==LCD_BTN_LEFT){
// count--;
// }
// else if (lcdReadButtons(uart2)==LCD_BTN_RIGHT){
// count++;
// }
// //If selected, display...
// else if (lcdReadButtons(uart2)==LCD_BTN_CENTER){
// lcdSetText(uart2,1,"Autonomous 2");
// lcdSetText(uart2,2,"is running.");
// exit=1;
// break;
// }
// break;
// //Autonomous 3.
// case 2: lcdSetText(uart2,1,"Autonomous 3");
// lcdSetText(uart2,2,"< Select >");
// if (lcdReadButtons(uart2)==LCD_BTN_LEFT){
// count--;
// }
// else if (lcdReadButtons(uart2)==LCD_BTN_RIGHT){
// count++;
// }
// //If selected, display...
// else if (lcdReadButtons(uart2)==LCD_BTN_CENTER){
// lcdSetText(uart2,1,"Autonomous 3");
// lcdSetText(uart2,2,"is running.");
// exit=1;
// break;
// }
// break;
// //Autonomous 4.
// case 3: lcdSetText(uart2,1,"Autonomous 4");
// lcdSetText(uart2,2,"< Select >");
// if (lcdReadButtons(uart2)==LCD_BTN_LEFT){
// count--;
// }
// else if (lcdReadButtons(uart2)==LCD_BTN_RIGHT){
// count++;
// }
// //If selected, display...
// else if (lcdReadButtons(uart2)==LCD_BTN_CENTER){
// lcdSetText(uart2,1,"Autonomous 4");
// lcdSetText(uart2,2,"is running.");
// exit=1;
// break;
// }
// break;
// case 4: lcdSetText(uart2,1,"Self Destruct");
// lcdSetText(uart2,2,"< Select >");
// if (lcdReadButtons(uart2)==LCD_BTN_LEFT){
// count--;
// }
// else if (lcdReadButtons(uart2)==LCD_BTN_RIGHT){
// count=0;
// }
// //If selected, display...
// else if (lcdReadButtons(uart2)==LCD_BTN_CENTER){
// lcdSetText(uart2,1,"Autonomous 4");
// lcdSetText(uart2,2,"is running.");
// count=0;
// exit=1;
// break;
// }
// break;
//
// default: count=0;
// break;
//
// }
//
// }
// auton = count+1;
// }
lcdSetText(uart2,1,"750EVOLUTION");
lcdSetText(uart2,2,"THIS IS A ROBOT");
}
