Nico De Witte / racing_robots

Racing Robots Session

Dependencies:   MbedJSONValue m3pi

This is the library for the Racing Robots session. It supports the M3PI robot of Polulu.

It is based on the "Arduino" principle of the init and loop function.

Just add a main.cpp file which contains:

Racing Robots main file

#include "robot_logic.h"

void init()
{
   //put your initialization logic here
}

void loop()
{
    //put your robot control logic here    
}

Features include:

  1. Controlling the LEDS
  2. Move forward and backward
  3. Turn
  4. Read the sensor values
  5. Use a PID controller

robot_logic.cpp

Committer:
sillevl
Date:
2015-06-01
Revision:
9:0385d1bfc38b
Parent:
8:597ce8a7d34b

File content as of revision 9:0385d1bfc38b:

#include "robot_logic.h"
#include "xbee.h"

// Some defines
#define MAX_SPEED 100
#define MAX_SENSOR 100
#define MAX_REAL_SPEED 0.75

#define MIN 0

// Static scope variables
static m3pi m3pi;

#ifdef XBEE
Xbee xbee(p28, p27);
#endif

// Static scope variables for keeping internal state
static int internal_speed = 0;          // [-100, +100]
static int internal_turnspeed = 0;     // [-100, +100]
static int internal_p = 0;
static int internal_i = 0;
static int internal_d = 0;
static int internal_previous_pos_of_line = 0;
static int internal_led_state = 0;

void drive(int speed)
{
    if (speed > MAX_SPEED || speed < -MAX_SPEED) {
        error("Speed out of range");
        return;
    }

    internal_speed = speed;

    if (speed == 0 || !canDrive()) {
        m3pi.stop();
    } else if (speed > 0) {
        m3pi.forward(MAX_REAL_SPEED*speed/MAX_SPEED);
    } else if (speed < 0) {
        m3pi.backward(MAX_REAL_SPEED*(-speed)/MAX_SPEED);
    }
}

void turn(int turnspeed)
{
    if (turnspeed > MAX_SPEED || turnspeed < -MAX_SPEED) {
        error("Turn out of range");
        return;
    }
    internal_turnspeed = turnspeed;

    float left = internal_speed;
    float right = internal_speed;

    left -= turnspeed;
    right += turnspeed;

    if (right < MIN)
        right = MIN;
    else if (right > MAX_SPEED)
        right = MAX_SPEED;

    if (left < MIN)
        left = MIN;
    else if (left > MAX_SPEED)
        left = MAX_SPEED;

    if(!canDrive()){
        left = right = 0;
    }

    m3pi.left_motor(MAX_REAL_SPEED*left/MAX_SPEED);
    m3pi.right_motor(MAX_REAL_SPEED*right/MAX_SPEED);
}

void stop(void)
{
    m3pi.stop();
}
void sensor_calibrate(void)
{
    m3pi.sensor_auto_calibrate();
}

int line_sensor(void)
{
    // Get position of line [-1.0, +1.0]
    float pos = m3pi.line_position();
    return ((int)(pos * MAX_SENSOR));
}

void pid_init(int p, int i, int d)
{
    internal_p = p;
    internal_i = i;
    internal_d = d;
}

int pid_turn(int line_position)
{
    float derivative, proportional, integral = 0;
    float power;

    proportional = line_position / 100.0;

    // Compute the derivative
    derivative = line_position - internal_previous_pos_of_line;

    // Compute the integral
    integral += proportional;

    // Remember the last position.
    internal_previous_pos_of_line = line_position;

    // Compute the power
    power = (proportional * (internal_p) ) + (integral*(internal_i)) + (derivative*(internal_d)) ;

    power = power * MAX_SPEED;
    if (power < -MAX_SPEED)
        power = -MAX_SPEED;
    else if (power > MAX_SPEED)
        power = MAX_SPEED;
    return power ;
}

void show_stats(void)
{
    m3pi.cls();          // Clear display

    // Display speed
    m3pi.locate(0, 0);
    m3pi.printf("S%d", internal_speed);

    // Display turn
    m3pi.locate(4,0);
    m3pi.printf("T%d", internal_turnspeed);

    // Display line
    m3pi.locate(0, 1);
    int line = line_sensor();
    m3pi.printf("POS %d", line);
}

void show_name(char * name)
{
    m3pi.cls();          // Clear display

    // Display speed
    m3pi.locate(0, 0);
    // x   The horizontal position, from 0 to 7
    // y   The vertical position, from 0 to 1
    m3pi.printf("%s", name);
}

void await(int milliseconds)
{
    wait_ms(milliseconds);
}

void led(LedIndex i, LedState state)
{
    if(state == LED_ON) {
        internal_led_state |= (1 << i);
    } else if(state == LED_OFF) {
        internal_led_state &= ~(1 << i);
    } else if(state == LED_TOGGLE) {
        internal_led_state ^= (1 << i);
    } else {
        error("Illegal LED state");
    }
    m3pi.leds(internal_led_state);
}

int canDrive()
{
#ifdef XBEE
    return !xbee.stopped();
#endif
    return true;  
}


void setCode(int code)
{
#ifdef XBEE
    xbee.setCode(code);    
#endif
}