File content as of revision 6:732aa91eb555:

#include "robot.h"

Timer mbed_uptime;
int timer_minute_count;
Ticker timer_ticker;
volatile char i2c_lock = 0;

char debug_mode = DEBUG_MODE;
char debug_output = DEBUG_OUTPUT_STREAM;

SerialComms serial;
Led led;
Sensors sensors;
Motors motors;

Serial pc(USBTX,USBRX);
Serial bt(p13,p14);

DigitalIn rpi1(p5,PullDown);
I2C primary_i2c (p9, p10);
DigitalOut case_led(p15);
DigitalOut mbed_led1(LED1);
DigitalOut mbed_led2(LED2);
DigitalOut mbed_led3(LED3);
DigitalOut mbed_led4(LED4);
AnalogIn vin_battery(p17);
char status_message [8];


// Public functions


void Robot::update_status_message(){
    // Compose an 8-character status message indicating the current status values
    // First two bytes are uptime in 1/10ths of seconds
    int current_uptime = (int) ((get_uptime() * 10.0)) % 65536;
    status_message[0] = current_uptime / 256;
    status_message[1] = current_uptime % 256;
    // Next two bytes are battery voltage * 4096
    int current_voltage = (int) (get_battery_voltage() * 4096);
    status_message[2] = current_voltage / 256;
    status_message[3] = current_voltage % 256;
    // Next byte is (left motor speed + 1) * 127
    status_message[4] = (int) ((motors.get_left_motor_speed() + 1.0) * 127.0);
    // Next byte is (right motor speed + 1) * 127
    status_message[5] = (int) ((motors.get_right_motor_speed() + 1.0) * 127.0);
    // Next byte is left motor current * 96
    status_message[6] = (int) (motors.get_current_left() * 96.0);
    // Next byte is right motor current * 96
    status_message[7] = (int) (motors.get_current_right() * 96.0);   
}

void Robot::case_led_toggle(){
    case_led = 1-case_led;   
}

void Robot::init(){
    //Flash the case LED during init routine
    Ticker case_led_ticker;
    case_led_ticker.attach(this,&Robot::case_led_toggle, 0.25);
    
    //Start the uptime timer
    timer_minute_count = 0;
    timer_ticker.attach(this,&Robot::_update_minutes, 300);  //Update minutes resets the uptime timer every 5 minutes to prevent stop overruns
    mbed_uptime.start();   
    
    //Setup serial interfaces
    serial.setup_serial_interfaces();
    debug("Practical Robotics Modular Robot\n");
    
    //Setup the primary i2c bus to 400kHz baud rate
    primary_i2c.frequency(400000);

    //Send a reset signal to the LED driver
    debug("Resetting LED driver    :  ");
    if(led.reset_led_driver() != 0)debug("FAILED\n");
    else debug("SUCCESS\n");
    wait(0.05);

    //Send the initialisation signal to the LED driver
    debug("Initialising LED driver :  ");
    if(led.init_led_driver() != 0)debug("FAILED\n");
    else debug("SUCCESS\n");
    
    wait(0.05);

    //Start the Sharp sensor ticker
    debug("Starting distance sensor ticker...\n");
    sensors.start_sensor_ticker();
    
    wait(0.05);
    
    //Setup the h-bridge drivers
    debug("Starting the H-bridge drivers...\n");
    motors.init();
    
    wait(0.05);
    
    //Wait for R-Pi pin to become high
    debug("Waiting for Raspberry Pi Zero...\n");
    led.start_animation(0,3);
    while(rpi1==0) wait_us(50);
    led.stop_animation();
    led.all_off();
    case_led_ticker.detach();
    case_led = 1;
    led.start_animation(5,25);
    debug("Initialisation finished: Starting program. \n");
}

float Robot::get_uptime(void)
{
    return mbed_uptime.read() + (timer_minute_count * 60);
}

float Robot::get_battery_voltage(void)
{
    // Voltage is measured through a 7.5V zener diode followed by a 1:1 potential divider
    
    float measure_voltage = ZENER_VOLTAGE + (vin_battery.read() * BATTERY_PD_MULTIPLIER);
    return measure_voltage;
}

void Robot::debug(const char* format, ...)
{
    char buffer[256];
    if (debug_mode) {
        va_list vl;
        va_start(vl, format);
        vsprintf(buffer,format,vl);
        if(debug_output & 2) bt.printf("%s", buffer);
        if(debug_output & 1) pc.printf("%s", buffer);
        va_end(vl);
    }
}



void Robot::_update_minutes()
{
    mbed_uptime.reset();
    timer_minute_count += 5;
}