/*
 * EyebotM8test.c
 *
 * Created: 31/10/2012
 * Author: Andrew Adamson
 */ 

#include <avr/io.h>
#include <util/delay.h>
#include <stdio.h>

//ADC calibration library
#include <stddef.h>
#include <avr/pgmspace.h>

//PCA9685 libraries
#include "pca9685.h"

//Bit positions
//Port A
#define PSD0_bp 0
#define PSD1_bp 1
#define PSD2_bp 2
#define PSD3_bp 3
#define PSD4_bp 4
#define PSD5_bp 5

//Port B
#define ADC0_bp 0
#define ADC1_bp 1
#define ADC2_bp 2 //Last four ADC's currently used for JTAG
#define ADC3_bp 3
#define ADC4_bp 4
#define ADC5_bp 5
#define VBATTSENSE_bp //Disconnected for JTAG at the moment

//Port C
//First two pins allocated to TWIC
#define M1_FL_bp 2 //Motor stall flags
#define M2_FL_bp 3
#define M3_FL_bp 4
#define M4_FL_bp 5
#define M12SLEEP_bp 6 //Sleep lines for each motor driver chip
#define M34SLEEP_bp 7

//Port D
#define M1O1_bp 0 //Motor 1 and 2 control lines
#define M1O2_bp 1
#define M2O1_bp 2
#define M2O2_bp 3
#define SPI_CS_bp 4 //SPI serial lines
#define SPI_MOSI_bp 5
#define SPI_MISO_bp 6
#define SPI_CLK_bp 7

//Port E
#define M3O1_bp 0 //Motor 3 and 4 control lines
#define M3O2_bp 1
#define M4O1_bp 2
#define M4O2_bp 3
#define SERVOEN_bp	4 //Extra power supply enable line
#define PWMEN_bp 5 //PCA9685 PWM chip output enable line
#define PWRON_bp 6 //Power on line (not needed until solder jumper is cut)

//Port F
#define M1_ENC1_bp 0 //Two encoders per motor
#define M1_ENC2_bp 1
#define M2_ENC1_bp 2
#define M2_ENC2_bp 3
#define M3_ENC1_bp 4
#define M3_ENC2_bp 5
#define M4_ENC1_bp 6
#define M4_ENC2_bp 7

//Port H
//Caution: "GPIO" already has a #define
#define IO0_bp 0 //16 GPIO lines
#define IO1_bp 1
#define IO2_bp 2
#define IO3_bp 3
#define IO4_bp 4
#define IO5_bp 5
#define IO6_bp 6
#define IO7_bp 7

//Port J
#define IO8_bp 0
#define IO9_bp 1
#define IO10_bp 2
#define IO11_bp 3
#define IO12_bp 4
#define IO13_bp 5
#define IO14_bp 6
#define IO15_bp 7


//Turns on the high current 5V supply that drives the servos and motors
void extra_enable(void) {
	//Set PE4 as output
	PORTE.DIR |= (1<<SERVOEN_bp);
	//Turn on the servo+motor power supply. There is an LED on the board to confirm the supply is on.
	PORTE.OUT |= (1<<SERVOEN_bp);
}

//Sets up the SPI ports and registers in slave mode
void spi_init(void) {
	/*
	* Configure MISO pin on Port D as output. Chip select is ignored for now because we're only writing and
	* there is only one slave
	*/
	PORTD.DIR |= 0b00100000;
	/* 
	* Most significant bit transmitted first by omitting DORD flag, 
	* enable spi, SPI mode 0, leave in slave mode by omitting master enable flag
	*/
	SPID.CTRL = SPI_ENABLE_bm;
}

//Reads a byte out of the SPI data register
char spi_read(void) {
	//Wait for a write to the data register to complete
	while(!(SPID.STATUS&SPI_IF_bm));
	//Read the byte out of the register
	return SPID.DATA;
}

//Receives a byte from the test program by John Hodge, reads it, and turns on an LED if it matches some value
void spi_test(void) {
		spi_init();
	while(1) {
		uint8_t val = spi_read();
		/*
		* 0x37 is the value sent by John Hodge's spi_test program
		*/
		if ( val == 0x37) {
			//Use the LED for the extra power supply as a diagnostic light
			extra_enable();
		}
		_delay_us(100);
	}
}

//Turns on motors 1 and 2. Reads one encoder line of motor 1 and makes IO0 match the input level
void motor_test(void) {
	//Port F is all motor encoder inputs
	PORTF.DIR = 0x00;
	
	//Enable the motor driver chips
	PORTC.DIR |= 0b11000000;
	PORTC.OUT |= (1<<M12SLEEP_bp);
	PORTC.OUT |= (1<<M34SLEEP_bp);
	
	//initialise output ports for the 4 motors
	PORTE.DIR |= 0x0F;
	PORTD.DIR |= 0x0F;

	//Enable the extra 5V power supply (assumes the motors are selected to run off 5V with the jumper)
	extra_enable();
		
	/*
	* Can verify motor signals either with a faulhaber motor (be sure to check the pinout, the old motors are different!)
	* or by connecting a CRO to the motor output pins. The lines below will make the motors spin at full speed in one direction. 
	* Speed control can be done using the timers on the xMega to generate a PWM signal. Change or add to these lines to 
	* control motors 3 and 4
	*/
	PORTD.OUT &= ~(1<<M1O1_bp);
	PORTD.OUT |= (1<<M1O2_bp);
	PORTD.OUT &= ~(1<<M2O1_bp);
	PORTD.OUT |= (1<<M2O2_bp);
	
	//Polling based code to check the encoder inputs are working properly
	//Reads encoder from motor 1 and sets a GPIO pin high if the encoder is outputting high 
	//Can be verified with oscilloscope
	PORTH.DIR = 0xFF;
	while(1) {
		if (PORTF.IN > 0) {
			PORTH.OUT = 0x01;
		}
		else {
			PORTH.OUT = 0x00;
		}
	}
}

//Turns on motor 1. Reads the stall flag for motor 1 and makes IO0 match the input level
void motor_stall_test() {
	//Enable the motor driver chips
	PORTC.DIR |= (1<<M12SLEEP_bp) | (1<<M34SLEEP_bp);
	PORTC.OUT |= (1<<M12SLEEP_bp);
	PORTC.OUT |= (1<<M34SLEEP_bp);
	
	//initialise output ports for the 4 motors (two outputs per motor)
	PORTE.DIR |= 0x0F;
	PORTD.DIR |= 0x0F;
	
	//set the motor flag pins as pull-up (the A3908 chip pulls the line low when the motor is stalled)
	PORTC.PIN2CTRL |= PORT_OPC_PULLUP_gc;
	PORTC.PIN3CTRL |= PORT_OPC_PULLUP_gc;
	PORTC.PIN4CTRL |= PORT_OPC_PULLUP_gc;
	PORTC.PIN5CTRL |= PORT_OPC_PULLUP_gc;

	//Enable the extra 5V power supply (we assume the motors are selected to run off 5V with the jumper during testing)
	extra_enable();
	
	//turn on motor 1
	PORTD.OUT &= ~(1<<M1O1_bp);
	PORTD.OUT |= (1<<M1O2_bp);
	
	/*
	* polling based code to check the motor-stall flags do get set when the motor is pulling more than 1A
	* test by putting a high wattage 5ohm resistor across the motor contacts (for a SHORT time) and 
	* checking any of the first eight GPIO pins goes high with an oscilloscope
	*/
	PORTH.DIR = 0xFF; //set all outputs on port H so we can use the GPIO pins
	while(1) {
		if (!(PORTC.IN & (1<<M1_FL_bp))) {
			PORTH.OUT = 0x01;
		}
		else {
			PORTH.OUT = 0x00;
		}
	}
}

//Switches all GPIO pins on and off at 1Hz
void gpio_test(void) {
	//Initialise ports
	//Make all the GPIO ports outputs
	PORTH.DIR=0xFF;
	PORTJ.DIR=0xFF;
	//Set all the pins high then low. Verify signals with CRO.
	//For more comprehensive testing, set every alternate pin high and also check with CRO to make sure there are no shorts
	//For super-comprehensive testing, change all the pin states as fast as possible and check signal integrity
	while (1) {
		PORTH.OUT = 0xFF;
		PORTJ.OUT = 0xFF;
		_delay_ms(500);
		PORTH.OUT = 0x00;
		PORTJ.OUT = 0x00;
		_delay_ms(500);
	}
}

//Enables the eyebot specific features required to drive servos
void servo_enable(void) {
	//Enable outputs on the PCA9685
	//Set port direction
	PORTE.DIR |= (1<<PWMEN_bp);
	//Active low output enable. This only controls the output, and does not disable I2C communication with the PCA9685
	PORTE.OUT &= ~(1<<PWMEN_bp);
	//Turn on the servo power
	extra_enable();
}

void servo_test(void) {
	//Set up eyebot specific things like enabling power
	servo_enable();
	//Initialise the PCA9685 on address 0xE0
	pca9685_init(0xE0);
	//Loop to make servos 1 to 3 swing backwards and forwards to their extremes (1ms to 2ms duty cycle)
	//Pass a value between 0 and 4095 to set duty cycle
	//Signal frequency is set in pca9685.h (currently 184Hz)
	uint16_t i;
	while (1) {
		//Count up
		for(i = 800; i <= 1550; i+=20) {
			pca9685_send(0xE0, i, 0);
			pca9685_send(0xE0, i, 1);
			pca9685_send(0xE0, i, 2);
			_delay_ms(4);
		}
		//Count down
		for(i = 1550; i >= 800; i-=20) {
			pca9685_send(0xE0, i, 0);
			pca9685_send(0xE0, i, 1);
			pca9685_send(0xE0, i, 2);
			_delay_ms(4);
		}
	}
}

//Helper function to calibrate the ADC according to factory-set calibration value
uint8_t ReadCalibrationByte( uint8_t index )
{
	uint8_t result;

	//Load the NVM Command register to read the calibration row
	NVM_CMD = NVM_CMD_READ_CALIB_ROW_gc;
	result = pgm_read_byte(index);

	//Clean up NVM Command register
	NVM_CMD = NVM_CMD_NO_OPERATION_gc;

	return( result );
}

//Reads value from IR distance sensor and outputs it as 8 bits on the first 8 IO pins
void adc_test(void) {
	//Most of this function is based on an example from
	// http://www.bostonandroid.com/manuals/xmega-precision-adc-howto.html
	
	//Calibrate the ADC from the production signature row
	ADCA.CALL = ReadCalibrationByte( offsetof(NVM_PROD_SIGNATURES_t, ADCACAL0) );
	ADCA.CALH = ReadCalibrationByte( offsetof(NVM_PROD_SIGNATURES_t, ADCACAL1) );
	
	//Set all of Port A as input
	PORTA.DIR = 0x00; 
	//Set Port H as output to display the result
	PORTH.DIR = 0xFF;
	//12 bit conversion
	ADCA.CTRLB = ADC_RESOLUTION_12BIT_gc;
	/*
	* Use internal 1V bandgap reference
	* This means that only voltages between ground and 1V can be measured, so
	* the voltage must be dropped to a suitable level in hardware
	*/
	ADCA.REFCTRL = ADC_REFSEL_INT1V_gc | 0x02;
	//Measure at 250kHz (divide system clock by 8)
	ADCA.PRESCALER = ADC_PRESCALER_DIV8_gc; 
	//Select single ended measurement
	ADCA.CH0.CTRL = ADC_CH_INPUTMODE_SINGLEENDED_gc; 
	//Choose which pin to use
	ADCA.CH0.MUXCTRL = ADC_CH_MUXPOS_PIN0_gc;
	//Set the enable bit to enable the ADC module
	ADCA.CTRLA |= 0x01;
	//Delay a little
	_delay_ms(3);
	//Go into a measurement loop
	while(1) {
		//Set the start-conversion bit to start a single conversion
		ADCA.CH0.CTRL |= ADC_CH_START_bm;
		//Wait for the conversion complete flag to be set
		while(!ADCA.CH0.INTFLAGS);
		//Read the value from the results register to a local variable
		int result = ADCA.CH0RES;
		//Divide down to a range that can be represented with 8 bits
		char psd = result/16;
		//Set the pins on port H so the value can be read with logic analyzer
		PORTH.OUT = psd;
		//Delay a little bit to make reading the byte easier with logic analyzer
		_delay_ms(10);
	}		
}

int main (void)
{	
	/*
	* Uncomment the test you want to perform. Do these tests individually since they were not
	* written to work together (most of the functions rely on polling rather than interrupts)
	*/
	//spi_test();
	//motor_test();
	//motor_stall_test();
	servo_test();
	//gpio_test();
	//extra_enable();
	//adc_test();
}
