Download EE 477 Final Report

ECE 477 Final Report
Spring 2008
return;
}
/**********************
*
* Turn Right Function
* Makes 4,5 go forward
* Makes 6,7 go reverse
*
**********************/
void turn_right ( )
{
PORTC |= (1<<2) | (1<<5);
PORTC &= ~(1<<3) & ~(1<<4);
return;
}
/**********************
*
* Brake Function
*
**********************/
void brake ( )
{
PORTC &= ~(1<<2) & ~(1<<3) & ~(1<<5) & ~(1<<4);
_delay_ms(10);
return;
}
/********************
*
* Update PID function
*
* *****************/
int16_t update_PID(uint16_t heading_desired, uint16_t curr_heading)
{
long int pid;
long int P = 0;
long int D = 0;
long int dt;
char s[200] ={0};
curr_time = (timer_cnt * 65535) + TCNT1;
dt = curr_time - prev_time;
//calculate heading error for proportional
heading_error = (long int)heading_desired - (long int)curr_heading;
P = gain_prop * heading_error;
D = (gain_deriv * (heading_error-prev_heading_error))/dt;
pid = P + D;
_delay_ms(32);
snprintf(s, 200, "d=%4d h=%4d head_err=%8ld prev_head_err=%8ld dt=%8ld P=%8ld
D=%8ld\r",heading_desired, curr_heading, heading_error, prev_heading_error, dt, P,D);
put_str(s);
prev_heading_error = heading_error;
F-139