File content as of revision 11:dbec260681a7:

#include "robot_logic.h"

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

#define MIN -MAX_SPEED

// 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) {
        error("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("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;
        
    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);
}