File content as of revision 2:74d5233b6284:

#include "mbed.h"
#include "TFC.h"
#include "Telemetry.hpp"

/*
    Example of the 'Telemetry' library, a portable communication library for embedded devices.
    The PC-side of this code is hosted a github.com/Overdrivr/telemetry-examples
    
    This code uses the car from the Freescale Cup competition initially dedicated to be a 
    line-following racing car, and turns it into a full-blown radio-controlled car.
    
    It is using a Bluetooth serial module to provide wireless connectivity. See the repository at
    github.com/Overdrivr/telemetry-examples for complete instructions on wiring everthing together.
    
    Repo : TO BE DONE  
*/

struct TM_state {
  float direction;
  float throttle;  
};

// Definition of the callback function
void process(TM_state* carState, TM_msg* msg);

int main()
{   
    // Create a Telemetry instance, running on uart at 115200 bauds
    Telemetry TM(115200);
    
    // a data structure to hold writeable parameters
    // i.e. the car direction and throttle that will be both be controlled from the laptop
    TM_state carState;
    carState.direction = 0;
    carState.throttle = 0;
    
    // Suscribe our custom processing function (= callback function), and pass the data structure
    // so that we can access it inside
    // This way, everytime a frame is received, Telemetry will call this function for us,
    // and pass the TM_state data structure to it.
    TM.sub(process, &carState);
    
    DigitalOut led(LED1);
    led = 1;
    
    // Init the TheFreescaleCup shield
    TFC_Init();
    
    // Some timers... for timing :p
    Timer servo_timer;
    Timer motor_timer;
    Timer tm_timer;
    Timer print_timer;
    
    servo_timer.start();
    motor_timer.start();
    tm_timer.start();
    print_timer.start();
    
    // Activate the engines !
    TFC_HBRIDGE_ENABLE;
    
    for(;;)
    {       
        TM.update();   
        
        // update servo control
        if(servo_timer.read_ms() > 10)
        {
            servo_timer.reset();
            TFC_SetServo(0, carState.direction);
        }
        
        // update motor control
        if(motor_timer.read_ms() > 5)
        {
            motor_timer.reset();
            TFC_SetMotorPWM(carState.throttle , carState.throttle);
        }
        
        // publish car state & blink a led
        if(print_timer.read_ms() > 200)
        {
            print_timer.reset();
            // Publish the car parameters, so we can access them from the computer
            TM.pub_f32("direction",carState.direction); 
            TM.pub_f32("throttle",carState.throttle);
            led = (led == 0) ? 1 : 0;  // Toggle the led
        }
    }
}

// This is our processing function called every time a frame is received
// First parameter is a pointer to the data structure we defined in main
// Second parameter is a pointer to a data structure containing all info about received frame 
void process(TM_state* carState, TM_msg* msg)
{
    // temp variable
    float value = 0.f;
    // If received topic == 'direction'
    if(strcmp(msg->topic,"direction") == 0)
    {
        // If data type matches 'float32'
        if(emplace_f32(msg,&value))
        {
            // Check values in order to avoid going beyond servomotor limits
            if(value > 1.f)
                value = 1.f;
            if(value < -1.f)
                value = -1.f;
            carState->direction = value;
        }
    }
    // Idem for throttle
    else if(strcmp(msg->topic,"throttle") == 0)
    {
        if(emplace_f32(msg,&value))
        {
            if(value > 1.f)
                value = 1.f;
            if(value < -1.f)
                value = -1.f;
            carState->throttle = value; 
        }
    }
}