/*  Tiptoes Servo Code Version 0.3_rue Aug 8 2006 
	Tiptoes is a semiautonomous 6 legged hexapod that walks with the use
	of 3 servos Rues almost total rewrite

				ATmega32 - 40 PIN DIP LAYOUT
                   --------             
		DTMF BIT 0 -> [ 1]PB0		PA0[40] -> HEAD SERVO
		DTMF BIT 1 -> [ 2]PB1		PA1[39] -> LEFT LEG SERVO
		DTMF BIT 2 -> [ 3]PB2		PA2[38] -> RIGHT LEG SERVO
		DTMF BIT 3 -> [ 4]PB3		PA3[37] -> CENTER SERVO
	      LEFT EYE LED <- [ 5]PB4		PA4[36] -> ARM SERVO
	     RIGHT EYE LED <- [ 6]PB5		PA5[35] -> GRIPPER SERVO
		IR EMITTER <- [ 7]PB6		PA6[34] <- HEAD LDR
	       IR RECEIVER -> [ 8]PB7		PA7[33] <- TAIL LDR
	       RESET/TRAIN -> [ 9]RST	       AREF[32] <- +5V VCC
	  	    5V VCC <- [10]VCC		GND[31] <- GROUND
		    GROUND -> [11]GND	       AVCC[30] <- +5V VCC
	       16 MHZ XTAL -> [12]XTL1		PC7[29] -> LCD DATA 7
	       16 MHZ XTAL -> [13]XTL2		PC6[28] -> LCD DATA 6
		   RWS-433 -> [14]PD0		PC5[27] -> LCD DATA 5
		   TWS-434 <- [15]PD1		PC4[26] -> LCD DATA 4
		not connected [16]PD2		PC3[25] -> LCD E  
		not connected [17]PD3		PC2[24] <- LCD RS 
	 REAR RIGHT FEELER -> [18]PD4		PC1[23] -> LCD RW 
 	  REAR LEFT FEELER -> [19]PD5		PC0[22]    not connected
	FRONT RIGHT FEELER -> [20]PD6		PD7[21] <- FRONT LEFT FEELER 
                 
                 PA0-Head 
                 
                 PA1-center 
                 PA6-LEft 
                 PA7-Right

*/

//-----| Include Files |-------

#include <avr/io.h>		
#include <avr/signal.h>	
#include <avr/interrupt.h>	
#include <string.h>
#include <stdio.h>
#include "global.h"			
#include "timer.h"		
#include "servo.h"		
#include "walkforwardT.inc"

#define OUTPUT             1
#define INPUT              0

#define SIGN(x)           (x)==0?0:(x)>0?1:-1
#define CMP(x,y)          (x)==(y)?0:(x)>(y)?1:-1

//--| function declarations |--
	
void servoSetup( void );
void servoUpdate( void );

//--|data types |--


//--| macros |--

#define flipcoin()   coin++


#define Heads()      (coin & 0x01)

#define leftOffset    25+128
#define rightOffset   0+128
#define rockOffset    55+128

#define acceleration  4

#define speed  16384

// preset left servo positions
#define leftLegsCenter()        leftServo.Position=128; servoSetPosition(1, leftServo.Position+leftOffset);
#define leftLegsGo()            servoSetPosition(1, -leftServo.Position+leftOffset);

//preset right servo positions
#define rightLegsCenter()       rightServo.Position=128; servoSetPosition(2, rightServo.Position+rightOffset);
#define rightLegsGo()           servoSetPosition(2, rightServo.Position+rightOffset);

//preset Center Servo Positions
#define rockCenter()            centerServo.Position=128; servoSetPosition(3, centerServo.Position+rockOffset);
#define rockGo()                servoSetPosition(3, -centerServo.Position+rockOffset);


typedef struct Servo_s {
   int    Position;
   int    Target;
   char   Ready;
} Servo_t;
   

//--| global variables |--

char        coin;

Servo_t  leftServo,
         rightServo,
         centerServo;    
        
// --| main |--

int main( void ) {

 int position;
 unsigned long counter, currState;

  timerInit();    // initialize the timer system
  sei();          // enable interrupts
  servoSetup();   // setup servo's
 
  position  = 70;
  currState = 0;  // its not like we have anywhere better to start
  counter   = 65535;
  
  int Targets[] = {-70, 0, 70};
 
  while(1) {
    flipcoin();  // this is used for simple random decisions
    
    if (counter-- == 0) {
      counter = speed;

      if (rightServo.Ready & leftServo.Ready & centerServo.Ready) {	 
         rightServo.Target   = Targets[stateTable[currState].state.rightLegs];          	      
	 leftServo.Target    = Targets[stateTable[currState].state.leftLegs];
         centerServo.Target  = Targets[stateTable[currState].state.middleLegs];
         currState = stateTable[currState].next;
      }
      
      servoUpdate();
      
    }
    
  }

}


// ---| functions |--

void servoUpdate() {

   if (rightServo.Target != rightServo.Position) {
     rightServo.Ready = 0;
     rightServo.Position += CMP(rightServo.Target,rightServo.Position);
     rightLegsGo();
   }else{
     rightServo.Ready = 1;
   }
   
   if (leftServo.Target != leftServo.Position) {
     leftServo.Ready = 0;
     leftServo.Position += CMP(leftServo.Target,leftServo.Position);
     leftLegsGo();
   }else{
     leftServo.Ready = 1;
   }
   
   if (centerServo.Target != centerServo.Position) {
     centerServo.Ready = 0;
     centerServo.Position += CMP(centerServo.Target,centerServo.Position);
     rockGo();
   }else{
     centerServo.Ready = 1;
   }
   
}


void servoSetup(void)	{

	servoInit();
	servoSetChannelIO(1, _SFR_IO_ADDR(PORTA), PA6); //left legs
	servoSetChannelIO(2, _SFR_IO_ADDR(PORTA), PA7); //right legs
	servoSetChannelIO(3, _SFR_IO_ADDR(PORTA), PA1); //center legs

	DDRA =  (INPUT<<PA0)|(OUTPUT<<PA1)|(INPUT<<PA2)|(INPUT<<PA3)|
                (INPUT<<PA4)|(INPUT<<PA5)|(OUTPUT<<PA6)|(OUTPUT<<PA7);
		 
	// enable and disable pullups
	PORTA  = (1<<PA0)|(1<<PA1)|(0<<PA2)|(0<<PA3)|(1<<PA4)|(1<<PA5)|(1<<PA6)|(1<<PA7);
	PORTB |= (1<<PB0)|(1<<PB1)|(1<<PB2)|(1<<PB3);  
	PORTD |= (1<<PD4)|(1<<PD5)|(1<<PD6)|(1<<PD7);

	leftLegsCenter();
	rightLegsCenter();
	rockCenter();	

}