diff options
Diffstat (limited to 'qtbotsim/robot.cpp')
| -rw-r--r-- | qtbotsim/robot.cpp | 212 |
1 files changed, 212 insertions, 0 deletions
diff --git a/qtbotsim/robot.cpp b/qtbotsim/robot.cpp new file mode 100644 index 0000000..8265a3b --- /dev/null +++ b/qtbotsim/robot.cpp @@ -0,0 +1,212 @@ +/* + * In the original BSD license, both occurrences of the phrase "COPYRIGHT + * HOLDERS AND CONTRIBUTORS" in the disclaimer read "REGENTS AND CONTRIBUTORS". + * + * Here is the license template: + * + * Copyright (c) 2011, Johan Thelin <[email protected]> + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * Neither the name of Johan Thelin nor the names of its contributors may be + * used to endorse or promote products derived from this software without + * specific prior written permission. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "robot.h" + +#include <QPainterPath> +#include <math.h> + +Robot::Robot(QObject *parent) + : QObject(parent) +{ + // All is reset in the reset method called first +} + +void Robot::step(QPixmap *pixmap) +{ + QPointF start = position(); + setPosition(position()+movementChange()); + QPointF end = position(); + setDirection(direction()+directionChange()); + + if (m_penActive) + { + QPainter p(pixmap); + p.setRenderHint(QPainter::Antialiasing); + p.setPen(Qt::red); + p.drawLine(start, end); + } + + updateSensors(*pixmap); +} + +void Robot::drawOverlay(QPainter &painter) +{ + QPainterPath path; + + path.addRect(QRectF(-QPointF(10, 20), QSizeF(20, 40))); + path.addEllipse(QPointF(0, 15), 2, 2); + + painter.setPen(Qt::black); + painter.setBrush(Qt::lightGray); + + painter.translate(position()); + painter.rotate(direction()); + + painter.drawPath(path); +} + +bool Robot::brightForward() const +{ + return m_brightForward; +} + +bool Robot::penActive() const +{ + return m_penActive; +} + +qreal Robot::leftEngineSpeed() const +{ + return m_leftEngineSpeed; +} + +qreal Robot::rightEngineSpeed() const +{ + return m_rightEngineSpeed; +} + +void Robot::reset(const QPixmap &pixmap) +{ + setPenActive(true); + setLeftEngineSpeed(0); + setRightEngineSpeed(0); + setDirection(270); + setPosition(QPointF(pixmap.width()/2, pixmap.height()/2)); + updateSensors(pixmap); +} + +void Robot::setPenActive(bool pa) +{ + if (pa != m_penActive) + { + m_penActive = pa; + emit penActiveChanged(m_penActive); + } +} + +void Robot::setLeftEngineSpeed(qreal s) +{ + s = qBound(-1.0, s, 1.0); + + if (s != m_leftEngineSpeed) + { + m_leftEngineSpeed = s; + emit leftEngineSpeedChanged(m_leftEngineSpeed); + } +} + +void Robot::setRightEngineSpeed(qreal s) +{ + s = qBound(-1.0, s, 1.0); + + if (s != m_rightEngineSpeed) + { + m_rightEngineSpeed = s; + emit rightEngineSpeedChanged(m_rightEngineSpeed); + } +} + +QPointF Robot::position() const +{ + return m_position; +} + +void Robot::setPosition(const QPointF &p) +{ + m_position = p; +} + +qreal Robot::direction() const +{ + return m_direction; +} + +void Robot::setDirection(qreal d) +{ + while (d>360.0) + d -= 360.0; + + m_direction = d; +} + +void Robot::setBrightForward(bool value) +{ + if (value != m_brightForward) + { + m_brightForward = value; + emit brightForwardChanged(m_brightForward); + } +} + +void Robot::updateSensors(const QPixmap &paper) +{ + QTransform t; + t.rotate(direction()); + QPointF sensorPos = position() + t.map(QPointF(0, 15)); + + setBrightForward(qGray(paper.toImage().pixel(sensorPos.toPoint())) > 20); +} + +QPointF Robot::movementChange() const +{ + QTransform t; + t.rotate(direction()+directionChange()); + + return t.map(QPointF(0, (leftEngineSpeed() + rightEngineSpeed())/2)); +} + +qreal Robot::directionChange() const +{ + /* + + <----+----> lwS + | . + | . + | . + | . + |a x---------------------- + |-. .... ) a + | . .... + |. .... + |. .... + <----+----> rwR + + + */ + + qreal tangens = (leftEngineSpeed()-rightEngineSpeed())/20.0; + qreal angle = atan(tangens); + + return angle*180.0/M_PI; +} |