diff --git a/detect.h b/detect.h index 4ca028c..b52e853 100644 --- a/detect.h +++ b/detect.h @@ -44,7 +44,10 @@ enum { POSCTL_CALIB_MODE_FINISH }; - +enum { + POSCTL_DEVTYPE_TTY = 0, + POSCTL_DEVTYPE_SIM +}; class PosCtl { @@ -53,6 +56,7 @@ class PosCtl { int mode; PID pid_axis[2]; std::string device; + int device_type; int device_fd; struct timeval calib_timestamp; int calib_mode; diff --git a/posctl.cc b/posctl.cc index 90e7ba2..c6a385e 100644 --- a/posctl.cc +++ b/posctl.cc @@ -21,6 +21,8 @@ #include "error.h" extern PosCtl posctl; +extern Simulation simulation; + extern GtkBuilder *_builder_; // work around for threads GtkWidget *posctl_rot_da = NULL; @@ -452,6 +454,7 @@ PosCtl::PosCtl() { calib_axis2max_len = 0.0; device_fd = -1; device = ""; + device_type = POSCTL_DEVTYPE_TTY; }; /* @@ -542,14 +545,18 @@ void PosCtl::CalibModeDelta(int x, int y) { OutputWriteStart(); fp.x = (x - calib_pos.x) / (float)timediff; - fp.y = (y - calib_pos.y) / (float)timediff; + fp.y = -(y - calib_pos.y) / (float)timediff; LockMutex(); calib_rot_angle = atan2(fp.x, fp.y) * 180.0 / M_PI; + while (calib_rot_angle < 0.0) calib_rot_angle += 360.0; calib_rot_len = sqrt(fp.x * fp.x + fp.y * fp.y); UnLockMutex(); calib_mode = POSCTL_CALIB_MODE_AXIS1_MI; + calib_pos.x = x; + calib_pos.y = y; + OutputWriteValue(0, a1min); gettimeofday (&calib_timestamp, NULL); gdk_threads_add_idle(cb_thread_posctl, NULL); @@ -561,6 +568,7 @@ void PosCtl::CalibModeAxis(int x, int y) { struct timeval tv; float timediff; double a1min, a1max, a2min, a2max; + position_f_2d fp; gettimeofday (&tv, NULL); timediff = (float)(tv.tv_sec - calib_timestamp.tv_sec) + ((tv.tv_usec - calib_timestamp.tv_usec) / 1000000.0); @@ -571,20 +579,37 @@ void PosCtl::CalibModeAxis(int x, int y) { pid_axis[0].GetParam(&a1min, &a1max, NULL, NULL, NULL); pid_axis[1].GetParam(&a2min, &a2max, NULL, NULL, NULL); + fp.x = (x - calib_pos.x) / (float)timediff; + fp.y = -(y - calib_pos.y) / (float)timediff; + if (calib_mode == POSCTL_CALIB_MODE_AXIS1_MI) { + calib_axis1min_angle = atan2(fp.x, fp.y) * 180.0 / M_PI; + while (calib_axis1min_angle < 0.0) calib_axis1min_angle += 360.0; + calib_axis1min_len = sqrt(fp.x * fp.x + fp.y * fp.y); OutputWriteValue(0, a1max); } else if (calib_mode == POSCTL_CALIB_MODE_AXIS1_MA) { - OutputWriteValue(0, a2min); + calib_axis1max_angle = atan2(fp.x, fp.y) * 180.0 / M_PI; + while (calib_axis1max_angle < 0.0) calib_axis1max_angle += 360.0; + calib_axis1max_len = sqrt(fp.x * fp.x + fp.y * fp.y); + OutputWriteValue(1, a2min); } else if (calib_mode == POSCTL_CALIB_MODE_AXIS2_MI) { - OutputWriteValue(0, a2max); + calib_axis2min_angle = atan2(fp.x, fp.y) * 180.0 / M_PI; + while (calib_axis2min_angle < 0.0) calib_axis2min_angle += 360.0; + calib_axis2min_len = sqrt(fp.x * fp.x + fp.y * fp.y); + OutputWriteValue(1, a2max); } else if (calib_mode == POSCTL_CALIB_MODE_AXIS2_MA) { + calib_axis2max_angle = atan2(fp.x, fp.y) * 180.0 / M_PI; + while (calib_axis2max_angle < 0.0) calib_axis2max_angle += 360.0; + calib_axis2max_len = sqrt(fp.x * fp.x + fp.y * fp.y); OutputWriteStart(); // reset speed output to 50% } calib_mode++; + calib_pos.x = x; + calib_pos.y = y; gettimeofday (&calib_timestamp, NULL); gdk_threads_add_idle(cb_thread_posctl, NULL); } @@ -635,8 +660,12 @@ void PosCtl::Loop (int posx, int posy) { void PosCtl::SetDevice (std::string d) { printf ("%s:%d %s new device:%s\n", __FILE__, __LINE__, __FUNCTION__, d.c_str()); + OutputClose(); device = d; + + if (d.compare ("SIMULATION") == 0) device_type = POSCTL_DEVTYPE_SIM; + else device_type = POSCTL_DEVTYPE_TTY; }; @@ -718,6 +747,11 @@ int PosCtl::OutputWriteValue (int axis, double value) { char outbuf[255]; printf ("%s:%d %s Axis %d Value:%f\n", __FILE__, __LINE__, __FUNCTION__, axis, value); + if (device_type == POSCTL_DEVTYPE_SIM) { + simulation.AxisSetValue(axis, value); + return 0; + } + if (device_fd <= 0) if (OutputOpen() != 0) return -1; // @@ -742,6 +776,11 @@ int PosCtl::OutputWriteStop () { printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__); + if (device_type == POSCTL_DEVTYPE_SIM) { + simulation.AxisStop(); + return 0; + } + snprintf (outbuf, 255, ":Q#"); if (device_fd <= 0) if (OutputOpen() != 0) return -1; WriteTTY(outbuf); diff --git a/videodev-simulation.cc b/videodev-simulation.cc index a6a40c0..b130ef1 100644 --- a/videodev-simulation.cc +++ b/videodev-simulation.cc @@ -99,6 +99,7 @@ int VideoDev_Simulation::CaptureStart() { if (inframe != NULL) Close(); if (Open() != VDEV_STATUS_OK) return VDEV_STATUS_ERROR; ConvertStart(&cdata, V4L2_PIX_FMT_RGB24); + get_cycletime(&lastframetv); return VDEV_STATUS_OK; }; @@ -121,7 +122,11 @@ int VideoDev_Simulation::CaptureStop() { #define SIMULATION_SIZE 15 int VideoDev_Simulation::Grab(VideoFrame *vf) { int posx, posy, x ,y; - double r, a; + double r, a, dsec; + + // try to match a speed of 20Hz + dsec = get_cycletime(&lastframetv); + if (dsec < 0.040) usleep (50000 - dsec*1000.0); memset (inframe, 0x0, w*h*3); simulation.GetPos(&posx, &posy); @@ -174,15 +179,35 @@ gpointer simulation_threadprocess_wrapper (gpointer data) { } Simulation::Simulation() { + int i; + posX = 900.0; posY = 500.0; running = 0; time_t t = time(NULL); srandom (t); + + // + // object movement dAngle = 2.0 * M_PI * (double)random() / (double) RAND_MAX; - dLen = 5.0 + 10.0 * (double)random() / (double) RAND_MAX; + dLen = 2.5 + 5.0 * (double)random() / (double) RAND_MAX; + printf ("%s:%d %s dAngle:%f dLen:%f\n", __FILE__, __LINE__, __FUNCTION__, dAngle, dLen); + + // + // axis movement + i = random() % 2; + + axis[i % 2].defAngle = dAngle + (10.0 * (double)random() / ((double) RAND_MAX)) - 5.0; + axis[i % 2].defLen = dLen + (1.0 * (double)random() / ((double) RAND_MAX) - 0.5); + axis[i % 2].v = 1.0; + axis[i % 2].active = 1; + i++; + axis[i % 2].defAngle = dAngle + (10.0 * (double)random() / ((double) RAND_MAX)) + 85.0; + axis[i % 2].defLen = dLen + (1.0 * (double)random() / ((double) RAND_MAX) - 0.5); + axis[i % 2].v = 1.0; + axis[i % 2].active = 1; }; Simulation::~Simulation() { @@ -218,18 +243,21 @@ void Simulation::ThreadProcess() { // // simulate rotation movement - // add random error to values - // calculate movement dx = sin(dAngle) * dLen * ms; - dy = cos(dAngle) * dLen * ms; + dy = -cos(dAngle) * dLen * ms; posX += dx; posY += dy; // // simulate motor axis movement - - + for (int i = 0; i < 2; i++) + if (axis[i].active) { + dx = sin(axis[i].defAngle) * axis[i].defLen * axis[i].v * ms; + dy = -cos(axis[i].defAngle) * axis[i].defLen * axis[i].v * ms; + posX -= dx; + posY -= dy; + } if (posX < 0) posX = 1920.0 - 1.0; if (posY < 0) posY = 1080.0 - 1.0; @@ -250,4 +278,17 @@ void Simulation::Stop() { } +void Simulation::AxisSetValue (int a, double v) { + printf ("%s:%d %s Axis:%d Value:%f\n", __FILE__, __LINE__, __FUNCTION__, a, v); + if (a < 0 || a > 1) return; + axis[a].v = v; + axis[a].active = 1; +} + + +void Simulation::AxisStop() { + printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__); + axis[0].active = 1; + axis[1].active = 1; +} diff --git a/videodev-simulation.h b/videodev-simulation.h index 0b37264..8ecab80 100644 --- a/videodev-simulation.h +++ b/videodev-simulation.h @@ -27,6 +27,14 @@ #include "videodev.h" +struct SimAxisCtl { + int active; + double v; + double defLen; + double defAngle; +}; + + class Simulation { private: float posX; @@ -34,6 +42,7 @@ private: float dAngle; float dLen; int running; + struct SimAxisCtl axis[2]; GMutex mutex; public: @@ -45,6 +54,9 @@ public: void Start(); void Stop(); + void AxisSetValue(int a, double v); + void AxisStop(); + void LockMutex() { g_mutex_lock(&mutex); }; void UnLockMutex() { g_mutex_unlock(&mutex); }; }; @@ -54,7 +66,7 @@ class VideoDev_Simulation: public VideoDev { private: uint32_t w; uint32_t h; - struct timeval starttv; + struct timeval lastframetv; unsigned char *inframe; ConvertData cdata; GThread *simulation_thread;