robomon.cpp

#include <math.h>
#include "robomon.h"
#include "robomon_orte.h"
#include "robot.h"
#include <pthread.h>
#include <stdio.h>
#include <stdlib.h>
#include <path_planner.h>

#include <QCoreApplication>
#include <QEvent>
#include <QDebug>
#include <QMessageBox>
#include <sharp.h>
#include <fstream>
#include <iostream>
#include <trgen.h>

bool robomon::event(QEvent *event) 
{
        char buffer[20];
        switch(event->type()) {
                case QEvent::GraphicsSceneMousePress:
                        break;
                case EVENT_POS:
                        sprintf(buffer,"%f",pozice.x);
                        this->tbX->setText(buffer);
                        sprintf(buffer,"%f",pozice.y);
                        this->tbY->setText(buffer);
                        sprintf(buffer,"%f",pozice.phi);
                        this->tbPhi->setText(buffer);
                        if(!isnan(pozice.x) || !isnan(pozice.y) || !isnan(pozice.phi))
                                this->rob->moveRobot(pozice.x,pozice.y,pozice.phi);
                        return TRUE;
                        break;
                case EVENT_IR:
                        this->setIR(stateIR);
                        return TRUE;
                        break;
                case EVENT_SHARP:
                        this->setSharp();
                        return TRUE;
                        break;
                case EVENT_DI:
                        this->setDI(stateDI);
                        return TRUE;
                        break;
                default:
                        return FALSE;
        }
        return FALSE;
}

void robomon::setDI(stateDigIn stateDI)
{
        this->cbDI0->setChecked((stateDI.state & (1<<0)));
        this->cbDI1->setChecked((stateDI.state & (1<<1)));
        this->cbDI2->setChecked((stateDI.state & (1<<2)));
        this->cbDI3->setChecked((stateDI.state & (1<<3)));
        this->cbDI4->setChecked((stateDI.state & (1<<4)));
        this->cbDI5->setChecked((stateDI.state & (1<<5)));
        this->cbDI6->setChecked((stateDI.state & (1<<6)));
        this->cbDI7->setChecked((stateDI.state & (1<<7)));
}

void robomon::setIR(unsigned short int stateIR)
{
        this->cbIR1->setChecked((sir.front & (1<<7)));
        this->cbIR2->setChecked((sir.front & (1<<6)));
        this->cbIR3->setChecked((sir.front & (1<<5)));
        this->cbIR4->setChecked((sir.front & (1<<4)));
        this->cbIR5->setChecked((sir.front & (1<<3)));
        this->cbIR6->setChecked((sir.front & (1<<2)));
        this->cbIR7->setChecked((sir.front & (1<<1)));
        this->cbIR8->setChecked((sir.front & (1<<0)));
        this->cbIR9->setChecked((sir.back & (1<<0)));
        this->cbIR10->setChecked((sir.back & (1<<1)));
        this->cbIR11->setChecked((sir.back & (1<<2)));
        this->cbIR12->setChecked((sir.back & (1<<3)));
        this->cbIR13->setChecked((sir.back & (1<<4)));
        this->cbIR14->setChecked((sir.back & (1<<5)));
        this->cbIR15->setChecked((sir.back & (1<<6)));
        this->cbIR16->setChecked((sir.back & (1<<7)));
}

robomon::robomon(QWidget* parent, Qt::WFlags fl)
: QWidget( parent, fl ), Ui::Form()
{       
        setupUi(this);

        QObject::connect(this->btStopMotors, SIGNAL(clicked()), this, SLOT(stopMotors()));
        
        QObject::connect(this->dServo3, SIGNAL(valueChanged(int)), this, SLOT(moveServo(int)));
        QObject::connect(this->dServo4, SIGNAL(valueChanged(int)), this, SLOT(moveServo(int)));
        
        QObject::connect(this->cbFrontDoor, SIGNAL(stateChanged(int)), this, SLOT(frontDoor(int)));
        QObject::connect(this->cbInnerDoor, SIGNAL(stateChanged(int)), this, SLOT(innerDoor(int)));
        QObject::connect(this->cbBackDoor, SIGNAL(stateChanged(int)), this, SLOT(backDoor(int)));
        QObject::connect(this->cbLaunch, SIGNAL(stateChanged(int)), this, SLOT(launchFront(int)));


        QObject::connect(this->btDopravnikFront, SIGNAL(clicked()), this, SLOT(stopDopravnikFront()));

        QObject::connect(this->btDopravnikBack, SIGNAL(clicked()), this, SLOT(stopDopravnikBack()));
        QObject::connect(this->btMap, SIGNAL(clicked()), this, SLOT(showMap()));

        QObject::connect(this->cbDO0, SIGNAL(stateChanged(int)), this, SLOT(setDO(int)));
        QObject::connect(this->cbDO1, SIGNAL(stateChanged(int)), this, SLOT(setDO(int)));
        QObject::connect(this->cbDO2, SIGNAL(stateChanged(int)), this, SLOT(setDO(int)));
        QObject::connect(this->cbDO3, SIGNAL(stateChanged(int)), this, SLOT(setDO(int)));
        QObject::connect(this->cbDO4, SIGNAL(stateChanged(int)), this, SLOT(setDO(int)));
        QObject::connect(this->cbDO5, SIGNAL(stateChanged(int)), this, SLOT(setDO(int)));
        QObject::connect(this->cbDO6, SIGNAL(stateChanged(int)), this, SLOT(setDO(int)));
        QObject::connect(this->cbDO7, SIGNAL(stateChanged(int)), this, SLOT(setDO(int)));

        QObject::connect(this->slMotorLeft, SIGNAL(valueChanged(int)), this, SLOT(setMotor(int)));
        QObject::connect(this->slMotorRight, SIGNAL(valueChanged(int)), this, SLOT(setMotor(int)));
        
    QObject::connect(this->cbSimulation, SIGNAL(stateChanged(int)), this, SLOT(setSimul(int)));
        
        QObject::connect(this->slB1, SIGNAL(valueChanged(int)), this, SLOT(setSharpVal(int)));
        QObject::connect(this->slB2, SIGNAL(valueChanged(int)), this, SLOT(setSharpVal(int)));
        QObject::connect(this->slB3, SIGNAL(valueChanged(int)), this, SLOT(setSharpVal(int)));
        
        QObject::connect(this->slA1, SIGNAL(valueChanged(int)), this, SLOT(setSharpVal(int)));
        QObject::connect(this->slA2, SIGNAL(valueChanged(int)), this, SLOT(setSharpVal(int)));
        QObject::connect(this->slA3, SIGNAL(valueChanged(int)), this, SLOT(setSharpVal(int)));
        QObject::connect(this->slA4, SIGNAL(valueChanged(int)), this, SLOT(setSharpVal(int)));
        

        this->scene = new PlaygroundScene();
    this->gvPlayground->setScene(this->scene);

    QObject::connect(this->cbObstackle, SIGNAL(stateChanged(int)), this, SLOT(setSimul(int)));
    QObject::connect(this->cbObstackle, SIGNAL(stateChanged(int)), this, SLOT(obstackleSimulation(int)));
    connect(this->cbObstackle, SIGNAL(stateChanged(int)), this->scene, SLOT(showObstacle(int)));
    connect(this->scene, SIGNAL(obstacleChanged(QPointF)), this, SLOT(changeObstacle(QPointF)));

        rob = new robot(NULL, scene);
        rob->setZValue(10);
        
        scene->addItem(rob);
//      rob->moveRobot(pos.x,pos.y,pos.phi);
        
        orte_function(this);
        serva.transporterInner = TRANSPORTER_OFF;
        serva.transporterFront = TRANSPORTER_OFF;
        serva.frontDoor = FRONT_DOOR_UP;
        serva.backDoor = BACK_DOOR_UP;
        serva.innerDoor = INNER_DOOR_UP;
}


robomon::~robomon()
{
        shmdt((void*)map);

        ORTEDomainAppDestroy(d);
}

void robomon::setSimul(int value)
{
        if(value) {
                publisherCreateSensShort(this);
                publisherCreateSensLong(this);
        }
        else {
                ORTEPublicationDestroy(publisherSharpShort);
                ORTEPublicationDestroy(publisherSharpLong);
        }
}

void robomon::moveServo(int value)
{
        serva.transporterInner = this->dServo3->value();
        serva.transporterFront = this->dServo4->value();
        ORTEPublicationSend(publisherServa);
}

void robomon::frontDoor(int state)
{
        if(state) serva.frontDoor = FRONT_DOOR_UP;
        else serva.frontDoor = FRONT_DOOR_DOWN;
        ORTEPublicationSend(publisherServa);
}

void robomon::launchFront(int state)
{
        if(state) serva.release = 255;
        else serva.release = 50;
        ORTEPublicationSend(publisherServa);
}

void robomon::innerDoor(int state)
{
        if(state) serva.innerDoor = INNER_DOOR_UP;
        else serva.innerDoor = INNER_DOOR_DOWN;
        ORTEPublicationSend(publisherServa);
}

void robomon::backDoor(int state)
{
        if(state) serva.backDoor = BACK_DOOR_UP;
        else serva.backDoor = BACK_DOOR_DOWN;
        ORTEPublicationSend(publisherServa);
}

void robomon::stopDopravnikFront(void)
{
        serva.transporterInner = TRANSPORTER_OFF;
        this->dServo3->setValue(TRANSPORTER_OFF);       
        ORTEPublicationSend(publisherServa);
}

void robomon::stopDopravnikBack(void)
{
        serva.transporterFront = TRANSPORTER_OFF;
        this->dServo4->setValue(TRANSPORTER_OFF);       
        ORTEPublicationSend(publisherServa);
}

void robomon::setSharpVal(int x)
{       
        sharpyLahve.short1 = this->slA1->value();
        sharpyLahve.short2 = this->slA2->value();
        sharpyLahve.short3 = this->slA3->value();
        sharpyLahve.short4 = this->slA4->value();
        ORTEPublicationSend(publisherSharpShort);
        sharpySouper.longSharpDist1 = this->slB1->value()/1000.0;
    sharpySouper.longSharpDist2 = this->slB2->value()/1000.0;
    sharpySouper.longSharpDist3 = this->slB3->value()/1000.0;
    ORTEPublicationSend(publisherSharpLong);
}

void robomon::setDO(int x)
{       
        stateDO[0] = this->cbDO0->isChecked();
        stateDO[1] = this->cbDO1->isChecked();
        stateDO[2] = this->cbDO2->isChecked();
        stateDO[3] = this->cbDO3->isChecked();
        stateDO[4] = this->cbDO4->isChecked();
        stateDO[5] = this->cbDO5->isChecked();
        stateDO[6] = this->cbDO6->isChecked();
        stateDO[7] = this->cbDO7->isChecked();
        ORTEPublicationSend(publisherDO);
}

void robomon::setSharp()
{
        char buffer[20];
    int val;
    val = ss1.longSharpDist1*1000;
        sprintf(buffer,"%dmm",val);
        this->tbSharpLong1->setText(buffer);
    if (val > pbSharpLong1->maximum()) val = pbSharpLong1->maximum();
    this->pbSharpLong1->setValue(val);
    
    val = ss1.longSharpDist2*1000;
//      sprintf(buffer,"%dmm",s_ir2mmLong(ss1.longSharp2));
    sprintf(buffer,"%dmm",val);
        this->tbSharpLong2->setText(buffer);
    if (val > pbSharpLong2->maximum()) val = pbSharpLong2->maximum();
    this->pbSharpLong2->setValue(val);
    
    val = ss1.longSharpDist3*1000;
//      sprintf(buffer,"%dmm",s_ir2mmLong(ss1.longSharp3));
    sprintf(buffer,"%dmm",val);
        this->tbSharpLong3->setText(buffer);
    if (val > pbSharpLong3->maximum()) val = pbSharpLong3->maximum();
    this->pbSharpLong3->setValue(val);
        
    this->pbSharpShort1->setValue(s_ir2mmShort(ss2.short1));
        sprintf(buffer,"%d",s_ir2mmShort(ss2.short1));
        this->tbSharpShort1->setText(buffer);
        this->pbSharpShort2->setValue(s_ir2mmShort(ss2.short2));
        sprintf(buffer,"%d",s_ir2mmShort(ss2.short2));
        this->tbSharpShort2->setText(buffer);
        this->pbSharpShort3->setValue(s_ir2mmShort(ss2.short3));
        sprintf(buffer,"%d",s_ir2mmShort(ss2.short3));
        this->tbSharpShort3->setText(buffer);
        this->pbSharpShort4->setValue(s_ir2mmShort(ss2.short4));
        sprintf(buffer,"%d",s_ir2mmShort(ss2.short4));
        this->tbSharpShort4->setText(buffer);
}

void robomon::setMotor(int x)
{
        short int motorL;
        short int motorR;

        if(this->cbJoinMotors->isChecked()) {
                slMotorRight->setValue(slMotorLeft->value());
        }
        motorL = (short int)(MOTOR_LIMIT * (slMotorLeft->value()/100.0));
        motorR = (short int)(MOTOR_LIMIT * (slMotorRight->value()/100.0));
        
        mspeed.left = motorL;
        mspeed.right = motorR;

        ORTEPublicationSend(publisherMotor);
}

void robomon::stopMotors()
{
        slMotorLeft->setValue(0);
        slMotorRight->setValue(0);
}

void robomon::showMap()
{
        size = PG_X / MAP_WIDTH;
        
        shared_segment_size = sizeof(char) * MAP_WIDTH * MAP_HEIGHT;
        
        /* Get segment identificator in a read only mode  */
        segment_id = shmget (SHM_MAP_KEY, shared_segment_size, S_IRUSR );
        if(segment_id==-1) {
                QMessageBox::critical(this,"robomon","Unable to open shared memory segment!");
                return;
        }

        /* Attach the shared memory segment */
        map =  (_Map*)shmat (segment_id, (void*) 0, 0);

        timer = new QTimer(this);
        connect(timer, SIGNAL(timeout()), this, SLOT(paintMap()));
        timer->start(200);
        
}

void robomon::paintMap()
{
    using namespace Qt;
        static QGraphicsRectItem *rects[MAP_WIDTH][MAP_HEIGHT];
        static bool firstMap = true;
        int x,y;
        
        for(int i=0; i < MAP_WIDTH; i++) {
                for(int j=0; j<MAP_HEIGHT; j++) {
                        x = TOP_LEFT_X+i*size;
                        y = TOP_LEFT_Y+j*size;
                                
                        if(firstMap) {                  
                                rects[i][j] = this->scene->addRect(QRectF(x,y,size,size),QPen(QBrush(Qt::black),1,Qt::SolidLine,Qt::FlatCap,Qt::BevelJoin),QBrush(Qt::lightGray));
                        
                                rects[i][j]->setZValue(4);
                        }
                                
                        switch(map->cells[j][i]) {              
                                case MAP_WALL:
                                        rects[i][j]->setBrush(QBrush(Qt::yellow));
                                        break;
                                case MAP_WALL_CSPACE:
                                        rects[i][j]->setBrush(QBrush(Qt::darkYellow));
                                        break;
                                case MAP_PATH:
                                        rects[i][j]->setBrush(QBrush(Qt::darkRed));
                                        break;
                                case MAP_START:
                                        rects[i][j]->setBrush(QBrush(Qt::red));
                                        break;
                                case MAP_GOAL:
                                        rects[i][j]->setBrush(QBrush(Qt::green));
                                        break;
                                case MAP_NEW_OBSTACLE:
                                        rects[i][j]->setBrush(QBrush(Qt::blue));
                                        break;
                                case MAP_NEW_OBSTACLE_CSPACE:
                                        rects[i][j]->setBrush(QBrush(Qt::cyan));
                                        break;
                                case MAP_FREE:
                                        rects[i][j]->setBrush(QBrush(lightGray));
                                        break;
                                default: {
                    QColor c1(lightGray), c2(blue);
                    double f = map->cells[j][i]/251.0;
                    QColor c(c1.red()   + f*(c2.red()   - c1.red()),
                             c1.green() + f*(c2.green() - c1.green()),
                             c1.blue()  + f*(c2.blue()  - c1.blue()));
                    
                                        rects[i][j]->setBrush(QBrush(c));
                                        break;
                }
                        }
                        //this->gvPlayground->update();
                }
        }
        firstMap = false;
        
}



/*$SPECIALIZATION$*/


void robomon::obstackleSimulation(int state)
{
    if (state) {
        ShmapInit(0);
        obstSimTimer = new QTimer(this);
        connect(obstSimTimer, SIGNAL(timeout()), this, SLOT(simulateObstackles()));
        obstSimTimer->start(100);
        this->setMouseTracking(true);
    } else {
        if (obstSimTimer) delete obstSimTimer;
        double distance = 0.8;
        sharpySouper.longSharpDist1 = distance;
        sharpySouper.longSharpDist2 = distance;
        sharpySouper.longSharpDist3 = distance;
        ORTEPublicationSend(publisherSharpLong);

    }
}

static double distanceToObstacle(Point obstacle, double obstacleSize)
{
    Point robot(pozice.x, pozice.y);
    const double sensorRange = 1.0; /*[meters]*/

    double distance, angle;

    angle = robot.angleTo(obstacle) - pozice.phi;
    angle = fmod(angle, 2.0*M_PI);
    if (angle > +M_PI) angle -= 2.0*M_PI;
    if (angle < -M_PI) angle += 2.0*M_PI;
    angle = fabs(angle);
    distance = robot.distanceTo(obstacle)-0.11;
    if (angle < atan(obstacleSize/2.0 / (distance+0.001))) {
        // We can see the obstackle from here.
        if (angle < M_PI/2.0) {
            distance = distance/cos(angle);
        }
        if (distance > sensorRange) distance = sensorRange;
    } else {
        distance = sensorRange;
    }
    return distance;
}

static double distanceToWall(){
        double distance=1.0, min_distance=1.0;
        int i,j;
        Point wall;
#if 0
        // Simulate wall
        //TODO change to macros
        for (i=1;i<30;i=i+28){
                for(j=0;j<21;j++){
                        wall.x=i/10.0;
                        wall.y=j/10.0;
                    //printf("(%f,%f)",wall.x, wall.y);
                        distance = distanceToObstacle(wall, 0.01);
                        if (distance<min_distance) min_distance = distance;
                }
        }
        for(j=1;j<21;j=j+19){
                for (i=1;i<30;i++){
                        wall.x=i/10.0;
                        wall.y=j/10.0;
                        printf("(%f,%f)\n",wall.x, wall.y);
                        distance = distanceToObstacle(wall, 0.01);
                        if (distance<min_distance) min_distance = distance;
                }
        }
#endif
#if 1
        // Simulate obstacles
        for(j=1;j<21;j++){
                for (i=1;i<30;i++){
                        MapCell thiscell = ShmapGetCellValue(i,j);
                        wall.x=-1.0;
                        //printf("Cell : %02X\n", thiscell);
                        if( thiscell== (MapCell) MAP_WALL ) {
                                // WALL
                                wall.x=ShmapCell2Point_X(i);
                                wall.y=ShmapCell2Point_Y(j);
                        } else if((j==1) || (j==20)){
                                // Vertical borders
                                wall.x=ShmapCell2Point_X(i);
                                wall.y=ShmapCell2Point_Y(j);
                        } else if ((i==1) || (i==29)) {
                                // Horizontal Borders
                                wall.x=ShmapCell2Point_X(i);
                                wall.y=ShmapCell2Point_Y(j);
                        }
                        
                        if (wall.x >= 0.0) {
                                //printf("[%d,%d],(%f,%f)\n",i,j,wall.x, wall.y);
                                distance = distanceToObstacle(wall, 0.1);
                                if (distance<min_distance) min_distance = distance;
                        }
                }
        }
        //ShmapDt();
        //ShmapFree();
#endif
        return min_distance;
}


void robomon::simulateObstackles()
{
    double distance, wall_distance;

    
    wall_distance = distanceToWall();
    distance = distanceToObstacle(simulatedObstacle, 0.5/*meters*/);
    if (wall_distance<distance) distance = wall_distance;
    
    sharpySouper.longSharpDist1 = distance;
    sharpySouper.longSharpDist2 = distance;
    sharpySouper.longSharpDist3 = distance;
    ORTEPublicationSend(publisherSharpLong);

    
}


void robomon::changeObstacle(QPointF pos)
{
    simulatedObstacle.x = pos.x();
    simulatedObstacle.y = pos.y();
    simulateObstackles();
}

---