/*
 * pca9685.h
 * Author: Andrew Adamson
 * Based on PIC code from: http://www.ccsinfo.com/forum/viewtopic.php?p=166816
 * Provides routines to initialise a PCA9685 and set servo PWM values
 */

#include "xmegatwi.h"
#include <util/delay.h>

#ifndef PCA9685_H
#define PCA9685_H

// Useful PCA9685 registers
#define MODE1 0x00	// 0x00 location of Mode1 register address
#define MODE2 0x01	// 0x01 location of Mode2 register address
#define SERVO0 0x06	// location for start of LED registers

/*
* Init code for the PCA9685
* Input: 8 bit write address of the PCA9685 to init
*/
void pca9685_init(uint8_t address) {
	/*
	* How to communicate with the PCA9685: send slave address, 
	* a pointer to a register, and then the value of the register
	*/
	twi_init(&TWIC);
	// Start
	i2c_startWrite(&TWIC, address);	
	// Mode 1 address
	i2c_write(&TWIC, MODE1);		
	// Setting mode to sleep so we can change the default PWM frequency
	i2c_write(&TWIC, 0b00110001);
	// Stop
	i2c_stop(&TWIC);
	// Required 50 us delay	
	_delay_us(50);
	// Start
	i2c_startWrite(&TWIC, address);	
	// PWM frequency PRE_SCALE address
	i2c_write(&TWIC, 0xfe);		
	//The PWM frequency is set here. Set to 0x79 for 20ms period, or 0x23 for 5.4ms period
	i2c_write(&TWIC, 0x23);	// Value = 25000000/(4096*frequency)-1
	// Stop
	i2c_stop(&TWIC);
	// Delay at least 500 us	
	_delay_us(500);	
	// Start
	i2c_startWrite(&TWIC, address);
	// Mode 1 register address
	i2c_write(&TWIC, MODE1);
	// Set to our preferred mode[ Reset, INT_CLK, Auto-Increment, Normal Mode]	
	i2c_write(&TWIC, 0xa1);
	// Stop
	i2c_stop(&TWIC);
	// Delay at least 500 us	
	_delay_us(500);	
	// Start
	i2c_startWrite(&TWIC, address);
	// Mode2 register address
	i2c_write(&TWIC, MODE2);
	// Set to our preferred mode: output logic state not inverted, outputs change on STOP,
	// totem pole structure, when OE = 1 (output drivers not enabled), SERVOn = 0
	i2c_write(&TWIC, 0b00000100);	
	// Stop
	i2c_stop(&TWIC);	
}

/* 
* Sends the 12 bit PWM data to the register
* Input: write address of the PCA9685, 0 to 4095 pwm value, 0 to 15 for servo channel
*/
void pca9685_send(uint8_t address, uint16_t value, uint8_t servo)
{	
	// temp variable for PWM
	uint8_t pwm;
	// fully on if larger than 4095	
	if(value > 4095) {						
		value = 4095;
	}
	// Start
	i2c_startWrite(&TWIC, address);	
	// Select the correct servo address
	i2c_write(&TWIC, SERVO0 + 4 * servo);
	// Servo on-time low byte
	i2c_write(&TWIC, 0x00);
	// Servo on-time high byte	
	i2c_write(&TWIC, 0x00);
	// Take a copy of value	
	pwm = value;
	// Servo off-time low byte
	i2c_write(&TWIC, pwm);
	// pwm is 16 bits long; shifts upper 8 to lower 8
	pwm = value>>8;
	// Servo off-time high byte
	i2c_write(&TWIC, pwm);
	// Stop
	i2c_stop(&TWIC);						
}
#endif /* PCA9685_H */