// ICC-AVR application builder : 2007-4-18 ???? 08:56:45
// Target : M16
// Crystal: 11.0592Mhz

#include "twi.h"

//the interface parameter between TWI and program
//receive_data,error_state, which does not need to setup
int receive_data;
char error_state=0;   		  // twi error
//setup the following three parameters according to real request
char slave_address=0x10;      //setup the slave device address£¬Bits 7..1:slave address£¬Bit 0£ºbroadcast
char bit_race=0x30;           // setup the Bit rate of master device£¬SCL=CPU frequency/(16+2*(TWBR)*4TWPS)
int send_data=0x02;     	  // Slave device transmit data to mater slave in slave mode
char receiveflag=0;
int exchangeflag=0;
unsigned char endflag=0;
void port_init(void)
{
 PORTA = 0xFF;
 DDRA  = 0xFF;
 PORTB = 0x00;
 DDRB  = 0xFF;
 PORTC = 0xFF;
 DDRC  = 0xFF;
 PORTD = 0x00;
 DDRD  = 0xFF;
}
void Delay_1ms(void)
{
	unsigned int i;
	for(i = 0;i < (unsigned int)8 * 143 - 2;i ++);
}
//***************************
//delay i ms
//***************************
void Delay_Nms(unsigned int i)
{
	unsigned int j;
	for(j = 0;j < i;j ++)
		Delay_1ms();
}
//UART0 initialisation
// desired baud rate: 9600
// actual: baud rate:9600 (0.0%)
// char size: 8 bit
// parity: Disabled
void uart0_init(void)
{
 UCSRB = 0x00; //disable while setting baud rate
 UCSRA = 0x00;
 UCSRC = 0x86;
 UBRRL = 0x47; //set baud rate lo
 UBRRH = 0x00; //set baud rate hi
 UCSRB = 0x98;
}

#pragma interrupt_handler uart0_rx_isr:12
void USART_Transmit( unsigned char data )
{
	/* wait  */
	while ( !( UCSRA & (1<<UDRE)) );
	/* translate data then transmit */
	UDR = data;
}
#pragma interrupt_handler uart0_rx_isr:12
void uart0_rx_isr(void)
{
	USART_Transmit(UDR);
	endflag=1;
}
//TIMER1 initialize - prescale:8
// WGM: 8) PWM phz freq correct, TOP=ICRn
// desired value: 20mSec
// actual value:  0.998mSec (0.2%)
void timer1_init(void)
{
  TCCR1B=0X00;//stop
  OCR1AH = 0x04;
  OCR1AL = 0x0D;
  OCR1BH = 0x03;
  OCR1BL = 0x92;
  ICR1H  = 0x35;
  ICR1L  = 0xFF;
  TCCR1A=(1<<COM1A1)|(0<<COM1A0)|(1<<COM1B1)|(0<<COM1B0); 
  TCCR1B=(1<<WGM13)|(0<<WGM12)|(0<<CS12)|(1<<CS11)|(0<<CS10);  
}

//************************ PWM generate *********************************************
//******************************************************************************
void Pwm_ControlA(float Angle) 
{  
	OCR1A=(Angle/180)*1382+346;     
	return;
}

//************************ PWM generate *********************************************
//******************************************************************************
void Pwm_ControlB(float Angle) 
{  
	OCR1B=(Angle/180)*1382+346;       
	return;
}

//TIMER0 initialize - prescale:8
// WGM: PWM Phase correct
// desired value: 550uSec
// actual value: 553.385uSec (-0.6%)
void timer0_init(void)
{
 TCCR0 = 0x00; //stop
 TCNT0 = 0x01; //set count
 OCR0  = 255;  //set compare
 TCCR0 = 0x62; //start timer
 PORTD|= BIT(PORTD2);
}

void init_devices(void)
{
 //stop errant interrupts until set up
 CLI(); //disable all interrupts
 port_init();
 uart0_init();
 twi_init();
 timer0_init();
 timer1_init();
 MCUCR = 0x00;
 GICR  = 0x00;
 TIMSK = 0x00; //timer interrupt sources
 SEI(); //re-enable interrupts
}

void main(void)
{
	int i;
	int yout1;
	int yout2;
	long int a1;
	long int a2;
	int a3;
	float j;
	float firstdata1;
	float firstdata2;
	float angle_joint1;
	float angle_joint2;
	init_devices();
	//Delay_Nms(1000);
	Pwm_ControlA(92);
	Pwm_ControlB(94);
	Delay_Nms(5000);
	receiveflag=1;
	firstdata1=92;
	firstdata2=94;
	while(1)
	{
		        while(receiveflag==0);//Wait for TWI being addressed
				/*Release attachment module to off the contact surface*/
			    PORTD &= ~BIT(PORTD2);
			    OCR0 = 0;  //set compare
			    TCCR0 = 0x62; //start timer
				Delay_Nms(2600);  //Delay to wait for the finish of releasing the attachment module
	
				twiWriteByte(0x20,0xF1);        //Joint module 3 are addressed to  begin the first step of inchworm
				USART_Transmit(error_state);    //Monitor the TWI state through seial port
				angle_joint1=60;                                         //The angle of joint module 1
				angle_joint1=(angle_joint1-firstdata1)/30;               //Servo motor rotate to the given position by 30 differential little angles
				angle_joint2=140;                                        //The angle of joint module 2
				angle_joint2=(angle_joint2-firstdata2)/30;               //Servo motor rotate to the given position by 30 differential little angles
				PORTA=0xBF; //LED D3 light, which means inchworm begin to impact
				for(i=1;i<=30;i++)
				{
					Pwm_ControlA(firstdata1+i*angle_joint1);
					Pwm_ControlB(firstdata2+i*angle_joint2);      
					Delay_Nms(40);              //Delay to ensure that the servo motor rotate to the given position
				}
				
				 /*Attachment module attach to contact surface*/
			    PORTD |= BIT(PORTD2);
			    OCR0 = 255;  //set compare
			    TCCR0 = 0x62; //start time
				Delay_Nms(2500);  //Delay to ensure the attachment module attach to contact surface
				//Following program means three joint modules rotate cooperatly. put down the tail throuth impacting 
				
				twiWriteByte(0x20,0xF2);        //Joint module 3 is addressed to  begin the second step of inchworm
				USART_Transmit(error_state);    //Monitor the TWI state through seial port
				angle_joint1=62;
				yout1=angle_joint1;                       //The angle of joint module 1
				Pwm_ControlA(angle_joint1);             //Servo motor rotate quichly to the given position throuth impact
				angle_joint2=154;
				yout2=angle_joint2;                     //The angle of joint module 2
				Pwm_ControlB(angle_joint2);             //Servo motor rotate quichly to the given position throuth impact
				Delay_Nms(500);                 //Delay to ensure the three joint modules complete the movement
				
				while(receive_data!=0xF3);    //Wait for TWI being addressed to complete the third step of inchworm
				receive_data=0; 
				angle_joint1=80;
				angle_joint1=(angle_joint1-yout1)/30;
				angle_joint2=94;
				angle_joint2=(angle_joint2-yout2)/30;
				PORTA=0x3F;                     //LED D3 and D4 light, which means inchworm complete four-link motional gait and go to inchwrom gait
				for(i=1;i<=30;i++)
				{
					Pwm_ControlA(yout1+i*angle_joint1);
					Pwm_ControlB(yout2+i*angle_joint2);
					Delay_Nms(40);
				}
				Delay_Nms(500);
				
                while(receive_data!=0xF4);     //Wait for TWI being addressed to complete the fourth step of inchworm
                receive_data=0;
				angle_joint1=92; 
				Pwm_ControlA(angle_joint1);
				angle_joint2=94;
				Pwm_ControlB(angle_joint2);
				
				PORTC=0xff;                    // LED black out, which means the joint module complete all the movement
				PORTA=0xFF;                        //LED D3 and D4 black out, which means the inchworm complete all the gait
			    receiveflag=0;
		        Delay_Nms(500);
	}
}