#include <MINDSi.h>
#include <Servo.h>

/***************************************************
/ MINDS-I Projects. mymindsi.com
/ 
/
/***************************************************/
Servo steer, drive;
int driveb[10], steerb[10];
int driveaverage, steeraverage;
int drivesend, steersend;
int *pdriveb = &driveb[0], psteerb = &steerb[0];


void setup() 
{
  steer.attach(2);  //set pins for steering servo (2)
  drive.attach(3);  // and speed controller to drive motor (3)
}

void loop() {
  
  *psteerb++ = getRadio(2); *pdriveb++ = getRadio(3); //set oldest value to new reading and advance pointers
  //take averages
  steersend = (steerb[0] + steerb[1] + steerb[2] + steerb[3] + steerb[4] + steerb[5] + steerb[6] + steerb[7] + steerb[8] + steerb[9])/10;
  drivesend = (driveb[0] + driveb[1] + driveb[2] + driveb[3] + driveb[4] + driveb[5] + driveb[6] + driveb[7] + driveb[8] + driveb[9])/10;
  
  if(psteerb == &steerb[9]) psteerb = &steerb[0]; //set pointers back to the earliest value once
  if(pdriveb == &driveb[9]) pdriveb = &driveb[0]; //they reach the end of the array

  drivesend -= 90;
  drivesend = drivesend*drivesend*drivesend/729000;
  drivesend += 90;

  steersend -= 90;
  steersend = steersend*steersend*steersend/729000;
  steersend += 90;

  if((drivesend >95 || drivesend < 85)) steer.write(drivesend); //deadband from 85 to 95
  else steer.write(90);  //if its not outside the deadband, stay centered
  if(steersend > 105 || steersend < 78) drive.write(steersend);
  else drive.write(90);
}