/*
Copyright (c) 2010, Dale Weber <robotguy@hybotics.org>
All rights reserved.

Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following
conditions are met:

    * Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
    * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer
		in the documentation and/or other materials provided with the distribution.
    * Neither the name of Hybrid Robotics nor the names of its contributors may be used to endorse or promote products derived
		from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING,
BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

Project:	Software for W.A.L.T.E.R.,
			the Wheeled Autonomous Learning Terrain Exploring Roving
Date:		15-May-2010
Version:	2.0.01
Author:		Dale Weber (robotguy@hybotics.org)
Controller:	AXON by Society of Robots
			SSC-32 Servo Controller by Lynxmotion
			Sabertooth 2X5 motor controller by Dimension Engineering
Power:		6V@2800 mAH battery for and servos
			7.2V@2800 mAH battery for motors and motor controller
			7.2V@2000 mAH battery for the electronics
Notes:		This software uses the Webbot framework, found at http://sourceforge.net/projects/webbotavrclib/
*/

#include "sys/axon.h"							// The Society of Robots Axon Microcontroller
#include "Sensors\Distance\Ping\PingSonar.h"
#include "Sensors\Distance\Sharp\GP2.h"

// Now include any other files that are needed here
#include "uart.h"
#include "rprintf.h"
#include "walter.h"

// Variable definitions
PingSonar ping[PING_MAX] = {
	MAKE_PingSonar(ADC0),
	MAKE_PingSonar(ADC1),
	MAKE_PingSonar(ADC2),
	MAKE_PingSonar(ADC3),
	MAKE_PingSonar(ADC4),
	MAKE_PingSonar(ADC5),
	MAKE_PingSonar(ADC6),
	MAKE_PingSonar(ADC7)
};

Sharp_GP2D12 ir[GP2D12_MAX] = {
	MAKE_Sharp_GP2D12(ADC8),
	MAKE_Sharp_GP2D12(ADC9),
	MAKE_Sharp_GP2D12(ADC10),
	MAKE_Sharp_GP2D12(ADC11),
	MAKE_Sharp_GP2D12(ADC12),
	MAKE_Sharp_GP2D12(ADC13),
	MAKE_Sharp_GP2D12(ADC14),
	MAKE_Sharp_GP2D12(ADC15)
};

// Holds all servo related data { servo pin, uS offset from 1500 uS center, home pos in uS,
//	uS position, angle -90 to +90 degrees }
SERVO servos[SERVO_NR] = {
	{  0, 0, 1500, 1500, 0 },				// Right Front leg
	{  1, 0, 1800, 1500, 0 },
	{  2, 0,  900, 1500, 0 },
	{  3, 0, 1500, 1500, 0 },
	{  4, 0, 1500, 1500, 0 },
	{  5, 0, 1500, 1500, 0 },				// Right Rear leg
	{  6, 0, 1500, 1500, 0 },
	{  7, 0, 1500, 1500, 0 },
	{  8, 0, 1500, 1500, 0 },
	{  9, 0, 1500, 1500, 0 },
	{ 16, 0, 1500, 1500, 0 },				// Left Front leg
	{ 17, 0, 1500, 1500, 0 },
	{ 18, 0, 1500, 1500, 0 },
	{ 19, 0, 1500, 1500, 0 },
	{ 20, 0, 1500, 1500, 0 },
	{ 21, 0, 1500, 1500, 0 },				// Left Rear leg
	{ 22, 0, 1500, 1500, 0 },
	{ 23, 0, 1500, 1500, 0 },
	{ 24, 0, 1500, 1500, 0 },
	{ 25, 0, 1500, 1500, 0 },
	{ 30, 0, 1500, 1500, 0 },				// Front Pan/Tilt
	{ 31, 0, 1500, 1500, 0 }
};

void switch_console (void) {
	rprintfInit(&uart1SendByte);			// Tell rprintf to output to UART1 (CONSOLE)
}

void switch_ssc32 (void) {
	rprintfInit(&uart0SendByte);			// Tell rprintf to output to UART0 (SSC-32)
}

// Functions and procedures
void do_heartbeat (void) {
	statusLED_off();
	delay_ms(500);
	statusLED_on();
	delay_ms(500);
}

void read_sensors (void) {
	int i;

	for (i = 0; i < PING_NR; i++) {
		distanceRead(ping[i]);
	}

	for (i = 0; i < GP2D12_NR; i++) {
		distanceRead(ir[i]);
	}
}

// Move a single servo - buffers until last servo
int move_servo (int joint, int newpos, int last) {
	rprintf("#%iP%i", servos[joint].pin, newpos);

	servos[joint].currpos = newpos;

	if (last) {
		rprintf("\r");
	}

	return 0;
}

// Move a single servo (timed move, quarter second increment) - buffers until last servo
int move_servo_timed (int joint, int newpos, int qsec, int last) {
	rprintf("#%iP%i", servos[joint].pin, newpos);

	servos[joint].currpos = newpos;

	if (last) {
		rprintf("T%i\r", qsec * 250);		// qsec = 1 would be 250 mS
	}

	return 0;
}

//	End of my functions, definitions and procedures

//	Now create any global variables such as motors, servos, sensors etc
//		This routine is called once only and allows you to do set up the hardware
//		Dont use any 'clock' functions here - use 'delay' functions instead
void appInitHardware(void) {
	int i, last = FALSE;

	statusLEDregister(STATUS_LED, TRUE);	// Register the status LED

	// Initialize all the UARTs
	uartInit(SSC32, 115200);				// Set UART0 to 115200 baud for the SSC-32
	rprintfInit(&uart0SendByte);			// Tell rprintf to output to UART0

	uartInit(CONSOLE, 115200);				// Set UART1 (USB) to 115200 baud

	uartInit(SABERTOOTH, 38400);			// Set UART2 to 38400 baud for motor controllers

	uartInit(XBEE, 115200);					// Set UART3 to 115200 baud for XBee wireless

	// Initialize the PING sensors
	for (i = 0; i < PING_NR; i++) {
		distanceInit(ping[i]);
	}

	// Initialize the Sharp GP2D12 IR sensors
	for (i = 0; i < GP2D12_NR; i++) {
		distanceInit(ir[i]);
	}

	// Move all servos to home position
	for (i = 0; i < SERVO_NR; i++) {
		if (i == (SERVO_NR - 1) ) {
			last = TRUE;
		}
		
		move_servo(i, servos[i].homep, last);
	}
}

//	This routine is called once to allow you to set up any other variables in your program
//		You can use 'clock' function here.
//		The loopStart parameter has the current clock value in ìS
TICK_COUNT appInitSoftware(TICK_COUNT loopStart) {
	return 0;								// Dont pause after
}

// This routine is called repeatedly - its your main loop
TICK_COUNT appControl(LOOP_COUNT loopCount, TICK_COUNT loopStart) {
	do_heartbeat();

	read_sensors();

	switch_console();
	rprintf("Ping distance = %d cm, GP2D12 distance = %d cm\n", ping[SENS_CENTER].distance.cm, ir[SENS_CENTER].distance.cm);
	switch_ssc32();

	//	rprintf("Ping distance = %d cm\n", ping[SENS_CENTER].distance.cm);

	return 1000000;							// Wait for 1 second before calling me again. 1000000us = 1 second
}