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:
pcordemans
Date:
2015-02-25
Revision:
6:0dc4e4225881
Parent:
5:355240d7126b
Child:
7:a72215b1910b

File content as of revision 6:0dc4e4225881:

#include "robot_logic.h"

// Some defines
#define MAX_SPEED 100
#define MAX_SENSOR 100
#define MAX_REAL_SPEED 0.3
#define MAX 0.3
#define MIN 0

// Static scope variables
static m3pi m3pi;

// 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) {
        printf("[ERROR] Drive speed out of range");
        return;
    }

    internal_speed = speed;

    if (speed == 0) {
        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("Speed 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;
    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 right;
    float left;

    float derivative, proportional, integral = 0;
    float power;
    float speed = internal_speed;

    proportional = line_position;

    // 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)) ;

    return power;
}

/**
  *Show speed, turn and sensor data
  */
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);
}