// ----------------------------------------------------
// TEST application for
// Cruise Control application for Panther Electro-Steer
//
// -- TEST0 -- Detect & report installed devices
//
// Copyright 2005,2006, Mike Noel, All Rights Reserved
// ----------------------------------------------------

#include <aCore.tea>
#include <aCmd.tea>
#include <aA2D.tea>
#include <aI2c.tea>
#include <aDig.tea>
#include <aPrint.tea>

// ********************* Definitions *********************

// digital i/o port definitions. 
// 0 - output - reflex uses to drive buzzer...
// 1 - output - drive ES relay to go port
// 2 - output - drive ES relay to go starboard
// 3 - input  - read ES switch labeled DN
// 4 - input  - read ES switch labeled UP
// 5 - output - enable Horizontal Daventech compass
// 6 - output - enable Vertical   Daventech compass
// 7 - input  - read configuration switch labeled 4
// 8 - input  - read configuration switch labeled 3
// 9 - input  - read configuration switch labeled 2
// 10- input  - read configuration switch labeled 1

#define PORTDIO		1
#define STARDIO		2
#define DnButton	3
#define UpButton	4

#define EnaHC		5
#define EnaVC		6

#define Config4		7
#define Config3		8
#define Config2		9
#define Config1		10

// ********************* Test Routine ********************


void main () {

// print pgm header msg
	aPrint_String("Test 0 - Detecting optional devices...\n"); aCore_Sleep(2000);
	aPrint_String("    ---- ignore '4:no iic ack' msgs...\n"); aCore_Sleep(2000);
	aPrint_String(" Found\n"); aCore_Sleep(1000);

// set i2c to low speed (100kHz)
	asm {
	    pushlb 0
	    popbm  aPortIICBaud
	    }

// confige the digital ports
	aDig_Config(EnaHC, ADIG_OUTPUT);
	aDig_Config(EnaVC, ADIG_OUTPUT);

// detect Accelerometer (skip first reading)
	aA2D_ReadInt(0); if (aA2D_ReadInt(0) > 100) {
		aPrint_String("  Accelerometer\n"); aCore_Sleep(1000);
		}

// send reset to um-fpu (if present) and detect it
	aI2C_WriteChar((unsigned char)0x0C8, 1, 0 );	// reset
	aCore_Sleep(100);
	if (aI2C_ReadChar((unsigned char)0x0C8, 0) != -1) { 
		aPrint_String("  FPU\n"); aCore_Sleep(1000);
		}

// detect MindSensor Horizontal compass
	if (aI2C_ReadChar((unsigned char)0x0E0, 0) != -1) {
		aPrint_String("  MindSensor horz compass\n"); aCore_Sleep(1000);
		}

// detect MindSensor Vertical compass
	if (aI2C_ReadChar((unsigned char)0x0E8, 0) != -1) {
		aPrint_String("  MindSensor vert compass\n"); aCore_Sleep(1000);
		}

// detect Daventech Horizontal compass
	aDig_Write(EnaVC, 0); aDig_Write(EnaHC, 1); aCore_Sleep(500);
	if (aI2C_ReadChar((unsigned char)0x0C0, 0) != -1) {
		aPrint_String("  Daventech horz compass\n"); aCore_Sleep(1000);
		}
	aDig_Write(EnaHC, 0);	
	
// detect Daventech Vertical compass
	aDig_Write(EnaVC, 1); aCore_Sleep(500);
	if (aI2C_ReadChar((unsigned char)0x0C0, 0) != -1) {
		aPrint_String("  Daventech vert compass\n"); aCore_Sleep(1000);
		}
	aDig_Write(EnaVC, 0);	
}