Go straight controller.cpp

1. include "go_straight_controller.h"
2. 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;

}