#include "hardware.h"

// Initialise the hardware
void appInitHardware(void) {
	initHardware();


}
// Initialise the software
TICK_COUNT appInitSoftware(TICK_COUNT loopStart){
	return 0;
}
// This is the main loop
TICK_COUNT appControl(LOOP_COUNT loopCount, TICK_COUNT loopStart) {

	// -------- Start Analogue Input-------
	// Read the Analogue Input and store results
	uint16_t left_sensorval = a2dConvert10bit(left_sensor);
	// Dump out the value
	rprintf("left_sensor: %d\n",left_sensorval);
	// -------- End   Analogue Input-------

	// -------- Start Analogue Input-------
	// Read the Analogue Input and store results
	uint16_t right_sensorval = a2dConvert10bit(right_sensor);
	// Dump out the value
	rprintf("right_sensor: %d\n",right_sensorval);
	// -------- End   Analogue Input-------

	// -------- Start Actuators -------
	// To control your.motors/servos then see actuators.h in the manual	// To retrieve the required speed of left_motor use:
	// DRIVE_SPEED speed=act_getSpeed(left_motor);
	// To set the required speed of left_motor use:
	// act_setSpeed(left_motor,speed);
	// This example will move the motors back and forth using the loopStart time:
	TICK_COUNT ms = loopStart / 1000;		// Get current time in ms
	int16_t now = ms % (TICK_COUNT)10000; 	// 10 sec for a full swing
	if(now >= (int16_t)5000){				// Goes from 0ms...5000ms
		now = (int16_t)10000 - now;			// then 5000ms...0ms
	}
	// Map it into DRIVE_SPEED range
	DRIVE_SPEED speed = interpolate(now, 0, 5000, DRIVE_SPEED_MIN, DRIVE_SPEED_MAX);
	// Set speed for all motors/servos
	act_setSpeed(&left_motor,speed);
	act_setSpeed(&right_motor,speed);
	// -------- End   Actuators -------

	return 0;
}
