From 305b29d71d52024f01fcdad770be059c0a0f5f6f Mon Sep 17 00:00:00 2001 From: Michal Vokac Date: Tue, 17 May 2011 17:21:31 +0200 Subject: [PATCH] robomon: Added jaws actuator visualization. Strategy writing and testing will be better with this visualization. --- src/robomon/RobomonAtlantis.cpp | 13 +++++---- src/robomon/Robot.cpp | 47 +++++++++++++++++++++++---------- src/robomon/Robot.h | 4 +-- src/robomon/robomon_orte.h | 2 +- 4 files changed, 44 insertions(+), 22 deletions(-) diff --git a/src/robomon/RobomonAtlantis.cpp b/src/robomon/RobomonAtlantis.cpp index 7351c3b6..f2e9a5ff 100644 --- a/src/robomon/RobomonAtlantis.cpp +++ b/src/robomon/RobomonAtlantis.cpp @@ -573,9 +573,12 @@ bool RobomonAtlantis::event(QEvent *event) case QEVENT(QEV_HOKUYO_SCAN): hokuyoScan->newScan(&orte.hokuyo_scan); break; -// case QEVENT(QEV_VIDLE_CMD): -// robotEstPosBest->setVidle(orte.vidle_cmd.req_pos); -// break; + case QEVENT(QEV_JAWS_CMD): + robotEstPosBest->setJaws(orte.jaws_cmd.req_pos.left); + robotRefPos->setJaws(orte.jaws_cmd.req_pos.left); + robotEstPosIndepOdo->setJaws(orte.jaws_cmd.req_pos.left); + robotEstPosOdo->setJaws(orte.jaws_cmd.req_pos.left); + break; case QEVENT(QEV_REFERENCE_POSITION): emit actualPositionReceivedSignal(); break; @@ -784,8 +787,8 @@ void RobomonAtlantis::createOrte() generic_rcv_cb, new OrteCallbackInfo(this, QEV_ESTIMATED_POSITION_BEST)); robottype_subscriber_hokuyo_scan_create(&orte, generic_rcv_cb, new OrteCallbackInfo(this, QEV_HOKUYO_SCAN)); - //robottype_subscriber_vidle_cmd_create(&orte, - // generic_rcv_cb, new OrteCallbackInfo(this, QEV_VIDLE_CMD)); + robottype_subscriber_jaws_cmd_create(&orte, + generic_rcv_cb, new OrteCallbackInfo(this, QEV_JAWS_CMD)); robottype_subscriber_fsm_main_create(&orte, rcv_fsm_main_cb, this); robottype_subscriber_fsm_motion_create(&orte, diff --git a/src/robomon/Robot.cpp b/src/robomon/Robot.cpp index bcc4d436..c2c273c9 100644 --- a/src/robomon/Robot.cpp +++ b/src/robomon/Robot.cpp @@ -25,8 +25,8 @@ Robot::Robot(const QString &text, const QPen &pen, const QBrush &brush) : this->pen = pen; this->brush = brush; setVisible(false); - setVidle(0); - moveRobot(1, 1, 0); + setJaws(JAW_LEFT_CLOSE); + moveRobot(ROBOT_START_X_M, ROBOT_START_Y_M, 0); } Robot::~Robot() @@ -60,16 +60,35 @@ void Robot::paint(QPainter *painter, QLineF(xa, yb, xa-xd, yc), QLineF(xa, yb, xa+xd, yc)}; painter->drawLines(arrow, 3); -// if (vidle != 0) { -// const float vidle_angle = (float)(vidle - VIDLE_DOWN)/(VIDLE_UP-VIDLE_DOWN)*M_PI/2.0; -// const float y = -ROBOT_VIDLE_LENGTH_M*1000*cos(vidle_angle); -// QLineF vidle[] = { -// QLineF(0*ROBOT_WIDTH_MM/3, 0, 0*ROBOT_WIDTH_MM/3, y), -// QLineF(1*ROBOT_WIDTH_MM/3, 0, 1*ROBOT_WIDTH_MM/3, y), -// QLineF(2*ROBOT_WIDTH_MM/3, 0, 2*ROBOT_WIDTH_MM/3, y), -// QLineF(3*ROBOT_WIDTH_MM/3, 0, 3*ROBOT_WIDTH_MM/3, y)}; -// painter->drawLines(vidle, 4); -// } + + QLineF jawsLines[2]; /* field witch jaws lines */ + const int offset = 40; /* offset of point of rotation from robot sides [mm] */ + const int hold = 40; /* offset from open position [mm] */ + const int close = 110; /* offset from open positon [mm] */ + const double y_hold = sqrt(pow(ROBOT_JAWS_LENGHT_MM, 2) - pow(hold, 2)); + const double y_close = sqrt(pow(ROBOT_JAWS_LENGHT_MM, 2) - pow(close, 2)); + + switch (jawsPosition) { + case JAW_LEFT_OPEN: + jawsLines[0] = QLineF(offset, ROBOT_HEIGHT_MM, offset, ROBOT_HEIGHT_MM + ROBOT_JAWS_LENGHT_MM); + jawsLines[1] = QLineF(ROBOT_WIDTH_MM - offset , ROBOT_HEIGHT_MM, ROBOT_WIDTH_MM - offset, ROBOT_HEIGHT_MM + ROBOT_JAWS_LENGHT_MM); + break; + + case JAW_LEFT_CATCH: + jawsLines[0] = QLineF(offset, ROBOT_HEIGHT_MM, offset + hold, ROBOT_HEIGHT_MM + ROBOT_JAWS_LENGHT_MM); + jawsLines[1] = QLineF(ROBOT_WIDTH_MM - offset , ROBOT_HEIGHT_MM, ROBOT_WIDTH_MM - offset - hold, ROBOT_HEIGHT_MM + y_hold); + break; + + case JAW_LEFT_CLOSE: + jawsLines[0] = QLineF(offset, ROBOT_HEIGHT_MM, offset + close, ROBOT_HEIGHT_MM + y_close); + jawsLines[1] = QLineF(ROBOT_WIDTH_MM - offset , ROBOT_HEIGHT_MM, ROBOT_WIDTH_MM - offset - close, ROBOT_HEIGHT_MM + y_close); + break; + } + + QPen penThick(Qt::black, 20, Qt::SolidLine); /* new pen with 20px thickness for jaws drawing */ + painter->setPen(penThick); + painter->drawLines(jawsLines, 2); /* draw jaws */ + painter->setPen(pen); /* set pen to previous slim pen */ painter->setFont(QFont("Arial", 40, 0, 0)); QTransform mirror; @@ -94,10 +113,10 @@ void Robot::mySetVisible(bool show) setVisible(show); } -void Robot::setVidle(int value) +void Robot::setJaws(int value) { QRectF r = boundingRect(); //r.setBottom(0); - vidle = value; + jawsPosition = value; update(r); } diff --git a/src/robomon/Robot.h b/src/robomon/Robot.h index 9d894139..3e68a8ac 100644 --- a/src/robomon/Robot.h +++ b/src/robomon/Robot.h @@ -29,9 +29,9 @@ public: void moveRobot(double x, double y, double angle); public slots: void mySetVisible(bool show); - void setVidle(int value); + void setJaws(int value); private: - int vidle; + int jawsPosition; QString text; QPen pen; QBrush brush; diff --git a/src/robomon/robomon_orte.h b/src/robomon/robomon_orte.h index f1c9d851..02e47eff 100644 --- a/src/robomon/robomon_orte.h +++ b/src/robomon/robomon_orte.h @@ -35,7 +35,7 @@ enum robomon_qev { QEV_FSM_ACT, QEV_FSM_MOTION, QEV_HOKUYO_SCAN, - QEV_VIDLE_CMD, + QEV_JAWS_CMD, }; class OrteCallbackInfo { -- 2.39.2