/* file: no_bump.leaf */

#include <aCmd.tea>
#include <aGPReflexes.tea>



/* assume the BrainStem has address 2 */
#define MODULE 2


/* Custom values to send up to the host in
   Debug messages to give them meaning. */
#define BtHMSG_STOPPED			(unsigned char)72
#define BtHMSG_TOO_CLOSE		(unsigned char)73

#define pSTAT				0

/* message ids */
#define mREADA2D_0			23
#define mREADA2D_1			1
#define mREADA2D_2			2
#define mSRV_A_DISABLE			3
#define mSRV_A_ENABLE			4
#define mSRV_B_DISABLE			5
#define mSRV_B_ENABLE			6
#define mSRV_C_DISABLE			7
#define mSRV_C_ENABLE			8
#define mWINDOW				9
#define mREPEAT				10
#define mMSG_STOPPED			11
#define mMSG_TOO_CLOSE			12
#define mSRV_ALL_STOP			13
#define mSRV_ALL_GO			14
#define mWAITTMR			15
#define mACTION				16
#define mEN_GO				17
#define mDIS_GO				18
#define mCHK_GO				19
#define mEN_STOP			20
#define mDIS_STOP			21
#define mCHK_STOP			22
#define mREAD				24
#define mCHECKTMR			25

/* user-defined vectors */
#define vWINDOW0			100
#define vWINDOW1			101
#define vWINDOW2			102
#define vWINDOW3			103
#define vSRV_ALL_STOP			104
#define vSRV_ALL_GO			105
#define vACTION				106
#define vREAD				107

/************* REFLEX COMMANDS *************/

/* Read IR rangefinder 0 */
message[mREADA2D_0] {
  MODULE, cmdA2D_RD, (unsigned char)0x40
}

/* Read IR rangefinder 1 */
message[mREADA2D_1] {
  MODULE, cmdA2D_RD, (unsigned char)0x41
}

/* Read IR rangefinder 2 */
message[mREADA2D_2] {
  MODULE, cmdA2D_RD, (unsigned char)0x42
}

/* Disables servo A */
message[mSRV_A_DISABLE]
{
	MODULE, cmdSRV_CFG, 0x00, (unsigned char)0x00
}

/* Enables servo A */
message[mSRV_A_ENABLE] 
{
	MODULE, cmdSRV_CFG, 0x00, (unsigned char)0x80
}

/* Disables servo B */
message[mSRV_B_DISABLE]
{
	MODULE, cmdSRV_CFG, 0x01, (unsigned char)0x00
}

/* Enables servo B */
message[mSRV_B_ENABLE] 
{
	MODULE, cmdSRV_CFG, 0x01, (unsigned char)0x80
}

/* Disables servo C */
message[mSRV_C_DISABLE]
{
	MODULE, cmdSRV_CFG, 0x02, (unsigned char)0x00
}

/* Enables servo C */
message[mSRV_C_ENABLE] 
{
	MODULE, cmdSRV_CFG, (unsigned char)2, (unsigned char)128
}

/* Does a window compare with the input value in position 4(default 0).
   When called, you can modify by adding a char to position 4 --
   char + mWINDOW4
   If the argument is less than the lower threshold, a raw input for vWINDOW1
   will be issued. If it between the thresholds, a raw input for vWINDOW2 will
   be issued.  If it is greater than the upper threshold, a raw input for 
   vWINDOW3 will be issued.
*/ 
message[mWINDOW] {
  MODULE, cmdWINDOW, vWINDOW0, 0, (unsigned char)60, (unsigned char)170
}

message[mMSG_STOPPED] {
	MODULE, cmdDEBUG, BtHMSG_STOPPED
}

message[mMSG_TOO_CLOSE] {
	MODULE, cmdDEBUG, BtHMSG_TOO_CLOSE
}

message[mSRV_ALL_STOP] {
	MODULE, cmdRAW_INPUT, vSRV_ALL_STOP, 0
}

message[mSRV_ALL_GO] {
	MODULE, cmdRAW_INPUT, vSRV_ALL_GO, 0
}

message[mWAITTMR] {
  MODULE, cmdTMR_SET, 2, (unsigned char)20, (unsigned char)136
}

message[mACTION] {
	MODULE, cmdRAW_INPUT, vACTION, 0
}	

message[mEN_GO] {
	MODULE, cmdRFLXE_CFG, 0, (unsigned char)233 /* 128(enable) + 105 (all go vector)*/
}

message[mDIS_GO] {
	MODULE, cmdRFLXE_CFG, 0, (unsigned char)105 /* 0(disable) + 105 (all go vector)*/
}

message[mCHK_GO] {
	MODULE, cmdRFLXE_CHK, 0, 0
}

message[mEN_STOP] {
	MODULE, cmdRFLXE_CFG, 1, (unsigned char)232 /* 128(enable) + 104 (all stop vector)*/
}

message[mDIS_STOP] {
	MODULE, cmdRFLXE_CFG, 1, (unsigned char)104 /* 0(disable) + 104 (all stop vector)*/
}

message[mCHK_STOP] {
	MODULE, cmdRFLXE_CHK, 1, 0
}

message[mREAD] {
	MODULE, cmdRAW_INPUT, vREAD, 0
}

message[mCHECKTMR] {
  MODULE, cmdTMR_SET, 1, (unsigned char)20, (unsigned char)136

}

/************* REFLEX VECTORS *************/


vector[vSRV_ALL_STOP]
{
	mSRV_A_DISABLE,
	mSRV_B_DISABLE,
	mSRV_C_DISABLE
}

vector[vSRV_ALL_GO]
{
	mSRV_A_ENABLE,
	mSRV_B_ENABLE,
	mSRV_C_ENABLE
}

vector[aGP_RFX_A2D_0] {
  char + mWINDOW[4]
}

vector[aGP_RFX_A2D_1] {
  char + mWINDOW[4]
}

vector[aGP_RFX_A2D_2] {
  char + mWINDOW[4]
}


vector[vWINDOW2] {
	mDIS_GO,
	mEN_STOP,
	mMSG_TOO_CLOSE
}

vector[vWINDOW3] {
	mDIS_GO,
	mEN_STOP,
	mMSG_STOPPED
}

/* This timer initiates the read and takes appropriate action */
vector[aGP_RFX_TIMER_1] {
	mEN_GO,
	mDIS_STOP,
	mREAD,
	mWAITTMR
}

vector[aGP_RFX_TIMER_2] {
	mACTION,
	mCHECKTMR
}


/* Trigger the input from the A2D converters connected to the
   IR rangefinders. */
vector[vREAD] {
	mREADA2D_0,
	mREADA2D_1,
	mREADA2D_2

}

/* Run the enabler checks, should run only the one that is
   currently enabled, to stop or start all motors */
vector[vACTION] {
	mCHK_GO,
	mCHK_STOP
}