ArmRotationWidget.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2009, CoroWare
00003  * All rights reserved.
00004  * 
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  * 
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Stanford U. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  * 
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include "ArmRotationWidget.h"
00031 
00032 #include <math.h>
00033 #include <QtGui>
00034 #include "corobot.h"
00035 
00036 
00037 //Constant related to the drawings
00038 const int ARM_X = 120;
00039 const int ARM_Y = 120;
00040 const int ARM_CENTER_X = ARM_X / 2;
00041 const int ARM_BODY_TOP = ARM_X * 7 / 10;
00042 const int ARM_SHOULDER_X = ARM_CENTER_X - ARM_X / 7;
00043 const int SIZE_ARM = 25; //the value does not match the size of the real arm. 
00044 const int LENGTH_ROBOT = 55; //the value does not match the size of the real robot.
00045 const int WIDTH_ROBOT = 40; //the value does not match the size of the real robot.
00046 ArmRotationWidget::ArmRotationWidget(QWidget *parent)
00047     : QGraphicsView(parent), timerId(0)
00048 {
00049 
00050     QGraphicsScene *scene = new QGraphicsScene(this);
00051     scene->setItemIndexMethod(QGraphicsScene::NoIndex);
00052     scene->setSceneRect(0, 00, 210, 110);
00053 
00054     setScene(scene);
00055     setCacheMode(CacheBackground);
00056     setViewportUpdateMode(BoundingRectViewportUpdate);
00057     setRenderHint(QPainter::Antialiasing);
00058     setTransformationAnchor(AnchorUnderMouse);
00059     scale(qreal(1), qreal(1));
00060     setMinimumSize(210, 110);
00061     setWindowTitle(tr("Elastic Nodes"));
00062 
00063     joint *joint1 = new joint(this); // this is the shoulder joint, which is actually fixed in this widget
00064     joint *joint2 = new joint(this); // this this the gripper in the widget, which can be moved by the user
00065 
00066     scene->addItem(joint1);
00067     scene->addItem(joint2);
00068 
00069     joint2->setFlag(joint2->ItemIsMovable); // Make the gripper moveable
00070 
00071 
00072     joint1->setPos(this->sceneRect().right()/2 + 5, this->sceneRect().bottom()-10 - LENGTH_ROBOT * 4/5 +5);
00073     joint2->setPos(this->sceneRect().right()/2 + 5,this->sceneRect().bottom()-10 - LENGTH_ROBOT * 4/5 +5 - SIZE_ARM);
00074 
00075     end_effector = QPointF(0,0);
00076 
00077     QGraphicsLineItem *line = new QGraphicsLineItem(joint1->pos().x()-5,joint1->pos().y()-5,joint2->pos().x()-5,joint2->pos().y()-5); // draw the line between the two joints, representing the arm itself
00078     scene->addItem(line);
00079 
00080  }
00081 
00082 
00083 void ArmRotationWidget::itemMoved()
00084 //timer starts if a movement is detected
00085 {
00086     if (!timerId)
00087         timerId = startTimer(1000 / 25);
00088 }
00089 
00090 
00091 
00092 void ArmRotationWidget::moveArmLeft()
00093 //moves the arm toward the robot
00094 {
00095     this->scene()->items().at(2)->setPos(end_effector + QPointF(-5,0));
00096 }
00097 
00098 void ArmRotationWidget::moveArmRight()
00099 //moves the arm far from the robot
00100 {
00101     this->scene()->items().at(2)->setPos(end_effector + QPointF(5,0));
00102 }
00103 
00104 
00105 void ArmRotationWidget::timerEvent(QTimerEvent *event)
00106 //execute this function when the timer is up.
00107 // Use to restrict the movement of the rotation of the arm the way it should be. 
00108  {
00109      Q_UNUSED(event);
00110      double angle;
00111 
00112         // get the two joints from the scene, the shoulder and the gripper
00113      QList<joint *> joints;
00114          foreach (QGraphicsItem *item, scene()->items()) {
00115              if (joint *j = qgraphicsitem_cast<joint *>(item))
00116                  joints << j;
00117          }
00118 
00119         // if the gripper joint moved
00120          if(joints.at(1)->x()!=end_effector.x() || joints.at(1)->y()!=end_effector.y()){
00121              double t1,t2;
00122                 // get the difference of position between the shoulder and gripper
00123              double x = joints.at(0)->x() - joints.at(1)->x();
00124              double y = joints.at(0)->y() - joints.at(1)->y();
00125                 
00126                 // reset the position of the gripper to where it should be to keep the fixed arm length. The angle stay the same as what the user defined.
00127              if(y > 0)
00128              {
00129                 angle = atan(x/y) + M_PI/2;
00130                 joints.at(1)->setPos(this->sceneRect().right()/2 + 5 + cos(angle)*SIZE_ARM, this->sceneRect().bottom()-10 - LENGTH_ROBOT * 4/5 +5 - sin(angle)*SIZE_ARM);
00131                 emit armAngle_rad(angle);
00132              }
00133              else // the arm went passed the 180 angles, so we restrict the arm from going further.
00134                 joints.at(1)->setPos(end_effector);
00135 
00136                 // draw the arm line again
00137              QList<QGraphicsLineItem *> lines;
00138              foreach (QGraphicsItem *item, scene()->items()) {
00139                 if (QGraphicsLineItem *l = qgraphicsitem_cast<QGraphicsLineItem *>(item))
00140                         lines << l;
00141              }
00142 
00143              lines.at(0)->setLine(joints.at(0)->pos().x()-5,joints.at(0)->pos().y()-5,joints.at(1)->pos().x()-5,joints.at(1)->pos().y()-5); 
00144 
00145              end_effector = joints.at(1)->pos();
00146          }
00147                 
00148 
00149  }
00150 
00151 
00152  void ArmRotationWidget::drawBackground(QPainter *painter, const QRectF &rect)
00153  // draw the widget background
00154  {
00155      Q_UNUSED(rect);
00156 
00157      // Shadow
00158      QRectF sceneRect = this->sceneRect();
00159      QRectF rightShadow(sceneRect.right(), sceneRect.top() + 5, 5, sceneRect.height());
00160      QRectF bottomShadow(sceneRect.left() + 5, sceneRect.bottom(), sceneRect.width(), 5);
00161      if (rightShadow.intersects(rect) || rightShadow.contains(rect))
00162          painter->fillRect(rightShadow, Qt::darkGray);
00163      if (bottomShadow.intersects(rect) || bottomShadow.contains(rect))
00164          painter->fillRect(bottomShadow, Qt::darkGray);
00165 
00166 
00167      // Fill
00168      QLinearGradient gradient(sceneRect.topLeft(), sceneRect.bottomRight());
00169      gradient.setColorAt(0, Qt::white);
00170      gradient.setColorAt(1, Qt::lightGray);
00171      painter->fillRect(rect.intersect(sceneRect), gradient);
00172      painter->setBrush(Qt::NoBrush);
00173      painter->drawRect(sceneRect);
00174 
00175      //Robot
00176       QRectF robot((sceneRect.right() - sceneRect.left())/2 - WIDTH_ROBOT/2, sceneRect.bottom()-LENGTH_ROBOT-10, WIDTH_ROBOT, LENGTH_ROBOT);
00177       painter->fillRect(robot, Qt::gray);
00178       painter->setBrush(Qt::NoBrush);
00179       painter->drawRect(robot);
00180 
00181 
00182      // Text
00183      QRectF textRect(sceneRect.left() + 4, sceneRect.top() + 4,
00184                      sceneRect.width() - 4, sceneRect.height() - 4);
00185 
00186      QFont font = painter->font();
00187      font.setBold(true);
00188      font.setPointSize(14);
00189      painter->setFont(font);
00190      painter->setPen(Qt::lightGray);
00191      painter->setPen(Qt::black);
00192  }
00193 


corobot_teleop
Author(s):
autogenerated on Sun Oct 5 2014 23:18:16