main.cpp

00001 #include "telemetry/Telemetry.hpp"
00002 #include "FRDM-TFC/TFC.h"
00003 
00004 /*
00005     Example of the 'Telemetry' library, a portable communication library for embedded devices.
00006     
00007     This code fetches all the data from the Freescale Cup RC-car.
00008     It relies on a more advanced shield library that is able to read current feedback from HBridges.
00009     
00010     The available data is the following :
00011     * Potentiometer 0
00012     * Potentiometer 1
00013     * 4-bit DIP switch
00014     * Battery level
00015     * current inside HBridge A
00016     * current inside HBridge B
00017     
00018     You can use the Pytelemetry Command Line Interface to open plots, visualize the received data,
00019     and communicate with the car.
00020     See https://github.com/Overdrivr/pytelemetrycli
00021 */
00022 
00023 struct TM_state
00024 {
00025     float throttle;
00026 };
00027 
00028 // Definition of the callback function
00029 void process(TM_state * state, TM_msg * msg);
00030 
00031 int main()
00032 {
00033     // Init the shield
00034     TFC_Init();
00035     
00036     // Create a Telemetry instance, running on uart at 115200 bauds
00037     Telemetry tm(115200);
00038     
00039     // a data structure to hold writeable parameters
00040     // i.e. the car direction and throttle that will be both be controlled from the laptop
00041     TM_state state;
00042     state.throttle = 0.0;
00043     
00044     // Suscribe our custom processing function (= callback function), and pass the data structure
00045     // so that we can access it inside
00046     // This way, everytime a frame is received, Telemetry will call this function for us,
00047     // and pass the TM_state data structure to it.
00048     tm.sub(process, &state);
00049     
00050     Timer refresh_timer;
00051     refresh_timer.start();
00052     
00053     // Activate the engines !
00054     TFC_HBRIDGE_ENABLE;
00055 
00056     for( ; ; )
00057     {
00058         // 20 times per second, re-send data
00059         if(refresh_timer.read_ms() > 50)
00060         {
00061             tm.pub_f32("HBcurrentA",TFC_ReadMotorCurrent(0));
00062             tm.pub_f32("HBcurrentB",TFC_ReadMotorCurrent(1));
00063             tm.pub_f32("pot0",TFC_ReadPot(0));
00064             tm.pub_f32("pot1",TFC_ReadPot(1));
00065             tm.pub_f32("bat",TFC_ReadBatteryVoltage());
00066             tm.pub_u8("dip",TFC_GetDIP_Switch());
00067             tm.pub_f32("throttle",state.throttle);
00068             
00069             TFC_SetMotorPWM(state.throttle ,state.throttle);
00070             // Update in order to process received data
00071             tm.update();
00072         }    
00073     }   
00074 }
00075 
00076 // This is our processing function called every time a frame is received
00077 // First parameter is a pointer to the data structure we defined in main
00078 // Second parameter is a pointer to a data structure containing all info about received frame 
00079 
00080 void process(TM_state * state, TM_msg * msg)
00081 {
00082     // If data type matches 'float32'
00083     if(msg->type == TM_float32)
00084     {
00085         float th = 0;
00086         // Emplace the data into 'th'
00087         if(emplace_f32(msg,&th))
00088         {
00089             // If the emplace has worked, push the value to the actual value state->throttle
00090             // This way the processing is entirely safe
00091             state->throttle = th;   
00092         }
00093     }   
00094 }