/* Robot Behaviours
   Last updated 24.2.01
 */

#include "H8.h"
#include "OutputPorts.h"
#include "MotorControl.h"


#ifndef   RobotBehaviours_H_DEFINED
#define   RobotBehaviours_H_DEFINED

/* Simple behaviours for a robot with two motors on port A and C.
   All routines just starts the behaviour and immediately returns.
   To make the behaviour last explicit elapse of time should be 
   controlled in the calling program.
 */

#define Aback    OnNeg
#define Aforward OnPos
#define Cback    OnNeg
#define Cforward OnPos

void RobotRest ( void )
{
  MotorASetState(Float); MotorCSetState(Float);
}

void RobotBrake ( void )
{
  MotorASetState(Brake);   MotorCSetState(Brake);
  MotorASetPower(16);      MotorCSetPower(16);
}

void RobotGoForward ( byte power )
{
  MotorASetState(Aforward); MotorCSetState(Cforward);
  MotorASetPower(power);    MotorCSetPower(power);
}

void RobotGoBackward ( byte power )
{
  MotorASetState(Aback); MotorCSetState(Cback);
  MotorASetPower(power); MotorCSetPower(power);
}

void RobotGoForwardAndTurn ( byte powerLeft, byte powerRight )
{
  MotorASetState(Aforward);  MotorCSetState(Cforward);
  MotorASetPower(powerLeft); MotorCSetPower(powerRight);
}

void RobotGoBackwardAndTurn ( byte powerLeft, byte powerRight )
{
  MotorASetState(Aback);     MotorCSetState(Cback);
  MotorASetPower(powerLeft); MotorCSetPower(powerRight);
}

void RobotTurnClockwise ( byte power )
{
  MotorASetState(Aforward); MotorCSetState(Cback);
  MotorASetPower(power);    MotorCSetPower(power);
}

void RobotTurnCounterClockwise ( byte power )
{
  MotorASetState(Aback); MotorCSetState(Cforward);
  MotorASetPower(power); MotorCSetPower(power);
}

void RobotCircleClockwise ( byte power )
{
  MotorASetState(Aforward); MotorCSetState(Brake);
  MotorASetPower(power);    MotorCSetPower(16);
}

void RobotCircleCounterClockwise ( byte power )
{
  MotorASetState(Brake);   MotorCSetState(Cforward);
  MotorASetPower(16);      MotorCSetPower(power);
}

void RobotBehavioursInit( void )
{
  /* MotorControl has been initialized from RTS */
}

void RobotBehavioursClose( void )
{
  MotorControlStopMotors();
}


#endif /* RobotBehaviours_H_DEFINED */