#include <wixel.h>
#include <radio_queue.h>

/* PARAMETERS *****************************************************************/

int32 CODE param_report_period_ms = 20;

/* VARIABLES ******************************************************************/
/* Dont think I need this, but want to leave it here just in case

// A big buffer for holding a report.  This allows us to print more than
// 128 bytes at a time to USB.
uint8 XDATA report[1024];

// The length (in bytes) of the report currently in the report buffer.
// If zero, then there is no report in the buffer.
uint16 DATA reportLength = 0;

// The number of bytes of the current report that have already been
// send to the computer over USB.
uint16 DATA reportBytesSent = 0;
*/
uint16 result1, result2, result3, step;

/* FUNCTIONS ******************************************************************/

void updateLeds()
{
    LED_GREEN(1);
    LED_YELLOW(1);
    LED_RED(0);
}

void adcMeasure()
{
	adcSetMillivoltCalibration(adcReadVddMillivolts());
	result1 = adcConvertToMillivolts(adcRead(0)); //Measures the voltage on pin P0_0, used for PWM for motor
	result2 = adcConvertToMillivolts(adcRead(1)); //Measures the voltage on pin P0_1, used for servo rotation
	result3 = adcConvertToMillivolts(adcRead(2)); //Measures the voltage on pin P0_2, used for emergency break
}

void sendAdcVoltage()
{
	uint8 XDATA * packet = radioQueueTxCurrentPacket();
	    if (packet != 0)
	    {
	        packet[0] = 3;   // must not exceed RADIO_QUEUE_PAYLOAD_SIZE
	        packet[1] = result1;
	        packet[2] = result2;
	        packet[3] = result3;
	        radioQueueTxSendPacket();
	    }
}

void main()
{
    systemInit();
    radioQueueInit();
    updateLeds();

    while(1)
    {
        boardService();
        adcMeasure();
        sendAdcVoltage();
    }
}