WaypointControllerI.C

00001 #include "Robots/SeaBeeIII/WaypointControllerI.H"
00002 #include "Robots/RobotBrain/RobotBrainComponent.H"
00003 #include "Component/ModelParam.H"
00004 #include "Component/ModelOptionDef.H"
00005 #include "Image/MathOps.H"
00006 
00007 #ifndef WAYPOINTCONTROLLERI_C
00008 #define WAYPOINTCONTROLLERI_C
00009 
00010 #define IMG_WIDTH 320
00011 #define IMG_HEIGHT 240
00012 
00013 #define MAX_TURN_ANGLE 35
00014 #define MAX_TURN_SPEED 100
00015 #define MAX_SPEED 70
00016 
00017 // ######################################################################
00018 WaypointControllerI::WaypointControllerI(OptionManager& mgr,
00019                                          const std::string& descrName, const std::string& tagName) :
00020   RobotBrainComponent(mgr, descrName, tagName),
00021   itsCurrentHeading(0),
00022   itsCurrentDepth(0),
00023   itsImgWidth(IMG_WIDTH),
00024   itsImgHeight(IMG_HEIGHT)
00025 {
00026   itsNavigationWaypoint.heading = 0;
00027   itsNavigationWaypoint.depth = 0;
00028   itsNavigationWaypoint.speed = 0;
00029 
00030   itsTrackingWaypoint.heading = 0;
00031   itsTrackingWaypoint.depth = 0;
00032   itsTrackingWaypoint.speed = 0;
00033 
00034   itsWaypointSource = BUOY;
00035         itsBuoyTimer.reset();
00036 }
00037 
00038 // ######################################################################
00039 void WaypointControllerI::registerTopics()
00040 {
00041   LINFO("Registering Waypoint Controller Message");
00042 
00043   registerSubscription("BeeStemMessageTopic");
00044   registerSubscription("BuoyColorSegmentMessageTopic");
00045 
00046   registerPublisher("BeeStemConfigTopic");
00047   registerPublisher("XBox360RemoteControlMessageTopic");
00048 
00049 }
00050 
00051 // ######################################################################
00052 void WaypointControllerI::evolve()
00053 {
00054   if(itsWaypointSource == NAV)
00055     {
00056       LINFO("Nav");
00057     }
00058   else
00059     {
00060       if(itsBuoyTimer.getSecs() > .5)
00061         {
00062           itsTrackingWaypoint.speed = 0;
00063           itsTrackingWaypoint.heading = 0;
00064         }
00065 
00066       LINFO("Heading: %d",itsTrackingWaypoint.heading);
00067       LINFO("Depth: %d",itsTrackingWaypoint.depth);
00068       LINFO("Speed: %d",itsTrackingWaypoint.speed);
00069       LINFO("=========");
00070       sendThrusterUpdate("HEADING_AXIS",itsTrackingWaypoint.heading *-1);
00071       sendThrusterUpdate("SPEED_AXIS",itsTrackingWaypoint.speed * -1);
00072 
00073       //sendHeadingUpdate(itsTrackingWaypoint.heading);
00074       //sendSpeedUpdate(itsTrackingWaypoint.speed);
00075       sendDepthUpdate(itsTrackingWaypoint.depth);
00076     }
00077 }
00078 
00079 // ######################################################################
00080 void WaypointControllerI::updateMessage(const RobotSimEvents::EventMessagePtr& eMsg,
00081                                         const Ice::Current&)
00082 {
00083         if(eMsg->ice_isA("::RobotSimEvents::BeeStemMessage"))
00084         {
00085                 RobotSimEvents::BeeStemMessagePtr msg = RobotSimEvents::BeeStemMessagePtr::dynamicCast(eMsg);
00086                 itsCurrentHeading = msg->compassHeading;
00087                 itsCurrentDepth = msg->externalPressure;
00088         }
00089         else if(eMsg->ice_isA("::RobotSimEvents::BuoyColorSegmentMessage") && itsWaypointSource == BUOY)
00090         {
00091                 RobotSimEvents::BuoyColorSegmentMessagePtr msg = RobotSimEvents::BuoyColorSegmentMessagePtr::dynamicCast(eMsg);
00092                 //LINFO("msg->x %f,itsCurrentHeading: %d", msg->size, itsCurrentHeading);
00093                 //LINFO("Size: %f", msg->size);
00094                 if(msg->size > 100.0)
00095 
00096                 {
00097                         //LINFO("OBJECT FOUND!!");
00098                         float headingError = msg->x - 0.5;
00099                         itsTrackingWaypoint.heading = (int)(headingError * MAX_TURN_SPEED);//(int)(headingError*MAX_TURN_ANGLE + itsCurrentHeading);
00100 
00101                         //       if(itsTrackingWaypoint.heading < 0)
00102                         //         itsTrackingWaypoint.heading += 360;
00103                         //       else if(itsTrackingWaypoint.heading >= 360)
00104                         //         itsTrackingWaypoint.heading -= 360;
00105 
00106                         itsTrackingWaypoint.depth = 950;
00107                         itsTrackingWaypoint.speed = MAX_SPEED*(1 - abs(headingError));
00108                         itsBuoyTimer.reset();
00109                 }
00110                 else
00111                 {
00112                         //LINFO("No Object Found");
00113                         //itsTrackingWaypoint.heading = 0;
00114                         //itsTrackingWaypoint.depth = 950;
00115                         //itsTrackingWaypoint.speed = 0;
00116                 }
00117         }
00118 }
00119 
00120 // ######################################################################
00121 void WaypointControllerI::sendDepthUpdate(int depth)
00122 {
00123   RobotSimEvents::BeeStemConfigMessagePtr msg = new RobotSimEvents::BeeStemConfigMessage;
00124 
00125   msg->headingK = 0.0;
00126   msg->headingP = 0.0;
00127   msg->headingI = 0.0;
00128   msg->headingD = 0.0;
00129   msg->updateHeadingPID = false;
00130   msg->depthK = 0.0;
00131   msg->depthP = 0.0;
00132   msg->depthI = 0.0;
00133   msg->depthD = 0.0;
00134   msg->updateDepthPID = false;
00135   msg->enablePID = 0;
00136   msg->enableVal = 0;
00137 
00138   msg->updateDesiredValue = 2;
00139   msg->desiredDepth = depth;
00140   this->publish("BeeStemConfigTopic", msg);
00141 }
00142 
00143 // ######################################################################
00144 void WaypointControllerI::sendSpeedUpdate(int speed)
00145 {
00146   RobotSimEvents::BeeStemConfigMessagePtr msg = new RobotSimEvents::BeeStemConfigMessage;
00147 
00148   msg->headingK = 0.0;
00149   msg->headingP = 0.0;
00150   msg->headingI = 0.0;
00151   msg->headingD = 0.0;
00152   msg->updateHeadingPID = false;
00153   msg->depthK = 0.0;
00154   msg->depthP = 0.0;
00155   msg->depthI = 0.0;
00156   msg->depthD = 0.0;
00157   msg->updateDepthPID = false;
00158   msg->enablePID = 0;
00159   msg->enableVal = 0;
00160 
00161   msg->updateDesiredValue = 3;
00162   msg->desiredSpeed = speed;
00163 
00164   this->publish("BeeStemConfigTopic", msg);
00165 }
00166 
00167 // ######################################################################
00168 void WaypointControllerI::sendHeadingUpdate(int heading)
00169 {
00170   RobotSimEvents::BeeStemConfigMessagePtr msg = new RobotSimEvents::BeeStemConfigMessage;
00171 
00172   msg->headingK = 0.0;
00173   msg->headingP = 0.0;
00174   msg->headingI = 0.0;
00175   msg->headingD = 0.0;
00176   msg->updateHeadingPID = false;
00177   msg->depthK = 0.0;
00178   msg->depthP = 0.0;
00179   msg->depthI = 0.0;
00180   msg->depthD = 0.0;
00181   msg->updateDepthPID = false;
00182   msg->enablePID = 0;
00183   msg->enableVal = 0;
00184 
00185   msg->updateDesiredValue = 1;
00186   msg->desiredHeading = heading;
00187   this->publish("BeeStemConfigTopic", msg);
00188 }
00189 
00190 // ######################################################################
00191 void WaypointControllerI::sendThrusterUpdate(string thruster, int val)
00192 {
00193   RobotSimEvents::JoyStickControlMessagePtr msg = new RobotSimEvents::JoyStickControlMessage;
00194   msg->axisName = thruster;
00195   msg->axis = 0;
00196   msg->axisVal = val;
00197   msg->button = -1;
00198   msg->butVal = 0;
00199   this->publish("XBox360RemoteControlMessageTopic", msg);
00200 }
00201 
00202 #endif
00203 
Generated on Sun May 8 08:42:07 2011 for iLab Neuromorphic Vision Toolkit by  doxygen 1.6.3