00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #include "Image/OpenCVUtil.H"
00039
00040 #include "Component/ModelManager.H"
00041 #include "Devices/DeviceOpts.H"
00042 #include "GUI/XWinManaged.H"
00043 #include "Raster/GenericFrame.H"
00044 #include "Raster/Raster.H"
00045 #include "Component/ModelManager.H"
00046 #include "Devices/DeviceOpts.H"
00047 #include "GUI/XWindow.H"
00048 #include "Image/DrawOps.H"
00049 #include "Image/CutPaste.H"
00050 #include "Image/Image.H"
00051 #include "Image/Pixels.H"
00052 #include "Image/MathOps.H"
00053 #include "Neuro/EnvVisualCortex.H"
00054 #include "Media/FrameSeries.H"
00055 #include "Media/MediaOpts.H"
00056 #include "Transport/FrameInfo.H"
00057 #include "Raster/GenericFrame.H"
00058 #include "Raster/Raster.H"
00059 #include "Util/Timer.H"
00060 #include "Util/log.H"
00061 #include "Util/MathFunctions.H"
00062 #include "Learn/Bayes.H"
00063 #include "Learn/BackpropNetwork.H"
00064 #include "Envision/env_image_ops.h"
00065 #include "Neuro/BeoHeadBrain.H"
00066 #include "Image/ShapeOps.H"
00067 #include "Devices/BeeSTEM.H"
00068 #include "rutz/shared_ptr.h"
00069
00070
00071 #include <ctype.h>
00072 #include <deque>
00073 #include <iterator>
00074 #include <stdlib.h>
00075 #include <string>
00076 #include <vector>
00077 #include <map>
00078
00079 #define UP 98
00080 #define DOWN 104
00081 #define RIGHT 102
00082 #define LEFT 100
00083
00084
00085 #define THRUSTER_UP_LEFT 1
00086 #define THRUSTER_UP_RIGHT 3
00087 #define THRUSTER_UP_BACK 2
00088 #define THRUSTER_FWD_RIGHT 4
00089 #define THRUSTER_FWD_LEFT 0
00090
00091
00092 XWinManaged *xwin;
00093 Timer timer;
00094 Image<PixRGB<byte> > disp;
00095 byte SmaxVal = 0;
00096 int smap_level = -1;
00097
00098 bool debug = 0;
00099 bool init_points = true;
00100
00101
00102
00103
00104 class BeeSTEM_PID_Listener : public BeeSTEMListener
00105 {
00106 public:
00107 BeeSTEM_PID_Listener(nub::soft_ref<BeeSTEM> &b) :
00108 itsBeeStem(b)
00109 {
00110 pidHeading.reset(new PID<float>(0.1, 0, 0, -100.0, 100.0));
00111 pidPitch.reset(new PID<float>(0.1, 0, 0, -100.0, 100.0));
00112 pidRoll.reset(new PID<float>(0.1, 0, 0, -100.0, 100.0));
00113
00114 thrust_h_left = 0;
00115 thrust_h_right = 0;
00116 thrust_v_left = 0;
00117 thrust_v_right = 0;
00118 thrust_v_back = 0;
00119
00120 target_heading = 0;
00121 }
00122
00123 virtual ~BeeSTEM_PID_Listener()
00124 { }
00125
00126
00127 void setTargetHeading(int _h) {
00128 target_heading = _h;
00129 heading_enable = true;
00130
00131
00132 thrust_h_left = 0;
00133 thrust_h_right = 0;
00134 thrust_v_left = 0;
00135 thrust_v_right = 0;
00136 thrust_v_back = 0;
00137 }
00138
00139 void setTargetPitch(int _h) {
00140 target_pitch = _h;
00141 pitch_enable = true;
00142
00143
00144 thrust_h_left = 0;
00145 thrust_h_right = 0;
00146 thrust_v_left = 0;
00147 thrust_v_right = 0;
00148 thrust_v_back = 0;
00149 }
00150
00151 void disableHeadingTarget() {
00152 heading_enable = false;
00153 }
00154
00155 void disablePitchTarget() {
00156 pitch_enable = false;
00157 }
00158
00159 void setPrintOnly(bool b) {
00160 print_only = b;
00161 }
00162
00163 int getPos()
00164 {
00165 return itsCurrentPos;
00166
00167 }
00168
00169
00170
00171 virtual void event(const BeeSTEMEventType t, const unsigned char dat1,
00172 const unsigned char dat2)
00173 {
00174 LDEBUG("Event: %d dat1 = %d, dat2 = %d", int(t), dat1, dat2);
00175
00176 switch(t) {
00177
00178 case COMPASS_HEADING_EVENT:
00179 {
00180 int heading = (unsigned int)dat1*2;
00181 itsCurrentPos = heading;
00182 if (pidHeading.is_valid())
00183 {
00184 float rot = 100*pidHeading->update(target_heading, (float)heading);
00185 thrust_h_left+=(int)rot;
00186 thrust_h_right-=(int)rot;
00187
00188 if(!print_only) {
00189
00190
00191 }
00192 }
00193
00194 }
00195 break;
00196 case COMPASS_PITCH_EVENT:
00197 {
00198 if(pitch_enable)
00199 {
00200 int pitch = (signed char)dat1;
00201 if (pidPitch.is_valid())
00202 {
00203 float pitchCorr = 100*pidPitch->update(target_pitch, (float)pitch);
00204 thrust_v_left += (int)pitchCorr;
00205 thrust_v_right -= (int)pitchCorr;
00206 itsBeeStem->setMotor(THRUSTER_UP_RIGHT, (int)thrust_v_right);
00207 itsBeeStem->setMotor(THRUSTER_UP_LEFT, (int)thrust_v_left);
00208 }
00209 }
00210 }
00211 break;
00212 case COMPASS_ROLL_EVENT:
00213 {
00214 int roll = (signed char)dat1;
00215 roll++;
00216 }
00217 break;
00218 case ACCEL_X_EVENT: break;
00219 case ACCEL_Y_EVENT: break;
00220 case INT_PRESS_EVENT: break;
00221 case EXT_PRESS_EVENT:
00222
00223 break;
00224 case TEMP1_EVENT: break;
00225 case TEMP2_EVENT: break;
00226 case DIG_IN_EVENT: break;
00227 case ADC_IN_EVENT: break;
00228 case MOTOR_A_CURR_EVENT: break;
00229 case MOTOR_B_CURR_EVENT: break;
00230 case MOTOR_C_CURR_EVENT: break;
00231 case MOTOR_D_CURR_EVENT: break;
00232 case MOTOR_E_CURR_EVENT: break;
00233 case ECHO_REPLY_EVENT: LINFO("BeeSTEM Echo Reply Recieved."); break;
00234 case RESET_EVENT: LERROR("BeeSTEM RESET occurred!"); break;
00235 case SW_OVERFLOW_EVENT: LERROR("BeeSTEM Software Overflow!"); break;
00236 case FRAMING_ERR_EVENT: LERROR("BeeSTEM Framing Error!"); break;
00237 case OVR_ERR_EVENT: LERROR("BeeSTEM Hardware Overflow!"); break;
00238 case HMR3300_LOST_EVENT: break;
00239 case ACCEL_LOST_EVENT: break;
00240 case TEMP1_LOST_EVENT: break;
00241 case TEMP2_LOST_EVENT: break;
00242
00243 case ESTOP_EVENT: break;
00244 case UNRECOGNIZED_EVENT: break;
00245 case BAD_OUT_CMD_SEQ_EVENT: LERROR("BeeSTEM Reports a Bad Command Sequence!"); break;
00246 case BAD_IN_CMD_SEQ_EVENT: break;
00247 case RESET_ACK_EVENT: LINFO("BeeSTEM Acknowledges Reset Request"); break;
00248 case NO_EVENT: break;
00249 default: LERROR("Unknown event %d received!", int(t)); break;
00250
00251 }
00252
00253
00254
00255 }
00256 private:
00257 nub::soft_ref<BeeSTEM> itsBeeStem;
00258 int heading;
00259 bool heading_enable, print_only;
00260 bool pitch_enable;
00261 int target_pitch;
00262
00263 rutz::shared_ptr< PID<float> > pidHeading;
00264 rutz::shared_ptr< PID<float> > pidPitch;
00265 rutz::shared_ptr< PID<float> > pidRoll;
00266 rutz::shared_ptr< PID<float> > pidDepth;
00267
00268 signed int thrust_h_left, thrust_h_right, thrust_v_left, thrust_v_right, thrust_v_back;
00269
00270 int target_heading;
00271 int itsCurrentPos;
00272 };
00273
00274
00275 void display(Image<PixRGB<byte> > &img, Image<PixRGB<byte> > &posValImg,
00276 int pressure, int heading, int tilt, int pan);
00277
00278 int main(int argc, const char **argv)
00279 {
00280
00281 ModelManager *mgr = new ModelManager("SeaBee Interface");
00282
00283 nub::ref<InputFrameSeries> ifs(new InputFrameSeries(*mgr));
00284 mgr->addSubComponent(ifs);
00285
00286 nub::ref<OutputFrameSeries> ofs(new OutputFrameSeries(*mgr));
00287 mgr->addSubComponent(ofs);
00288
00289 nub::soft_ref<BeeSTEM> b(new BeeSTEM(*mgr,"BeeSTEM", "BeeSTEM", "/dev/ttyS1"));
00290 mgr->addSubComponent(b);
00291
00292
00293 rutz::shared_ptr<BeeSTEM_PID_Listener> PID_Listener(new BeeSTEM_PID_Listener(b));
00294 rutz::shared_ptr<BeeSTEMListener> lis2; lis2.dynCastFrom(PID_Listener);
00295 b->setListener(lis2);
00296
00297 mgr->exportOptions(MC_RECURSE);
00298
00299 mgr->setOptionValString(&OPT_InputFrameSource, "V4L2");
00300 mgr->setOptionValString(&OPT_FrameGrabberMode, "YUYV");
00301 mgr->setOptionValString(&OPT_FrameGrabberDims, "1024x576");
00302 mgr->setOptionValString(&OPT_FrameGrabberByteSwap, "no");
00303 mgr->setOptionValString(&OPT_FrameGrabberFPS, "30");
00304
00305
00306 if (mgr->parseCommandLine(argc, argv, "", 0, 0) == false) return(1);
00307
00308
00309 Dims imageDims(320,240);
00310
00311 xwin = new XWinManaged(Dims(imageDims.w()*2,imageDims.h()*2+20),
00312 -1, -1, "SeaBee interface");
00313 disp = Image<PixRGB<byte> >(imageDims.w(),imageDims.h()+20, ZEROS);
00314
00315
00316 mgr->start();
00317
00318
00319 ifs->startStream();
00320
00321 timer.reset();
00322
00323 int target_heading = 100;
00324
00325 b->setReporting(HMR3300, true);
00326 b->setReporting(INT_PRESS, true);
00327
00328 int x = 0;
00329 Image<PixRGB<byte> > posValImg(256, 256, ZEROS);
00330
00331 while(1) {
00332
00333 const FrameState is = ifs->updateNext();
00334 if (is == FRAME_COMPLETE)
00335 break;
00336
00337
00338 GenericFrame input = ifs->readFrame();
00339 if (!input.initialized())
00340 break;
00341 Image<PixRGB<byte> > frontImg = rescale(input.asRgb(), 320, 240);
00342
00343 ofs->writeRGB(frontImg, "input", FrameInfo("Copy of input", SRC_POS));
00344
00345
00346 int key = xwin->getLastKeyPress();
00347
00348 int val = PID_Listener->getPos();
00349 int y = (256/2) + ((target_heading - val));
00350
00351 if (!x)
00352 {
00353 posValImg.clear();
00354 drawLine(posValImg, Point2D<int>(0, 256/2), Point2D<int>(256, 256/2), PixRGB<byte>(255,0,0));
00355 }
00356 LINFO("%i %i %i", val, (val - target_heading), y);
00357 posValImg.setVal(x,y,PixRGB<byte>(0,256,0));
00358 x = (x+1)%256;
00359
00360
00361 int speed = 100;
00362 switch(key)
00363 {
00364 case 38:
00365 b->setMotor(2,-1 * speed);
00366 b->setMotor(1,-1 * speed);
00367 b->setMotor(3,-1 * speed);
00368 break;
00369 case 52:
00370 b->setMotor(2,speed );
00371 b->setMotor(1,speed );
00372 b->setMotor(3,speed );
00373 break;
00374 case LEFT:
00375 b->setMotor(0, 1 * speed);
00376 b->setMotor(4, -1 * speed);
00377 break;
00378 case RIGHT:
00379
00380
00381 b->setMotor(0, -1 * speed);
00382 b->setMotor(4, 1 * speed);
00383 break;
00384 case UP:
00385 b->setMotor(0, -1 * speed);
00386 b->setMotor(4, -1 * speed);
00387 break;
00388 case DOWN:
00389 b->setMotor(0, speed);
00390 b->setMotor(4, speed);
00391 break;
00392 case 65:
00393 b->setMotor(0,0);
00394 usleep(1000);
00395 b->setMotor(1,0);
00396 usleep(1000);
00397 b->setMotor(2,0);
00398 usleep(1000);
00399 b->setMotor(3,0);
00400 usleep(1000);
00401 b->setMotor(4,0);
00402 usleep(1000);
00403 break;
00404 case 33:
00405 PID_Listener->setTargetHeading((signed char)b->getCompassPitch());
00406 break;
00407 default:
00408 if (key != -1)
00409 LINFO("Key = %i", key);
00410 break;
00411 }
00412 display(frontImg, posValImg, (int)b->getIntPress(), (int)b->getCompassHeading()*2, (signed char)b->getCompassPitch(), (signed char)b->getCompassRoll());
00413
00414 }
00415
00416
00417
00418 mgr->stop();
00419
00420
00421 return 0;
00422 }
00423
00424 void display(Image<PixRGB<byte> > &img, Image<PixRGB<byte> > &posValImg,
00425 int pressure, int heading, int tilt, int pan)
00426 {
00427 static int avgn = 0;
00428 static uint64 avgtime = 0;
00429 char msg[255];
00430
00431
00432 xwin->drawImage(img, 0, 0);
00433 xwin->drawImage(posValImg, img.getWidth()+1, 0);
00434
00435
00436 avgn++;
00437 avgtime += timer.getReset();
00438 if (avgn == 20)
00439 {
00440 avgtime = 0;
00441 avgn = 0;
00442 }
00443
00444 sprintf(msg, "pre=%i\t head=%i\t tilt=%i\t pan=%i\t ",
00445 pressure, heading, tilt, pan);
00446
00447
00448
00449 Image<PixRGB<byte> > infoImg(img.getWidth()*2, 20, NO_INIT);
00450 writeText(infoImg, Point2D<int>(0,0), msg,
00451 PixRGB<byte>(255), PixRGB<byte>(127));
00452 xwin->drawImage(infoImg, 0,img.getHeight()*2);
00453
00454 }
00455
1.6.3