Go straight controller.cpp

#include "go_straight_controller.h"
#include "ci_logger.h"

CGoStraightController::CGoStraightController( void )
   : CCIController( )
{
}

CGoStraightController::~CGoStraightController( void )
{
}

CCIController::ReturnVal CGoStraightController::Init( void ) 
{
   CCILogger::SetLogPrefix( "SGT" );
   CCILogger::LogMsg( "Initialising GoStraight Controller\n" );

   // Call base class init function which does a bunch of sbot initialisation
   CCIController::Init( );

   return CCIController::RETURN_OK;
}

CCIController::ReturnVal CGoStraightController::ControlStep( void )
{
   static int iFirstTimeRound = 0;

   if ( iFirstTimeRound == 0 )
   {
      iFirstTimeRound = 1;
      CCILogger::LogMsg( "Setting track speeds (2, 2)" );
      m_pcCISBot->SetSpeed( 2, 2 );
   }

   return CCIController::RETURN_CONTINUE;
}


CCIController::ReturnVal CGoStraightController::Stop( void )
{
   CCILogger::LogMsg( "Stopping GoStraight Controller\n" );
   m_pcCISBot->SetSpeed( 0, 0 );
   return CCIController::RETURN_OK;
}