/* 
   Test of RobotBehaviours.h
   Last updated 24.2.02.
 */

#include "RCX_String.h"
#include "RCX_Control.h"
#include "H8.h" 
#include "Time.h"
#include "LCD.h"
#include "Battery.h"
#include "Buttons.h"

#include "RobotRTS.h"
#include "RobotBehaviours.h"

void _start(void) 
{ 
  byte power;

  ButtonsInit();
  lcd_show_int16(BatteryLevel());

  while ( ! Running );

  RTSInit();
  RobotBehavioursInit();

  RobotGoForward(8);lcd_show_int16(8);
  BusyPauseMS(3000);

  RobotRest();
  BusyPauseMS(3000);

  RobotGoBackward(7);lcd_show_int16(7);
  BusyPauseMS(3000);

  RobotTurnClockwise(6);lcd_show_int16(6);
  BusyPauseMS(3000);

  RobotTurnCounterClockwise(5);lcd_show_int16(5);
  BusyPauseMS(3000);

  power = 0;
  while ( power < 17 ) {
    RobotGoForward(power); 
    lcd_show_int16(power); power++;
    BusyPauseMS(1000);
  }
  
  power = 0;
  while ( power < 17) {
    RobotGoBackward(power); 
    lcd_show_int16(power); power++;
    BusyPauseMS(1000);
  }
  RobotGoForwardAndTurn(16,8); 
  lcd_show_int16(16); lcd_show_digit(8); BusyPauseMS(3000);
  RobotGoForwardAndTurn(13,2); 
  lcd_show_int16(13); lcd_show_digit(2); BusyPauseMS(3000);
  RobotGoForwardAndTurn(2,8); 
  lcd_show_int16(2); lcd_show_digit(8); BusyPauseMS(3000);
  RobotGoForwardAndTurn(8,2); 
  lcd_show_int16(8); lcd_show_digit(2); BusyPauseMS(3000);  
  RobotGoForwardAndTurn(2,8); 
  lcd_show_int16(2); lcd_show_digit(8); BusyPauseMS(3000);
  RobotGoForwardAndTurn(8,2); 
  lcd_show_int16(8); lcd_show_digit(2); BusyPauseMS(3000);
  RobotGoForwardAndTurn(2,8); 
  lcd_show_int16(2); lcd_show_digit(8); BusyPauseMS(3000);
  RobotGoForwardAndTurn(3,7);
  lcd_show_int16(3); lcd_show_digit(7); BusyPauseMS(3000);
  RobotGoForwardAndTurn(7,3);
  lcd_show_int16(7); lcd_show_digit(3); BusyPauseMS(3000);
  RobotGoForwardAndTurn(4,6);
  lcd_show_int16(4); lcd_show_digit(6); BusyPauseMS(3000);
  RobotGoForwardAndTurn(6,4);
  lcd_show_int16(6); lcd_show_digit(4); BusyPauseMS(3000);
  RobotGoForwardAndTurn(5,5);
  lcd_show_int16(5); lcd_show_digit(5); BusyPauseMS(3000);

  RobotGoBackwardAndTurn(5,5); 
  lcd_show_int16(5); lcd_show_digit(5); BusyPauseMS(3000);
  RobotGoBackwardAndTurn(4,6); 
  lcd_show_int16(4); lcd_show_digit(6); BusyPauseMS(3000);
  RobotGoBackwardAndTurn(6,4);
  lcd_show_int16(6); lcd_show_digit(4); BusyPauseMS(3000);
  RobotGoBackwardAndTurn(3,7);
  lcd_show_int16(3); lcd_show_digit(7); BusyPauseMS(3000);
  RobotGoBackwardAndTurn(7,3); 
  lcd_show_int16(7); lcd_show_digit(3); BusyPauseMS(3000);
  RobotGoBackwardAndTurn(2,8); 
  lcd_show_int16(2); lcd_show_digit(8); BusyPauseMS(3000);
  RobotGoBackwardAndTurn(8,2); 
  lcd_show_int16(8); lcd_show_digit(2); BusyPauseMS(3000);

  RobotRest(); lcd_show_int16(0); lcd_show_digit(0);
  BusyPauseMS(3000);

  RobotGoForward(8); lcd_show_int16(8); lcd_show_digit(8); BusyPauseMS(3000);

  RobotBehavioursClose();
  BusyPauseMS(3000);

  RTSClose();

  RCX_Reset();

}