movehelper.cc

Go to the documentation of this file.

#include "trgen.h"
#include "robot.h"
#include "movehelper.h"
#include "fsmmove.h"
#include <path_simplifier.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/stat.h>
#include <math.h>
#include <map.h>

struct TrajectoryConstraints trajectoryConstraintsDefault = {
        maxv: 0.3,              // m/s
        maxomega: 1.5,          // rad/s
        maxangacc: 1,           // rad/s^2
        maxacc: 0.3,            // m/s^2
        maxcenacc: 1,           // m/s^2
        maxe: (double)ROBOT_WIDTH_MM/2.0/1000.0 // m
};


static void robot_trajectory_new_ex(struct TrajectoryConstraints *tc, bool backward)
{
        Trajectory *ot, *nt;
        
        if (!tc) tc = &trajectoryConstraintsDefault;

        nt = new Trajectory(*tc, backward);
        ROBOT_LOCK(new_trajectory);
        ot = (Trajectory*)robot.new_trajectory;
        robot.new_trajectory = nt;

        ROBOT_UNLOCK(new_trajectory);
        if (ot) delete(ot);
}


void robot_trajectory_new(struct TrajectoryConstraints *tc) {
        robot_trajectory_new_ex(tc, false);
}
void robot_trajectory_new_backward(struct TrajectoryConstraints *tc) {
        robot_trajectory_new_ex(tc, true);
}

void robot_trajectory_add_point(double x_m, double y_m)
{
        Trajectory *w;
        
        ROBOT_LOCK(new_trajectory);
        w = (Trajectory*)robot.new_trajectory;
        if (w) {
                w->addPoint(x_m, y_m);
        }
        ROBOT_UNLOCK(new_trajectory);
}

double target_x, target_y;

void robot_trajectory_add_final_point(double x_m, double y_m, double heading_deg)
{
        target_x = x_m;
        target_y = y_m;
        robot_trajectory_add_point(target_x, target_y);
        
        Trajectory *w;
        ROBOT_LOCK(new_trajectory);
        w = (Trajectory*)robot.new_trajectory;
        if (w) {
                w->finalHeading = heading_deg/180.0*M_PI;
        }
        ROBOT_UNLOCK(new_trajectory);

        FSM_SIGNAL(MOTION, EV_NEW_TRAJECTORY);
}


void robot_trajectory_stop()
{
        delete_actual_trajectory();
}


void robot_send_speed(double left, double right) {
        double mul;
        unsigned l, r;

        mul = 1000.0 / ROBOT_WHEEL_RADIUS_MM; // to angular speed
        mul *= 28.0/(2.0*M_PI)/*gear*/*(1<<8);  // to pxmc speed

        // I hope it is not neccessary to lock here
        l = (int)(left * mul);
        r = (int)(right * mul);
        robot.orte_speed.left = l;//(l&0xff) << 8 | (l&0xff00) >> 8;
        robot.orte_speed.right = r;//(r&0xff) << 8 | (r&0xff00) >> 8;
//      DBG("speed l=%8.3g (%6d) r=%8.3g (%6d)", 
//          left, l, // robot.orte_speed.left, 
//          right, r //robot.orte_speed.right
//          );
        ORTEPublicationSend(robot.publisherMotor);
}

void robot_goto(double x, double y /*, double phi*/)
{
        ROBOT_LOCK(goal);
        robot.goal.x = x;
        robot.goal.y = y;
        ROBOT_UNLOCK(goal);
        FSM_SIGNAL(MOTION, EV_NEW_GOAL);
}

---