src/robomon/Robot.cpp

   1 /*
   2  * Robot.cpp                    07/10/31
   3  *
   4  * Draw and move a robot on the playground.
   5  *
   6  * Copyright: (c) 2007 CTU Dragons
   7  *            CTU FEE - Department of Control Engineering
   8  * Authors: Martin Zidek, Michal Sojka, Tran Duy Khanh
   9  * License: GNU GPL v.2
  10  */
  11
  12 #include <math.h>
  13 #include <QPen>
  14
  15 #include "PlaygroundScene.h"
  16 #include "Robot.h"
  17 #include <robodim.h>
  18 #include <actuators.h>
  19 #include <iostream>
  20
  21 Robot::Robot(const QString &text, const QPen &pen, const QBrush &brush) :
  22         QObject(), QGraphicsItem()
  23 {
  24         this->text = text;
  25         this->pen = pen;
  26         this->brush = brush;
  27         setVisible(false);
  28         setJaws(JAW_LEFT_CLOSE);
  29         moveRobot(ROBOT_START_X_M, ROBOT_START_Y_M, 0);
  30 }
  31
  32 Robot::~Robot()
  33 {
  34 }
  35
  36 QRectF Robot::boundingRect() const
  37 {
  38         return QRectF(0, 0,
  39                       ROBOT_WIDTH_MM, ROBOT_AXIS_TO_BACK_MM + ROBOT_AXIS_TO_FRONT_MM);
  40 }
  41
  42 void Robot::paint(QPainter *painter,
  43                 const QStyleOptionGraphicsItem *option, QWidget *widget)
  44 {
  45         Q_UNUSED(option);
  46         Q_UNUSED(widget);
  47         Q_UNUSED(painter);
  48
  49         painter->setPen(pen);
  50         painter->setBrush(brush);
  51         painter->drawRect(QRectF(0, 0, ROBOT_WIDTH_MM,
  52                           ROBOT_AXIS_TO_BACK_MM + ROBOT_AXIS_TO_FRONT_MM));
  53         const float xa = ROBOT_WIDTH_MM/2.0;
  54         const float ya = ROBOT_AXIS_TO_BACK_MM;
  55         const float yb = ROBOT_AXIS_TO_BACK_MM + ROBOT_AXIS_TO_FRONT_MM*0.8;
  56         const float yc = ROBOT_AXIS_TO_BACK_MM + ROBOT_AXIS_TO_FRONT_MM*0.4;
  57         const float xd = ROBOT_WIDTH_MM/4.0;
  58         QLineF arrow[] = {
  59                 QLineF(xa, ya, xa, yb),
  60                 QLineF(xa, yb, xa-xd, yc),
  61                 QLineF(xa, yb, xa+xd, yc)};
  62         painter->drawLines(arrow, 3);
  63
  64         QLineF jawsLines[2];    /* field witch jaws lines */
  65         const int offset = 40;  /* offset of point of rotation from robot sides [mm] */
  66         const int hold = 40;    /* offset from open position [mm] */
  67         const int close = 110;  /* offset from open positon [mm] */
  68         const double y_hold = sqrt(pow(ROBOT_JAWS_LENGHT_MM, 2) - pow(hold, 2));
  69         const double y_close = sqrt(pow(ROBOT_JAWS_LENGHT_MM, 2) - pow(close, 2));
  70
  71         switch (jawsPosition) {
  72                 case JAW_LEFT_OPEN:
  73                         jawsLines[0] = QLineF(offset, ROBOT_HEIGHT_MM, offset, ROBOT_HEIGHT_MM + ROBOT_JAWS_LENGHT_MM);
  74                         jawsLines[1] = QLineF(ROBOT_WIDTH_MM - offset , ROBOT_HEIGHT_MM, ROBOT_WIDTH_MM - offset, ROBOT_HEIGHT_MM + ROBOT_JAWS_LENGHT_MM);
  75                         break;
  76
  77                 case JAW_LEFT_CATCH:
  78                         jawsLines[0] = QLineF(offset, ROBOT_HEIGHT_MM, offset + hold, ROBOT_HEIGHT_MM + ROBOT_JAWS_LENGHT_MM);
  79                         jawsLines[1] = QLineF(ROBOT_WIDTH_MM - offset , ROBOT_HEIGHT_MM, ROBOT_WIDTH_MM - offset - hold, ROBOT_HEIGHT_MM + y_hold);
  80                         break;
  81
  82                 case JAW_LEFT_CLOSE:
  83                         jawsLines[0] = QLineF(offset, ROBOT_HEIGHT_MM, offset + close, ROBOT_HEIGHT_MM + y_close);
  84                         jawsLines[1] = QLineF(ROBOT_WIDTH_MM - offset , ROBOT_HEIGHT_MM, ROBOT_WIDTH_MM - offset - close, ROBOT_HEIGHT_MM + y_close);
  85                         break;
  86         }
  87
  88         QPen penThick(Qt::black, 20, Qt::SolidLine);    /* new pen with 20px thickness for jaws drawing */
  89         painter->setPen(penThick);
  90         painter->drawLines(jawsLines, 2);       /* draw jaws */
  91         painter->setPen(pen);   /* set pen to previous slim pen */
  92
  93         painter->setFont(QFont("Arial", 40, 0, 0));
  94         QTransform mirror;
  95         mirror.scale(1,-1);
  96         painter->setTransform(mirror, true);
  97         painter->drawText(QRect(0, 0, ROBOT_WIDTH_MM, -ROBOT_AXIS_TO_BACK_MM),
  98                           Qt::AlignCenter, text);
  99 }
 100
 101 void Robot::moveRobot(double x, double y, double phi)
 102 {
 103         QPointF pos(x, y);
 104
 105         pos = PlaygroundScene::world2scene(pos);
 106
 107         setPos(pos);
 108         setTransform(QTransform().rotateRadians(phi-M_PI/2.0).translate(-ROBOT_WIDTH_MM/2.0, -ROBOT_AXIS_TO_BACK_MM));
 109 }
 110
 111 void Robot::mySetVisible(bool show)
 112 {
 113         setVisible(show);
 114 }
 115
 116 void Robot::setJaws(int value)
 117 {
 118         QRectF r = boundingRect();
 119         //r.setBottom(0);
 120         jawsPosition = value;
 121         update(r);
 122 }