example_control.cpp

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>

#include <iomanip>
#include <iostream>
#include <string>

#include <can_wrap.hpp>
using can::operator<<;

#include <control_frames.h>
#include <status_frames.h>

int main( int argc, char* argv[] )
{
    const std::string canChannel = "vcan0";

    AxleTorqueFrame frontAxle;
    AxleTorqueFrame rearAxle( false );
    StatusFrame status;
    SteeringFrame steering;
    WheelCountsFrame wheelCounts;
    WheelSpeedsFrame wheelSpeeds;

    AxleControlFrame frontControl( frontAxle, wheelSpeeds );
    AxleControlFrame rearControl( rearAxle, wheelSpeeds, false );
    StatusControlFrame statusControl( status );
    SteeringControlFrame steeringControl( steering );

    std::array< ProcessFrame* const, 6 > frames { &frontAxle, &rearAxle, &status, &steering, &wheelCounts, &wheelSpeeds };
    std::array< ControlFrame* const, 4 > controls { &frontControl, &rearControl, &statusControl, &steeringControl };

    try
    {
        const int canSocket = can::connect( canChannel );

        for( int c=0; c<100000; ++c )
        {
            const can_frame frame = can::read( canSocket );

            // read all in
            for( auto& f : frames )
            {
                if( f->frameId == frame.can_id )
                {
                    f->process( frame );
                    std::cout << *f << std::endl;
                }
            }

            // write all out
            for( auto& f : controls )
            {
                if( f->frameId == frame.can_id )
                {
                    f->process( canSocket );
                }
            }
        }

        can::close( canSocket );
    }
    catch( const std::exception& e )
    {
        std::cerr << e.what() << std::endl;
    }

	return 0;
}