#include "mobilitycomponents_i.h" #include "mobilitydata_i.h" #include "robot.h" #include #include Robot::Robot(int argc, char *argv[], bool verbose=false) { trans_correct = 0.008066; r_turn_correct = 1.0632; l_turn_correct = 1.0232; maxVelocity = 0.0; accel = 0.0; fprintf(stderr, "Starting up mobility stuff\n"); CORBA::Object_ptr ptempObj; pHelper = new mbyClientHelper(argc,argv); char *robotName = mbyUtility::get_option(argc,argv,"-robot","MagellanPro"); if (robotName == NULL) robotName = "MagellanPro"; char pathName[255]; fprintf(stderr, "About to get the ir object\n"); sprintf(pathName,"%s/IR/Segment",robotName); ptempObj = pHelper->find_object(pathName); pIRSeg = MobilityGeometry::SegmentState::_narrow(ptempObj); fprintf(stderr, "\nIR started\n"); fprintf(stderr, "About to get the sonar object\n"); sprintf(pathName,"%s/Sonar/Segment",robotName); ptempObj = pHelper->find_object(pathName); pSonarSeg = MobilityGeometry::SegmentState::_narrow(ptempObj); fprintf(stderr, "Sonar started\n"); sprintf(pathName,"%s/Drive/Location",robotName); ptempObj = pHelper->find_object(pathName); pOdoTrans = MobilityGeometry::TransformState::_narrow(ptempObj); fprintf(stderr, "Odometry Started\n"); sprintf(pathName,"%s/Drive/Command",robotName); // Use robot name arg. ptempObj = pHelper->find_object(pathName); OurCommand.velocity.length(2); pDriveCommand = MobilityActuator::ActuatorState::_duplicate( MobilityActuator::ActuatorState::_narrow(ptempObj)); fprintf(stderr, "Drive started\n"); OurCommand.velocity[0] = 0.0; OurCommand.velocity[1] = 0.0; pDriveCommand->new_sample(OurCommand,0); visionInitialized = false; fprintf(stderr, "Initialization done!\n"); } void Robot::forward(float dist, float velocity) { struct timeval *tvp1; struct timeval *tvp2; struct timeval tv1; struct timeval tv2; tvp1 = &tv1; tvp2 = &tv2; double time1, time2; int dummy; float dur; if(dist<0) velocity *= -1; dur = fabs(dist/velocity); OurCommand.velocity[0] = velocity; OurCommand.velocity[1] += (trans_correct*velocity); pDriveCommand->new_sample(OurCommand,0); dummy = gettimeofday(tvp1,NULL); time1 = tv1.tv_sec + (double)tv1.tv_usec/1000000.0; dummy = gettimeofday(tvp2,NULL); time2 = tv2.tv_sec + (double)tv2.tv_usec/1000000.0; while(time2 - time1 < dur){ pDriveCommand->new_sample(OurCommand,0); dummy = gettimeofday(tvp2,NULL); time2 = tv2.tv_sec + (double)tv2.tv_usec/1000000.0; } OurCommand.velocity[0] = 0.0; OurCommand.velocity[1] = 0.0; pDriveCommand->new_sample(OurCommand,0); } void Robot::turn(float theta) { struct timeval *tvp1; struct timeval *tvp2; struct timeval tv1; struct timeval tv2; tvp1 = &tv1; tvp2 = &tv2; double time1, time2; int dummy; float dur; float omega = 0.628; dur = fabs(theta/omega); if(theta<0) OurCommand.velocity[1] = -r_turn_correct*omega + trans_correct*OurCommand.velocity[0]; else if(theta>0) OurCommand.velocity[1] = l_turn_correct*omega + trans_correct*OurCommand.velocity[0]; else OurCommand.velocity[1] = 0.0 + trans_correct*OurCommand.velocity[0]; pDriveCommand->new_sample(OurCommand,0); dummy = gettimeofday(tvp1,NULL); time1 = tv1.tv_sec + (double)tv1.tv_usec/1000000.0; dummy = gettimeofday(tvp2,NULL); time2 = tv2.tv_sec + (double)tv2.tv_usec/1000000.0; while(time2 - time1 < dur){ pDriveCommand->new_sample(OurCommand,0); dummy = gettimeofday(tvp2,NULL); time2 = tv2.tv_sec + (double)tv2.tv_usec/1000000.0; } OurCommand.velocity[0] = 0.0; OurCommand.velocity[1] = 0.0; pDriveCommand->new_sample(OurCommand,0); } void Robot::setVelocity(float v, float w) { OurCommand.velocity[0] = v; if(w<0) OurCommand.velocity[1] = r_turn_correct*w + trans_correct*v; else if(w>0) OurCommand.velocity[1] = l_turn_correct*w + trans_correct*v; else OurCommand.velocity[1] = trans_correct*v; pDriveCommand->new_sample(OurCommand,0); } void Robot::getOdometry(float &x,float &y,float &theta) { pOdoData = pOdoTrans->get_sample(0); x=pOdoData->m[0]; y=pOdoData->m[1]; theta=(float)(atan2((double)pOdoData->m[4], (double)pOdoData->m[3])); } void Robot::setMaxVelocity(float v) { maxVelocity = v; } float Robot::getMaxVelocity() { return maxVelocity; } void Robot::setAcceleration(float a) { accel = a; } float Robot::getAcceleration() { return accel; } MobilityGeometry::SegmentData_var Robot::getSonar() { return pSonarSeg->get_sample(0); } MobilityGeometry::SegmentData_var Robot::getIR() { return pIRSeg->get_sample(0); } void Robot::initVision() { CORBA::Object_ptr ptempObj; char pathName[255]; // Build pathname to the component we want to use to drive the // robot. (in theory this should use a command line argument, but // we'll just assume that the vision server is always named // framegrab0) sprintf(pathName,"framegrab0/RawImage"); // Locate object within robot. ptempObj = pHelper->find_object(pathName); pImage = MobilityImage::ImageState::_duplicate( MobilityImage::ImageState::_narrow(ptempObj)); visionInitialized = true; fprintf(stderr, "\nCamera started\n"); } // copy the image into the (already allocated memory) // void Robot::copyImage(unsigned char *im) { if (!visionInitialized) return; pImageData = pImage->get_sample(0); for(int i=0; i<(int)pImageData->data.length(); i+= 3) { // copy R,G, and B in order im[i] = (unsigned char) pImageData->data[i+2]; im[i+1] = (unsigned char) pImageData->data[i+1]; im[i+2] = (unsigned char) pImageData->data[i]; } } // create memory for the image and put the new image there // unsigned char * Robot::getImage() { if (!visionInitialized) return 0; pImageData = pImage->get_sample(0); unsigned char * tempImg = (unsigned char *)malloc(pImageData->data.length()); for(int i=0; i<(int)pImageData->data.length(); i++) { // copy R,G, and B in order tempImg[i] = (unsigned char) pImageData->data[i+2]; tempImg[i+1] = (unsigned char) pImageData->data[i+1]; tempImg[i+2] = (unsigned char) pImageData->data[i]; } return tempImg; }