File content as of revision 2:6fa291470764:

#include "Cansat2016.h"

//************************************
//
// Constructor
//
//************************************
Cansat2016::Cansat2016(VNH5019 motor):_motor(motor)
{

    distance = 500.0;

    action = 's';
    speed = 64;
}

// distance of robot and target
void Cansat2016::calc_distance(void)
{
    sub_x = (target.x*10000.0 - robot.x*10000.0) * 11.11350;
    sub_y = (target.y*10000.0 - robot.y*10000.0) * 9.11910;
    distance = sqrt(sub_x*sub_x + sub_y*sub_y);

    if(distance > 5.0) speed = 128;
    else speed = 96;
}

// direction of robot
void Cansat2016::robot_compass(DigitalIn *sensor)
{
    int c = 0;

    // convert from base 2 to base 10
    if(sensor[0]) c += 1;
    if(sensor[1]) c += 2;
    if(sensor[2]) c += 4;
    if(sensor[3]) c += 8;

    // decide number from data sheet
    switch(c) {
        case 0:
            robot.direction = 8;    // N
            break;
        case 1:
            robot.direction = 11;   // NNE
            break;
        case 2:
            robot.direction = 3;    // NE
            break;
        case 3:
            robot.direction = 0;    // ENE
            break;
        case 4:
            robot.direction = 7;    // E
            break;
        case 5:
            robot.direction = 12;   // ESE
            break;
        case 6:
            robot.direction = 4;    // SE
            break;
        case 7:
            robot.direction = 15;   // SSE
            break;
        case 8:
            robot.direction = 9;    // S
            break;
        case 9:
            robot.direction = 10;   // SSW
            break;
        case 10:
            robot.direction = 2;    // SW
            break;
        case 11:
            robot.direction = 1;    // WSW
            break;
        case 12:
            robot.direction = 6;    // W
            break;
        case 13:
            robot.direction = 13;   // WNW
            break;
        case 14:
            robot.direction = 5;    // NW
            break;
        case 15:
            robot.direction = 14;   // N
            break;
    }
}

// direction of target from robot
void Cansat2016::target_compass(void)
{
    double angle=0.0;
    if(sub_x == 0.0) {
        if(sub_y >= 0.0) angle = 0.0;
        else  angle = M_PI;
    } else if(sub_y == 0.0) {
        if(sub_x >= 0.0) angle = M_PI / 2.0;
        else angle = 3.0 * M_PI / 2.0;
    }
    if(sub_x > 0.0)angle = atan2(sub_x,sub_y);
    else angle = atan2(sub_x,sub_y) + 2.0 * M_PI;

    angle = angle / M_PI * 180.0;

    // decide number like robot compass
    if((348.75 <= angle && angle < 360.0)|| (0.0 <= angle && angle < 11.25)) target.direction = 0;  // N
    else if(11.25 <= angle && angle < 33.75) target.direction = 1;      // NNE
    else if(33.75 <= angle && angle < 56.25) target.direction = 2;      // NE
    else if(56.25 <= angle && angle < 78.75) target.direction = 3;      // ENE
    else if(78.75 <= angle && angle < 101.25) target.direction = 4;     // E
    else if(101.25 <= angle && angle < 123.75) target.direction = 5;    // ESE
    else if(123.75 <= angle && angle < 146.25) target.direction = 6;    // SE
    else if(146.25 <= angle && angle < 168.75) target.direction = 7;    // SSE
    else if(168.75 <= angle && angle < 191.25) target.direction = 8;    // S
    else if(191.25 <= angle && angle < 213.75) target.direction = 9;    // SSW
    else if(213.75 <= angle && angle < 236.25) target.direction = 10;   // SW
    else if(236.25 <= angle && angle < 258.75) target.direction = 11;   // WSW
    else if(258.75 <= angle && angle < 281.25) target.direction = 12;   // W
    else if(281.25 <= angle && angle < 303.75) target.direction = 13;   // WNW
    else if(303.75 <= angle && angle < 326.25) target.direction = 14;   // NW
    else if(326.25 <= angle && angle < 348.75) target.direction = 15;   // NNW
}

// f->forward, r->right turn, l->left turn, b->back, s->stop
void Cansat2016::robot_action(void)
{
    int n  = 0;
    if(robot.direction <= target.direction) n = target.direction - robot.direction;
    else n = target.direction - robot.direction + 16;

    // direction of targeet and robot are same
    if(n == 0) {
        action = 'f';
        // direction of target is right from direction of robot
    } else if(0 < n && n <= 8) {
        action = 'r';
        // direction of target is left from direction of robot
    } else if(8 < n && n < 16) {
        action = 'l';
    }
}

// f->forward, r->right turn, l->left turn, b->back, s->stop
void Cansat2016::motor_control(void)
{
    // forward
    if(action == 'f') {
        _motor.changeSpeed(1, speed, 1, speed);
    }

    // right turn
    if(action == 'r') {
        _motor.changeSpeed(1, speed, 1, 64);
    }

    // light turn
    if(action == 'l') {
        _motor.changeSpeed(1, 64, 1, speed);
    }

    // back
    if(action == 'b') {
        _motor.changeSpeed(2, 96, 2, 96);
    }

    // stop
    if(action == 's') {
        _motor.changeSpeed(0, speed, 0, speed);
    }
}


//*************************************
//
// Set
//
//*************************************
void Cansat2016::set_robotGPS(double x, double y)
{
    robot.x = x;
    robot.y = y;
}

void Cansat2016::set_targetGPS(double x, double y)
{
    target.x = x;
    target.y = y;
}

void Cansat2016::set_action(char a)
{
    action = a;
}

void Cansat2016::set_speed(int value)
{
    speed = value;
}

//****************************************
//
// Get
//
//*****************************************
double Cansat2016::get_x(void)
{
    return robot.x;
}
double Cansat2016::get_y(void)
{
    return robot.y;
}
int Cansat2016::get_compass(char c)
{
    if(c == 'r') return robot.direction;
    else if(c == 't') return target.direction;
    else return NULL;
}

double Cansat2016::get_distance(void)
{
    return distance;
}

char Cansat2016::get_action(void)
{
    return action;
}