void main(void)
{
DisableInterrupts;
PLL_Init_64M();
UART_Init();
Init_Port();
Init_AD();
Init_PWM();
// LCD_init();
// LCD_clear();
// RST = 1;
PAT_Init();
// PWMDTY23 =6300 ;
// DDRA_DDRA7 = 1;
EnableInterrupts;
for(;;)
{
Mode();
// uart_putchar('a');
/* OutData[0] = (int16)(roadflag[2]);
OutData[1] = (int16)(roadflag[3]);
OutData[2] = (int16)(Steer);
OutData[3] = (int16)(roadflag[1]);
OutPut_Data(); */
// Ramp_Detection();
GetExpectSpeed();
Steer_PID();
stop_car();
//Motor(1800);
}
}
void main(void){
Init();
SCI0BD = 156; // 9600 baud
SCI0CR2 = 0x0C; // enable transmitter and receiver
Init_AD(); // initialize ATD
ATD0CTL5 = 0x20; //one channel, 8 conversions, continuous conversions
// PWMDTY3 = 30; //starting speed - slow
// RTI_count=0;
// RTI_flag=0;
// printf ("\n\r");
while(1) {
conv = ATDDR0H;
// Use printf for debugging
// printf("AtD 0: %d PID: %f PWM: %d \n", ATDDR0H, PID, PWMDTY5); //256 steps, 0 to 5V, 19.53mV/step
// printf("AtD 0: %d, PWM: %d \n", ATDDR0H, PWMDTY5);
/* ----- PID ----- */
//calculate PID terms
error = conv - 75;
for(i = 9; i >= 1; i--) { //recording previous errors
D[i] = D[i-1];
}
D[0] = error;
for(i = 0; i >= 9; i++) { //calculate average error
avg_error += D[i];
}
avg_error = avg_error / 10;
P_term = Kp * (error); //calculate PID terms
I_term = (Kp / Ti) * (avg_error);
D_term = Kp * Td *(error - avg_error);
PID = (P_term + D_term + I_term);
/* Code For Steering */
turn = conv + PID;
// if(((turn)*(-0.186)+(52.24)) > maxServo)
// PWMDTY5 = maxServo;
// else if(((turn)*(-0.186)+(52.24)) < minServo)
// PWMDTY5 = minServo;
// else
// PWMDTY5 = ((turn) * (-0.186) + (52.24));
if((turn) <= 75) //right turn
if(((turn)*(-0.197)+(51.77)) > maxServo)
PWMDTY5 = maxServo;
else if(((turn)*(-0.197)+(51.77)) < minServo)
PWMDTY5 = minServo;
else
PWMDTY5 = ((turn) * (-0.197)+(51.77));
if((turn) > 75) //left turn
if(((turn)*(-0.1846)+(50.846)) > maxServo)
PWMDTY5 = maxServo;
else if(((turn)*(-0.1846)+(50.846)) < minServo)
PWMDTY5 = minServo;
else
PWMDTY5 = ((turn) * (-0.1846) + (50.846));
/* Code For Motor Control */
if(flag)
{
if(PulseCount == 0)
PWMDTY3 = 20;
else if(PulseCount < SET_COUNT)
PWMDTY3 = PWMDTY3 + 1;
else if(PulseCount > SET_COUNT)
PWMDTY3 = PWMDTY3 - 1;
flag = 0;
PulseCount = 0;
}
} // while(1)
}
