// RCX Control of RC Nitro Flash Racer (4589)
// Code adapted from John Barnes Manas motor control
// see http://news.lugnet.com/robotics/?n=15832


#define Channel_1 5
#define Channel_2 6
#define Channel_3 7

#define EM_FLOAT 0
#define EM_OFF 8
#define EM_FWD 7
#define EM_REV 15

#define ST_RIGHT 7
#define ST_LEFT 15
#define ST_STRAIGHT 0

int steering,drive; //Global motor control locations

task main()
{
// This demo starts the Send_IR comms task, then moves
// Nitro Flash

start Send_IR;

steering=ST_STRAIGHT;
drive=EM_OFF;
Wait(100);

// Drives straight ahead
steering=ST_STRAIGHT;
drive=EM_FWD;
Wait(100);

// Turns right
steering=ST_RIGHT;
drive=EM_FWD;
Wait(100);

// Turns left
steering=ST_LEFT;
drive=EM_FWD;
Wait(100);

// Stops
steering=ST_STRAIGHT;
drive=EM_OFF;
Wait(100);

// Backs up
steering=ST_STRAIGHT;
drive=EM_REV;
Wait(100);

// The end...
stop Send_IR;
PlaySound (SOUND_DOUBLE_BEEP);
}
// The Send_IR task continuously resends the motor settings.
// When the RCX is halted, this task stops and the
// motors automatically shut off

task Send_IR()
{
// First ensure serial settings are still ok
SetSerialComm(SERIAL_COMM_4800|SERIAL_COMM_DUTY25|SERIAL_COMM_76KHZ);
SetSerialPacket(SERIAL_PACKET_DEFAULT);

// Setting to low power so we don't run the battery down too quick
// may require a white reflector. So I use HI power
SetTxPower(TX_POWER_HI);

  while (true)
  {
    // Set the first unit two message bytes and send them
    SetSerialData(0,Channel_1*0x10+steering);
    SetSerialData(1,drive*0x10+0x10-((Channel_1+steering+drive)&0xf));
    SendSerial(0,2);
    // Delay for a while so we can resend them regularly
    Wait(20);
  }
}