/* Software to go with the 2-motor servowalker */
/* Simply drives 2 servos (front, rear) to different positions.  Alternates. */

#include "servoWalker.h"

void init(void){  
  /* Setup output on the motor control pins */
  DDRB = (1 << FRONT_MOTOR) | (1 << REAR_MOTOR);
    
  /* Set up Timer1 (16bit) to give a pulse every 20ms*/
  /* Using Fast PWM mode -- top in ICR1, match on OCR1A, OCR1B */
  TCCR1A = (1 << COM1A1) | (1 << COM1B1); /* output on pins, p. 98 */
  TCCR1A |= (1 << WGM11);	/* fast PWM, top is ICR1, p. 99 */
  TCCR1B = (1 << WGM13) | (1 << WGM12);	
  TCCR1B |= (1 << CS10);/* no prescaling -- counting in microseconds, p. 100 */

  FRONT_MOTOR_OC_REGISTER = FRONT_CENTER; /* start off centered */
  REAR_MOTOR_OC_REGISTER = REAR_CENTER;
  ICR1 = 20000;			/* TOP value = 20ms */

}

int main(void) {
  init();			/* set up the chip */
  stand();			/* center the legs */
  _delay_ms(1000);		/* pause for a while standing */
  _delay_ms(1000);		
  _delay_ms(1000);
  _delay_ms(1000);
  _delay_ms(1000);

  while(1){
    walkForward(300, 150, 300, 150, 0);	/* then walk! */
  }

}

/* Walking functions */

void turnFront(uint16_t turnPulseWidth, uint16_t millisecondsTurnTime){
  FRONT_MOTOR_OC_REGISTER = turnPulseWidth; /* tell motor where to go */
  _delay_ms(millisecondsTurnTime); /* wait for it to get there */
}

void turnRear(uint16_t turnPulseWidth, uint16_t millisecondsTurnTime){
  REAR_MOTOR_OC_REGISTER = turnPulseWidth;/* tell motor where to go */
  _delay_ms(millisecondsTurnTime);/* wait for it to get there */
}

void stand(void){		/* centers legs for standing */
  turnRear(REAR_CENTER, 200);	/* rear first b/c has tendency to bobble */
  turnFront(FRONT_CENTER, 200);
  turnRear(REAR_CENTER, 100);
}

void walkForward(uint16_t frontRight, uint16_t rearRight, uint16_t frontLeft, uint16_t rearLeft, 
		 uint16_t pause){
  /* Timing (SLEW_SCALE) is ad-hoc.  
     Slew rate depends on how much the engines are loaded: 
     leg geometry, terrain, battery charge, and phase of the moon */
  turnFront(FRONT_CENTER + frontRight, (frontRight + frontLeft)*SLEW_SCALE); 
  _delay_ms(pause);
  turnRear(REAR_CENTER - rearRight, (rearRight + rearLeft)*SLEW_SCALE);
  _delay_ms(pause);
  turnFront(FRONT_CENTER - frontLeft, (frontRight + frontLeft)*SLEW_SCALE); 
  _delay_ms(pause);
  turnRear(REAR_CENTER + rearLeft, (rearRight + rearLeft)*SLEW_SCALE);    
  _delay_ms(pause);
}  

void walkBackward(uint16_t frontRight, uint16_t rearRight, uint16_t frontLeft, uint16_t rearLeft, 
		 uint16_t pause){
   turnFront(FRONT_CENTER + frontRight, (frontRight + frontLeft)*SLEW_SCALE); 
  _delay_ms(pause);
  turnRear(REAR_CENTER + rearRight, (rearRight + rearLeft)*SLEW_SCALE);
  _delay_ms(pause);
  turnFront(FRONT_CENTER - frontLeft, (frontRight + frontLeft)*SLEW_SCALE); 
  _delay_ms(pause);
  turnRear(REAR_CENTER - rearLeft, (rearRight + rearLeft)*SLEW_SCALE);    
  _delay_ms(pause);
}