void resetGame()
{
resetLevel();
initRobot();
setFadeMode(&reset_fade,FADE_OUT,1);
}
/*
* main.c
*/
int main(void)
{
WDTCTL = WDTPW | WDTHOLD; // Stop watchdog timer
initRobot();
modTimer(70, 40);
moveForward();
P1DIR |= (BIT0 | BIT6);
for (;;) {
if (isRightClose() == FALSE) {
P1OUT &= ~BIT0;
P1OUT |= BIT6;
halfLeft();
__delay_cycles(100000);
} else if (isCenterClose() == FALSE) {
halfLeft();;
P1OUT &= ~BIT6;
P1OUT |= BIT0;
} else {
P1OUT &= ~BIT0;
P1OUT &= ~BIT6;
moveForward();
}
}
return 0;
}
int main(void)
{
initRobot();
moveForward();
stopRobot();
}
void torsoController::InitializeRobot(IRobotStatusProvider* robotStatusProvider)
{
m_robotStatusProvider = robotStatusProvider;
realTorso.SetProvider(robotStatusProvider);
initRobot(false);
printf("initialize end\n");
return;
}
int main(int argc, char **argv)
{
initRobot();
loadVector();
//print something from vector to check it was read correctly
//[timeInst][RX][Data 0-8: Rx X Y Sp Dir ObstN ObstW ObstE ObstS]
// printf("Check timeInst 1, R2 = %d\n", timeInstMatrix[40][1][0]);
return scheduler();
}
void
SSLPathPlanner::init ()
{
robots_control_type[0] = ssl::AUTONOMOUS;
robots_control_type[1] = ssl::AUTONOMOUS;
robots_control_type[2] = ssl::AUTONOMOUS;
robots_control_type[3] = ssl::AUTONOMOUS;
robots_control_type[4] = ssl::AUTONOMOUS;
manifold = StateManifoldPtr (new RealVectorStateManifold (2));
RealVectorBounds bounds (2);
bounds.setLow (0, -(ssl::config::FIELD_WIDTH / 2.0 + 0.5));
bounds.setHigh (0, (ssl::config::FIELD_WIDTH / 2.0 + 0.5));
bounds.setLow (1, -(ssl::config::FIELD_HEIGHT / 2.0 + 0.5));
bounds.setHigh (1, (ssl::config::FIELD_HEIGHT / 2.0 + 0.5));
manifold->as<RealVectorStateManifold> ()->setBounds (bounds);
planner_setup = new SimpleSetup (manifold);
planner_setup->setStateValidityChecker (boost::bind (&SSLPathPlanner::isStateValid, this, _1));
pRRT* p_rrt = new pRRT (planner_setup->getSpaceInformation ());
p_rrt->setGoalBias (0.8);
p_rrt->setRange (0.08);
p_rrt->setThreadCount (8);
// RRTConnect* rrt_connect = new RRTConnect (planner_setup->getSpaceInformation ());
// rrt_connect->setRange (0.08);
// planner_setup->setPlanner (PlannerPtr (rrt_connect));
planner_setup->setPlanner (PlannerPtr (p_rrt));
//wait for global_state to finish initialization procedure
std::cout << "Path Planner is waiting for global_data" << std::endl;
while (!global_st_rcvd && nh.ok ())
ros::spinOnce ();
//wait for pose_control to finish initialization procedure
std::cout << "Path Planner is waiting for pose_control" << std::endl;
while (!pose_ctrl_rcvd && nh.ok ())
ros::spinOnce ();
std::cout << "Path Planner is waiting for odom_control" << std::endl;
while (!odom_rcvd && nh.ok ())
ros::spinOnce ();
//do initial plans for the robots according to the control configuration
for (unsigned int i = 0; i < ssl::config::TEAM_CAPACITY; i++)
{
if (robots_control_type[i] != ssl::NO_CONTROL)
{
if (team_state_[i].state != ssl::OUT_OF_FOV)
initRobot (i);
}
}
}
void main()
{
initRobot();
while(1)
{
//lettura dello stato dei sensori
line_R = Adc_Read(5);
line_C = Adc_Read(6);
line_L = Adc_Read(7);
if(line_L<512 & line_C>512 & line_R<512)
{
// 0-1-0 vado dritto alla veocità di crociera
PWM1_Change_Duty(190);
PWM2_Change_Duty(190);
Delay_ms(5);
}
if(line_L<512 & line_C>512 & line_R>512)
{
// 0-1-1 deve accelerare la ruota sinistra
PWM1_Change_Duty(145);
PWM2_Change_Duty(190);
Delay_ms(5);
}
if(line_L<512 & line_C<512 & line_R>512)
{
// 0-0-1 deve accelerare la ruota sinistra
PWM1_Change_Duty(135);
PWM2_Change_Duty(210);
Delay_ms(5);
}
if(line_L>512 & line_C>512 & line_R<512)
{
// 1-1-0 deve accelerare la ruota destra
PWM1_Change_Duty(190);
PWM2_Change_Duty(145);
Delay_ms(5);
}
if(line_L>512 & line_C<512 & line_R<512)
{
// 1-0-0 deve accelerare la ruota destra
PWM1_Change_Duty(210);
PWM2_Change_Duty(135);
Delay_ms(5);
}
if(line_L<512 & line_C<512 & line_R<512)
{
// 0-0-0 fuori dalla linea, indietro
PWM1_Change_Duty(90);
PWM2_Change_Duty(90);
Delay_ms(5);
}
} //end main loop
} //end main()
int mainRobot(int argc, char** argv) {
glutInit(&argc, argv);
glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB);
glutInitWindowSize(500, 500);
glutInitWindowPosition(100, 100);
glutCreateWindow(argv[0]);
initRobot();
glutDisplayFunc(displayRobot);
glutReshapeFunc(reshapeRobot);
glutKeyboardFunc(keyboardRobot);
glutMainLoop();
return 0;
}
int main(void){
char data[9];
initRobot();
if(getSensors(&data[0]) == -1) printf("chyba\n");
// if(getULT(&data[0]) == -1) printf("chyba\n");
// int data[10];
// int dataULT[10];
// readSensors(&data[0]);
// readULT(&dataULT[0]);
printf("%d\n",data[1]);
// LED3(0);
return 1;
}
/*
* main.c
*/
void main(void) {
WDTCTL = WDTPW | WDTHOLD; // Stop watchdog timer
//Delays dictate how long each function is executed. To move forward or backward a further distance,
//increase the delay after the moveRobotForward or moveRobotBackward functions. To turn larger angles,
//increase the delay after the turnRobotRight or turnRobotLeft functions.
initRobot();
__delay_cycles(3000000); //Delay here to give user time to get robot to floor
moveRobotForward();
__delay_cycles(1000000);
stopRobot();
__delay_cycles(1000000);
moveRobotBackward();
__delay_cycles(1000000);
stopRobot();
__delay_cycles(1000000);
turnRobotRight();
__delay_cycles(300000);
stopRobot();
__delay_cycles(1000000);
turnRobotLeft();
__delay_cycles(300000);
stopRobot();
__delay_cycles(1000000);
turnRobotRight();
__delay_cycles(900000);
stopRobot();
__delay_cycles(1000000);
turnRobotLeft();
__delay_cycles(900000);
stopRobot();
while(1)
{
}
}
/*
* main.c
*/
int main(void) {
WDTCTL = WDTPW | WDTHOLD; // Stop watchdog timer
initRobot();
while (1) {
moveRobotForward(100);
__delay_cycles(700000);
moveRobotBackward(100);
__delay_cycles(700000);
turnRobotLeft(100);
__delay_cycles(700000);
turnRobotRight(100);
__delay_cycles(700000);
turnRobotLeft(100);
__delay_cycles(1500000);
turnRobotRight(100);
__delay_cycles(1500000);
}
}
RobotMeshModel::RobotMeshModel():nh_priv_("~") {
//Load robot description from parameter server
std::string robot_desc_string;
if(!nh_.getParam("robot_description", robot_desc_string)) {
ROS_ERROR("Could not get urdf from param server");
}
if (!urdf_.initString(robot_desc_string)) {
ROS_ERROR("Failed to parse urdf");
}
modelframe_= "/torso_lift_link";
nh_priv_.param<std::string>("robot_description_package_path", description_path, "..");
ROS_INFO("package_path %s", description_path.c_str());
nh_priv_.param<std::string>("camera_topic", camera_topic_, "/wide_stereo/right" );
nh_priv_.param<std::string>("camera_info_topic", camera_info_topic_, "/wide_stereo/right/camera_info" );
//Load robot mesh for each link
std::vector<boost::shared_ptr<urdf::Link> > links ;
urdf_.getLinks(links);
for (int i=0; i< links.size(); i++) {
if (links[i]->visual.get() == NULL) continue;
if (links[i]->visual->geometry.get() == NULL) continue;
if (links[i]->visual->geometry->type == urdf::Geometry::MESH) {
//todo: this should really be done by resource retriever
boost::shared_ptr<urdf::Mesh> mesh = boost::dynamic_pointer_cast<urdf::Mesh> (links[i]->visual->geometry);
std::string filename (mesh->filename);
if (filename.substr(filename.size() - 4 , 4) != ".stl" && filename.substr(filename.size() - 4 , 4) != ".dae") continue;
if (filename.substr(filename.size() - 4 , 4) == ".dae")
filename.replace(filename.size() - 4 , 4, ".stl");
ROS_INFO("adding link %d %s",i,links[i]->name.c_str());
filename.erase(0,25);
filename = description_path + filename;
boost::shared_ptr<CMeshO> mesh_ptr(new CMeshO);
if(vcg::tri::io::ImporterSTL<CMeshO>::Open(*mesh_ptr,filename.c_str())) {
ROS_ERROR("could not load mesh %s", filename.c_str());
continue;
}
links_with_meshes.push_back(links[i]);
meshes[links[i]->name] = mesh_ptr;
tf::Vector3 origin(links[i]->visual->origin.position.x, links[i]->visual->origin.position.y, links[i]->visual->origin.position.z);
tf::Quaternion rotation(links[i]->visual->origin.rotation.x, links[i]->visual->origin.rotation.y, links[i]->visual->origin.rotation.z, links[i]->visual->origin.rotation.w);
offsets_[links[i]->name] = tf::Transform(rotation, origin);
}
}
initRobot();
//get camera intinsics
ROS_INFO("waiting for %s", camera_info_topic_.c_str());
cam_info_ = ros::topic::waitForMessage<sensor_msgs::CameraInfo>(camera_info_topic_);
cameraframe_ = cam_info_->header.frame_id;
ROS_INFO("%s: robot model initialization done!", ros::this_node::getName().c_str());
}
int main(void) {
WDTCTL = WDTPW | WDTHOLD; // Stop watchdog timer
initMSP430();
while (1) {
if (packetIndex > 40) {
_disable_interrupt();
while(packetData[i] != 2 && i < 80){
i++;
}
for(j = 0; j < 31; j++){
i++;
irPacket += packetData[i];
irPacket <<= 1;
}
if(irPacket == CH_UP){
initRobot();
moveForward(50);
newPacket = TRUE;
}
if(irPacket == CH_DW){
initRobot();
moveBackward(50);
newPacket = TRUE;
}
if(irPacket == VOL_UP){
initRobot();
turnRight(50);
newPacket = TRUE;
}
if(irPacket == VOL_DW){
initRobot();
turnLeft(50);
newPacket = TRUE;
}
if(irPacket == ONE){
newPacket = TRUE;
}
if(irPacket == TWO){
newPacket = TRUE;
}
if(irPacket == THR){
newPacket = TRUE;
}
if(irPacket == PWR){
newPacket = TRUE;
shutDown();
}
if(newPacket == TRUE){
initMSP430();
newPacket = FALSE;
}
i = 0;
packetIndex = 0;
_enable_interrupt();
} // end if new IR packet arrived
}
}
int main(int argc,char *argv[]) {
struct sockaddr_in server;
int
sock,
port,
err;
char
buff[2000];
if (argc != 3) {
printf("Numero de argumentos invalido\n");
printf("Modo de uso:\n\n");
printf("\t robot [IP] [PORT]\n\n");
printf("\t IP: dirección ip del servidor. e.j. 127.0.0.1\n");
printf("\t PORT: número del puerto del servidor. e.j. 5600\n\n");
exit(-1);
}
sock = socket(PF_INET, SOCK_STREAM, 0);
port = atoi(argv[2]);
server.sin_family = AF_INET;
server.sin_port = htons(port);
server.sin_addr.s_addr = inet_addr(argv[1]);
err = connect(sock, (struct sockaddr *)&server, sizeof(server));
if (err == -1) {
printf("No se pudo establecer la conexión con el servidor\n");
exit(-1);
}
FD = fdopen(sock, "w+");
signal(SIGCONT, s_cont);
if (pipe(pipeFD) == -1) {
printf("Error al inicial el pipe. pipe() returned -1\n");
exit(-1);
}
setbuf(FD, NULL);
initRobot();
buff[0] = '\0';
if (login() == FALSE) {
printf("Error al iniciar la sesion\n");
fclose(FD);
exit(-1);
}
CONTROLLER_ID = getpid();
ROBOT_ID = fork();
if (ROBOT_ID == -1) {
printf("Error al iniciar, fork() returned -1\n");
exit(-1);
}
if (ROBOT_ID > 0) {
signal(NTFY_ROBOT_PAUSE, pausedRobot);
// CONTROLLER
while (_fgets(buff, 2000, FD) != NULL) {
debug printf("[Controller %d] msg from server: %s\n", getpid(), buff);
if (strpos(buff, "S ACCEPTED") != -1) {
kill(ROBOT_ID, S_ACCEPTED);
} else if (strpos(buff, "S WIN") != -1) {
kill(ROBOT_ID, S_WIN);
break;
} else if (strpos(buff, "S LOSE") != -1) {
kill(ROBOT_ID, S_LOSE);
break;
} else if (strpos(buff, "S DRAW") != -1) {
kill(ROBOT_ID, S_DRAW);
break;
} else if (strpos(buff, "S URTURN") != -1) {
if (resumeRobot() == 0) {
write(pipeFD[1], buff, strlen(buff) + 1);
}
debug printf("[Controller %d]: Durmiendo\n", getpid());
pause();
debug printf("[Controller %d]: Despertado\n", getpid());
}
}
kill(ROBOT_ID, SIGTERM);
wait(ROBOT_ID);
shutdown(sock, 2);
fclose(FD);
close(pipeFD[0]);
close(pipeFD[1]);
debug printf("## Fin del programa ##\n");
exit(0);
} else if (ROBOT_ID == 0) {
// ROBOT
ROBOT_ID = getpid();
signal(S_ACCEPTED, s_accepted);
signal(S_WIN, s_win);
signal(S_LOSE, s_lose);
signal(S_DRAW, s_draw);
while(1) {
// se bloquea a la espera del tubo
doTurn();
}
}
}
/**
* Main robot function that controls the behaviour of the robot.
* Needs a connected socket to communicate with the simulator or HW
*/
void robot(int sck, FILE *sckfd)
{
int val1;
int curX = 2;
int curY = 0;
int direction = NORTH;
initRobot(sck, 100, curX, curY);
//initGrid();
int nummoves = 0;
// Challenge A/B
int destX = 5, destY = 5;
FILE *file;
file = fopen("log.txt", "w");
// Loop until destination is reached
while (1) {
// system("cls");
int nextX, nextY, dir, rotation;
calculateRoute(curX, curY, destX, destY);
d_printGrid(curX, curY);
if (!nodes[curX][curY]->next) {
fprintf(stderr, "No route found\n");
sendExit(sck);
return;
}
nextX = nodes[curX][curY]->next->pos.x;
nextY = nodes[curX][curY]->next->pos.y;
dir = findDirection(curX, curY, nextX, nextY);
rotation = dir - direction;
if (rotation > 2)
rotation -= 4;
if (rotation < -2)
rotation += 4;
fprintf(stderr, "Next (%d,%d), rotation: %d\n", nextX, nextY, dir);
if (rotation < 0) { //turn left
fprintf(file, "Turn left %d\r\n", abs(rotation));
setCommand(sck, TURN, 1, abs(rotation), 0);
} else if (rotation > 0) {//turn right
fprintf(file, "Turn right %d\r\n", abs(rotation));
setCommand(sck, TURN, 2, rotation, 0);
}
fflush(file);
// wait until robot is ready with turning
if (rotation != 0) {
while (1) {
getCommand(sck, sckfd, READY, 1, &val1, NULL);
if (val1 == 1)
break;
}
fprintf(file, "Ready!\r\n");
}
// Update direction
direction = dir;
if (curY == 0 || curY == 6 || curX == 0 || curX == 6)
nummoves = 1;
else
nummoves = 2;
printf("Cur (%d,%d) Nummoves: %d\n", curX, curY, nummoves);
printf("Move forward\n");
fprintf(file, "Move forward %d\r\n", nummoves);
fflush(file);
setCommand(sck, MOVE, 1, nummoves, 0);
if (nummoves == 1) {
// Wait while not ready
while (1) {
getCommand(sck, sckfd, READY, 1, &val1, NULL);
if (val1 == 1) {
break;
}
}
curX = nextX;
curY = nextY;
} else if (nummoves == 2) {
// Wait while not ready
while (1) {
getCommand(sck, sckfd, READY, 1, &val1, NULL);
if (val1 == 1) {
break;
}
}
// Check for mine
printf("Check for mine\n");
getCommand(sck, sckfd, MINE, 1, &val1, NULL);
if (val1 == 1) {
//removeConnection(x1, y1, x2, y2);
// Clear mine flag
setCommand(sck, MINE, 1, 0, 0);
fprintf(file, "Mine found!\r\n");
printf("Mine found!\n");
// Get back!:D
//return 0;
} else {
printf("No mine found!\n");
}
}
}
fclose(file);
sendExit(sck);
}
