File content as of revision 5:355240d7126b:

#include "robot_logic.h"

// Some defines
#define MAX_SPEED 100
#define MAX_SENSOR 100
#define MAX_REAL_SPEED 1.0
#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_turn_speed = 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;

/*
 * Drive the robot forward or backward.
 * If the robot was turning it will stop turning and drive in a straight line.
 *
 * @speed The speed percentage with which to drive forward or backward.
 * Can range from -100 (full throttle backward) to +100 (full throttle forward).
 */
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);
    }
}

/*
 * Turn the robot left or right while driving.
 *
 * @turnspeed The percentage with which to turn the robot.
 * Can range from -100 (full throttle left) to +100 (full throttle right).
 */
void turn(int turnspeed) {
    if (turnspeed > MAX_SPEED || turnspeed < -MAX_SPEED) {
        error("Speed out of range");
        return;
    }

    internal_turn_speed = turnspeed;

    if (turnspeed == 0) {           // Drive straight
        drive(internal_speed);
    } else {
        // int left = 0;
        // int right = 0;
        // if (internal_speed > 0) {     // Right | Left forward
        //     left = internal_speed + turnspeed / 2;
        //     right = internal_speed - turnspeed / 2;

        //     if (left > MAX_SPEED) {
        //         left = MAX_SPEED;
        //         right = MAX_SPEED - turnspeed;
        //     } else if (right > MAX_SPEED) {
        //         right = MAX_SPEED;
        //         left = MAX_SPEED + turnspeed;
        //     }
        // }
        // else if (internal_speed < 0) {     //  Right | Left backward
        //     left = internal_speed - turnspeed / 2;
        //     right = internal_speed + turnspeed / 2;

        //     if (left < -MAX_SPEED) {
        //         left = -MAX_SPEED;
        //         right = -MAX_SPEED - turnspeed;
        //     } else if (right < -MAX_SPEED) {
        //         right = -MAX_SPEED;
        //         left = -MAX_SPEED + turnspeed;
        //     }
        // }

        // Compute new speeds
        int right = internal_speed+turnspeed;
        int left  = internal_speed-turnspeed;
        
        // limit checks
        if (right < MIN)
            right = MIN;
        else if (right > MAX)
            right = MAX;
            
        if (left < MIN)
            left = MIN;
        else if (left > MAX)
            left = MAX;

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

/*
 * Stop the robot.
 */
void stop(void) {
    m3pi.stop();
}

/*
 * Calibrate the line follow sensors.
 * Take note that the pololu should be placed over the line
 * before this function is called and that it will rotate to
 * both sides.
 */
void sensor_calibrate(void) {
    m3pi.sensor_auto_calibrate();
}

/*
 * Read the value from the line sensor. The returned value indicates the
 * position of the line. The value ranges from -100 to +100 where -100 is
 * fully left, +100 is fully right and 0 means the line is detected in the middle.
 *
 * @return The position of the line with a range of -100 to +100.
 */
int line_sensor(void) {
    // Get position of line [-1.0, +1.0]
    float pos = m3pi.line_position();
    return ((int)(pos * MAX_SENSOR));
}

/*
 * Initialize the PID drive control with
 * the P, I and T factors.
 *
 * @p The P factor
 * @i The I factor
 * @d The D factor
 */
void pid_init(int p, int i, int d) {
    internal_p = p;
    internal_i = i;
    internal_d = d;
}

/*
 * Determine PID turnspeed with which the pololu should
 * turn to follow the line at the given position.
 *
 * @line_position The position of the line in a range of [-100, +100]
 *
 * @returns The turnspeed in a range of [-100, +100]
 */
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 drive speed and sensor data
void show_stats(void) {
    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("FOR 100%%");

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

/*
 * Shows the name of the robot on the display.
 * 
 * @name C character string (null-terminated) with the name of the robot (max 8 chars)
 */
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);
}


/*
 * Wait for an approximate number of milliseconds.
 *
 * @milliseconds The number of milliseconds to wait.
 */
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);
}