/* Driving a (remote control) model car with an EyeBot     */

/* Thomas Braunl, The Univ. of Auckland, New Zealand, 2000 */



#include "eyebot.h"

#define STEER_CHANNEL 2

#define PCONST 1



int steer_angle,steer_current,ad_current;

MotorHandle MSteer;

int adjust;



void IRQSteer();



TimerHandle STEERInit()

{

  MSteer=MOTORInit(RIGHTMOTOR);

  return OSAttachTimer(5,IRQSteer);

}



int STEERRelease(TimerHandle h)

{ int i;

  i=OSDetachTimer(h);

  MOTORRelease(MSteer);

  return i;

}



/* direction: -100 left 0 straight 100 right */

int STEERSet(TimerHandle h, int direction)

{

  if (&h==NULL) return -1;

  steer_angle=direction;

  return 0;

}



/* Control routinue */

/*

void IRQSteer() 

{

 

  

  ad_current=OSGetAD(STEER_CHANNEL);

  steer_current=(ad_current-400)/3-100;

  adjust=(steer_angle-steer_current)*PCONST;

  if (adjust<-100) adjust=-100; else

  if (adjust> 100) adjust= 100;   

  MOTORDrive(MSteer,adjust);

}

*/



void IRQSteer() 

{

  ad_current=OSGetAD(STEER_CHANNEL);

  steer_current=(ad_current-400)/3-100;

  if (steer_angle-steer_current >  10) MOTORDrive(MSteer,  75); else

  if (steer_angle-steer_current < -10) MOTORDrive(MSteer, -75); else

  MOTORDrive(MSteer,0);

}



int main()

{

TimerHandle h;

char z;

int direction;



  LCDMode(SCROLLING|NOCURSOR);

  LCDMenu("<< "," ^ "," >>","End");

  h=STEERInit();



  direction=0;



  do

  {

    z=KEYRead();

    switch (z) 

    {

      case KEY1:direction-=10; if (direction<-100) direction=-100;break;

      case KEY2:direction=0; break;

      case KEY3:direction+=10; if (direction> 100) direction=100; break;

      case KEY4:break;

    }

    STEERSet(h,direction);

    LCDSetPos(0,0);

  

    LCDPrintf("Desire: %4d\n",direction);

    LCDPrintf("Current:%4d\n",steer_current);

    LCDPrintf("AD: %5d\n",ad_current);

    LCDPrintf("Adj: %5d\n",adjust);

  } while (z!=KEY4);

  

  STEERRelease(h);

  return (0);

}