void main(void) {
int i;
unsigned char buf[80];
unsigned char str[5];
unsigned char first;
unsigned char second;
str[0]='0';
str[1]='x';
MCU_init_24MHz();
initLCDSPI();
// To display normal text, just enter its ASCII number. A number from 0x00 to 0x07 displays the user defined
// custom character, 0x20 to 0x7F displays the standard set of characters, 0xA0 to 0xFD display characters and
// symbols that are factory-masked on the ST7066U controller. 0xFE is reserved.
// Refer to manual for full character table
str2LCD("Voltage:");
// Have to manually move cursor after 16 characters
// It will eventually move to second line, but it has to go through cursor positions 0x10-0x3F from my understanding
setCursor(0x40); // line 2
ATD_init();
ATDCTL5 = 0x23; // start an A/D conversion, scan, single channel (7)
while(1) {
for(i=0; i<80; i+=8) {
while(!(ATDSTAT0 & 0x80)); // wait until conversion is complete
//ATDCTL5 = 0x07; // start the next A/D conversion (NO NEED in scan mode)
buf[i] = ATDDR0H; // ignore ATDDR0L since the conversion is 8-bit only
buf[i+1] = ATDDR1H;
buf[i+2] = ATDDR2H;
buf[i+3] = ATDDR3H;
buf[i+4] = ATDDR4H;
buf[i+5] = ATDDR5H;
buf[i+6] = ATDDR6H;
buf[i+7] = ATDDR7H;
}
first=buf[0]>>4;
second=buf[0]&0x0F;
if (first > 0x09) {
str[2]=first+'A'-10;
} else {
str[2]=first+'0';
}
if (second > 0x09) {
str[3]=second+'A'-10;
} else {
str[3]=second+'0';
}
str[4]='\0';
clearScreen();
str2LCD("Voltage:");
setCursor(0x40); // line 2
str2LCD(str);
waitus(500000);
} // set up a breakpoint here to observe buf[] values
}
void main(void)
{
unsigned char index[3];
unsigned int position=0;
int error=0;
//Declaration related to PID control systems.
int iterm=0;
unsigned int servo_output=1500;
float correction;
Leds_and_Switches_Init();
ISR_init();
ATD_init();
dc_motor_init(); // Current pwm duty is 50
servo_init();
//uart0_9600_init(); //To be removed in the final version of the software.
for(;;)
{
//uart0_tx_string(" :::::::::::::: I'm now in main Loop !! :::::::::::: ");
//This function compensates the sen_data array and puts the result into com_data array
compensate_value(sen_data,com_data);
// Placing it here, will increase the lag between value read and action taken.. hence
// making the system unresponsive ...
ATD0CTL5_SCAN=0; //This will start a new conversion ....
if(com_data[0] <=5 && com_data[1] <=5 && com_data[2] <=5 && com_data[3] <=5 && com_data[4] <=5 && com_data[5] <=5 && com_data[6] <=5 && com_data[7] <=5)
{
//The flag (ATDSTAT0_SCF=0;) clears automatically when we access status register and then the result register..
//dc_motor_stop();
}
/* else if(com_data[0] >=240 && com_data[1] >=240 && com_data[2] >=240 && com_data[3] >=240 && com_data[4] >=240 && com_data[5] >=240 && com_data[6] >=240)
{
}
*/
else
{
sort_array(com_data,index);
position = (index[1] * 512) + (com_data[0]-com_data[2]) ;
error = 1536 - position;
//PID
// limits bound on iterm.
if (iterm>=30000) //32767 is max limit of int
{
//To buffer the fast responses of propotional control, we have the integral control.
iterm = 30000;
LED3=~LED3;
}
else if (iterm<=-30000) //32767 is max limit of int
{
//To buffer the fast responses of propotional control, we have the integral control.
iterm = -30000;
LED4=~LED4;
}
else
{
iterm += error/20 ;
}
correction = (kp*error) + (ki *iterm) ;
//anti wind up circutary
if (correction >= MAX_CORRECTION)
{
correction = MAX_CORRECTION;
}
if (correction <= MIN_CORRECTION)
{
correction = MIN_CORRECTION;
}
// -ve correction means -ve error, ie turn right
// +ve correction means +ve error ie turn left.
servo_output = 1500-(unsigned int)correction;
servo_set (servo_output);
}
} //for loop end here
}
void main(void)
{
unsigned int position=0;
int error=0,perror=0;
long int iterm=0;
//Declaration related to PID control systems.
unsigned char com_data[7];
unsigned int servo_output=1500;
int correction;
unsigned char lap=0;
unsigned int totalsum;
int start_count=0; //variable to be removed...
Leds_and_Switches_Init();
EEPROM_Init();
//EEPROM_reset();
EEPROM_read();
dc_motor_init(); // Current pwm duty is 85
ISR_init();
ATD_init();
pulse_counter_init();
servo_init();
//uart0_9600_init(); //To be removed in the final version of the software.
// TI1_Enable();
while(1)
{
//This function compensates the sen_data array and puts the result into com_data array.
compensate_value(sen_data,com_data);
// Placing it here, will increase the lag between value read and action taken.. hence
// making the system unresponsive ...
ATD0CTL5_SCAN=0; //This will start a new conversion ....
//START STOP DETEECTION
//119 15 116 221 134 52 189
totalsum = com_data[4]+com_data[5]+com_data[6]+com_data[0]+com_data[1]+com_data[2]+com_data[3];
if(totalsum>START_OR_CROSS && totalsum<SURE_START) //either CROSS or start ...
{
//SUM OF SENSORS IS LARGE OS EITHER A CROSS OR START..
//LED1=LED_ON ;
if(start_count>delay_dc)
{
if(lap>=2)
{
dc_motor_stop();
//TSCR1_TEN = 0; // Stop timer
//dc_newval_flag=0;
}
else
{
lap++;
}
}
}
// CAN BE COMMENTED IN FINAL VERSION OF CODE
else if(totalsum<ALL_WHITE)
{
//LED2=LED_ON ;
//do nothing..
}
else
{
//LED3=LED_ON ;
start_count=0; //for start/cross detection
sort_array(com_data);
/*To find the correction, we have three branches .... */
//GUARD 1
if(index ==0 && com_data[index]<GUARD_VAL)
{
//LEFT
//maore the 1500
correction =MAX_CORRECTION;
//LED4=LED_ON;
}
//GUARD 2
else if(index==6 && com_data[index]<GUARD_VAL)
{
//RIGHT
// less than 1500
correction =MIN_CORRECTION;
//LED4=LED_ON;
}
else
{
//LED3=LED_OFF;
position = (index << 9) + (com_data[index+1]-com_data[index-1]) ;
perror=error; //stores previous error
error = 1536 - position;
//PID
correction = (int)((int)((kp*error)/DIV100) + (int)((ki *iterm)/DIV1000) + (int)(kd*(error-perror)/DIV1000));
//anti wind up circutary
if (correction >= MAX_CORRECTION)
{
correction = MAX_CORRECTION;
}
else if (correction <= MIN_CORRECTION)
{
correction = MIN_CORRECTION;
}
else
{
// limits bound on iterm.
if (iterm>=ITERM_LIMIT)
{
//To buffer the fast responses of propotional control, we have the integral control.
iterm = ITERM_LIMIT;
//LED4=~LED4;
}
else if (iterm<=-ITERM_LIMIT)
{
//To buffer the fast responses of propotional control, we have the integral control.
iterm = -ITERM_LIMIT;
//LED4=~LED4;
}
else
{
iterm += error/20;
}
}
}
servo_output = (unsigned int)(SET_POINT-correction);
servo_set(servo_output);
}
}
}
void main(void)
{
Leds_and_Switches_Init();
EEPROM_Init();
//EEPROM_reset();
EEPROM_read();
dc_motor_init(); // Current pwm duty is 85
ISR_init();
ATD_init();
servo_init();
while(1)
{
while(START)
{
//This function compensates the sen_data array and puts the result into com_data array.
compensate_value(sen_data,com_data);
// Placing it here, will increase the lag between value read and action taken.. hence
// making the system unresponsive ...
ATD0CTL5_SCAN=0; //This will start a new conversion ....
//START STOP DETEECTION
//119 15 116 221 134 52 189
totalsum = com_data[4]+com_data[5]+com_data[6]+com_data[0]+com_data[1]+com_data[2]+com_data[3];
if(totalsum>START_OR_CROSS && totalsum<SURE_START) //either CROSS or start ...
{
//SUM OF SENSORS IS LARGE OS EITHER A CROSS OR START..
//LED1=LED_ON ;
if(start_count>delay_dc) //&& correction < CORR_LIMIT) GOT THE LOGIC
{
if(lap>=2)
{
//START=0;
//dc_motor_stop();
//TSCR1_TEN = 0; // Stop timer
//dc_newval_flag=0;
}
else
{
// lap++;
}
}
else
{
start_count++;
}
}
// CAN BE COMMENTED IN FINAL VERSION OF CODE
else if(totalsum<ALL_WHITE)
{
//LED2=LED_ON ;
//do nothing..
}
else
{
//LED3=LED_ON ;
start_count=0; //for start/cross detection
sort_array(com_data);
/*To find the correction, we have three branches .... */
//GUARD 1
/* if(index ==0 && com_data[index]<GUARD_VAL)
{
//LEFT
//maore the 1500
correction =MAX_CORRECTION;
//LED4=LED_ON;
}
//GUARD 2
else if(index==6 && com_data[index]<GUARD_VAL)
{
//RIGHT
// less than 1500
correction =MIN_CORRECTION;
//LED4=LED_ON;
}
else */
{
//LED3=LED_OFF;
if(index==0)
{
//position = 0; //as com_data[0] > com_data[1]
position = 2 *com_data[1];
}
else if (index==6)
{
//position = 3072; //as com_data[] > com_data[]
position = 3072 - 2*com_data[5];
}
else
{
position = (index << 9) + (com_data[index+1]-com_data[index-1]);
}
perror=error; //stores previous error
error = (long)1536 - (long)position;
//PID
correction = (int)((int)((kp*error)/DIV1000) + (int)((ki *iterm)/DIV10000) + (int)(kd*(error-perror)/DIV10000));
//anti wind up circutary
if (correction >= MAX_CORRECTION)
{
correction = MAX_CORRECTION;
}
else if (correction <= MIN_CORRECTION)
{
correction = MIN_CORRECTION;
}
else
{
// limits bound on iterm.
if ((iterm+error)>=ITERM_LIMIT)
{
//To buffer the fast responses of propotional control, we have the integral control.
iterm = ITERM_LIMIT;
//LED4=~LED4;
}
else if ((iterm+error)<=-ITERM_LIMIT)
{
//To buffer the fast responses of propotional control, we have the integral control.
iterm = -ITERM_LIMIT;
//LED4=~LED4;
}
else
{
if ( icount <200)
{
iterm += error/20;
icount ++;
}
else
{
icount=0;
if(error < RESET_ITERM || (-error) > RESET_ITERM )
{
iterm=0;
}
}
}
}
}
}
/* PID of dc motor should be applied based on the result of the pid of SERVO ie ""correction""
As the frequency of action of DC PID is slow,, 65 ms, so the lag is enough to drive the car out of track..
DIRECT ACCESS TO DC MOTOR SHOULD BE MADE .. while settiing thr """setpoint"""
*/
// correction will be handled from the guards also...
if(correction > SHARP_TURN || (-correction) > SHARP_TURN)
{
//dc_motor_speed(10);
//setpoint=2;
//kp=8;
}
else if ((correction>MIDDLE_TURN && correction <SHARP_TURN) || (correction < -MIDDLE_TURN && correction >-SHARP_TURN))
{
//dc_motor_speed(20);
//setpoint=3;
//kp=12;
}
else
{
//dc_motor_speed(50);
//setpoint =4;
}
servo_output = (unsigned int)(SET_POINT-correction);
servo_set(servo_output);
}
}
}
void main(void)
{
unsigned char index;
unsigned int position=0;
int error=0;
//Declaration related to PID control systems.
int iterm=0;
unsigned int servo_output=1500;
float correction;
int i_stop;
unsigned int sumleft,sumright,totalsum;
int start_count=0; //variable to be removed...
Leds_and_Switches_Init();
IEE1_Init();
//EEPROM_reset();
EEPROM_read();
ISR_init();
ATD_init();
dc_motor_init(); // Current pwm duty is 85
servo_init();
//uart0_9600_init(); //To be removed in the final version of the software.
for(;;)
{
//uart0_tx_string(" :::::::::::::: I'm now in main Loop !! :::::::::::: ");
//This function compensates the sen_data array and puts the result into com_data array
compensate_value(sen_data,com_data);
// Placing it here, will increase the lag between value read and action taken.. hence
// making the system unresponsive ...
ATD0CTL5_SCAN=0; //This will start a new conversion ....
//START STOP DETEECTION
//119 15 116 221 134 52 189
sumleft = com_data[0]+com_data[1]+com_data[2];
sumright = com_data[4]+com_data[5]+com_data[6];
totalsum = sumleft+sumright+com_data[3];
if(totalsum>START_CROSS) //either cross or start ...
{
//either start or cross
//if((sumright-sumleft) < CUT_OFF_LR && sumright-sumleft>0 || (sumleft-sumright) < CUT_OFF_LR && sumright-sumleft>0)
{
// either 90 cross detected or satrt
if(totalsum<START_CROSS_SURE_START)
{
// Sure start
start_count ++;
if(start_count>delay_dc)
{
dc_motor_stop();
}
}
else
{
LED1=LED_ON ;
start_count=0;
//sure 90 deg cross
//DO NOTHING...
}
}
//else
{
//Sure non 90 cross
//DO NOTHING
}
}
else if(totalsum<ALL_WHITE)
{
i_stop++;
if(i_stop>=100) //STOP TIME
{
dc_motor_stop();
}
}
else
{
start_count=0;
index=sort_array(com_data);
i_stop=0; //To be removed in the final versions of the code
/*
if(com_data[index]<15)
{
//aa white
}
*/
//GUARD 1
if(index ==0 && com_data[index]<GUARD_VAL)
{
servo_set (1500+MIN_CORRECTION);
LED3=LED_ON;
}
//GUARD 2
else if(index==6 && com_data[index]<GUARD_VAL)
{
servo_set (1500+MAX_CORRECTION);
LED3=LED_ON;
}
else
{
//LED3=LED_OFF;
position = (index * 512) + (com_data[index+1]-com_data[index-1]) ;
error = 1536 - position;
//PID
/*
Done 10000 from 30000 because iterm once saturates becuse unhanlable .
*/
// limits bound on iterm.
if (iterm>=ITERM_LIMIT) //32767 is max limit of int
{
//To buffer the fast responses of propotional control, we have the integral control.
iterm = ITERM_LIMIT;
//LED4=~LED4;
}
else if (iterm<=-ITERM_LIMIT) //32767 is max limit of int
{
//To buffer the fast responses of propotional control, we have the integral control.
iterm = -ITERM_LIMIT;
//LED4=~LED4;
}
else
{
iterm += error/30;
}
correction = (kp*error) + (ki *iterm);
//anti wind up circutary
if (correction >= MAX_CORRECTION)
{
correction = MAX_CORRECTION;
}
if (correction <= MIN_CORRECTION)
{
correction = MIN_CORRECTION;
}
// 0 1 .... 5 6
// -ve correction means -ve error (position-6) ie turn right
// +ve correction means +ve error (position-0) ie turn left.
servo_output = SET_POINT-(unsigned int)correction;
servo_set (servo_output);
}
}
}
}
void main(void)
{
unsigned int position=0;
int error=0,perror=0;
long int iterm=0;
//Declaration related to PID control systems.
unsigned char com_data[7];
unsigned int servo_output=1500;
int correction;
unsigned char lap=0;
unsigned int totalsum;
int start_count=0; //variable to be removed...
// DC pid control variables
static int error_dc,perror_dc=0; //error can be +ve or -ve
int correction_dc=0; //+ve or -ve
unsigned char speed_dc = 40;
unsigned char count=0;
Leds_and_Switches_Init();
EEPROM_Init();
//EEPROM_reset();
EEPROM_read();
dc_motor_init(); // Current pwm duty is 85
ISR_init();
ATD_init();
pulse_counter_init();
servo_init();
//uart0_9600_init(); //To be removed in the final version of the software.
TI1_Enable();
while(1)
{
//This function compensates the sen_data array and puts the result into com_data array.
compensate_value(sen_data,com_data);
// Placing it here, will increase the lag between value read and action taken.. hence
// making the system unresponsive ...
ATD0CTL5_SCAN=0; //This will start a new conversion ....
// CODE FOR DC_PID CONTROL..
if(dc_newval_flag)
{
dc_newval_flag=0;
perror_dc=error_dc;
error_dc = setpoint - pulse_count;
correction_dc =(int) (((KP_DC*error_dc )/DIV10)+ (KD_DC*(error_dc-perror_dc)/DIV10));
if(speed_dc + correction_dc>=200)
{
speed_dc=200;
}
else if(speed_dc + correction_dc <=0)
{
speed_dc =0;
}
else
{
// A type of integrator
speed_dc +=correction_dc;
}
dc_motor_speed(speed_dc);
}
//START STOP DETEECTION
//119 15 116 221 134 52 189
totalsum = com_data[4]+com_data[5]+com_data[6]+com_data[0]+com_data[1]+com_data[2]+com_data[3];
if(totalsum>START_OR_CROSS && totalsum<SURE_START) //either CROSS or start ...
{
//SUM OF SENSORS IS LARGE OS EITHER A CROSS OR START..
//LED1=LED_ON ;
if(start_count>delay_dc)
{
if(lap>=2)
{
dc_motor_stop();
//TSCR1_TEN = 0; // Stop timer
//dc_newval_flag=0;
}
else
{
lap++;
}
}
}
// CAN BE COMMENTED IN FINAL VERSION OF CODE
else if(totalsum<ALL_WHITE)
{
//LED2=LED_ON ;
//do nothing..
}
else
{
//LED3=LED_ON ;
start_count=0; //for start/cross detection
sort_array(com_data);
/*To find the correction, we have three branches .... */
//GUARD 1
if(index ==0 && com_data[index]<GUARD_VAL)
{
//LEFT
//maore the 1500
correction =MAX_CORRECTION;
//LED4=LED_ON;
}
//GUARD 2
else if(index==6 && com_data[index]<GUARD_VAL)
{
//RIGHT
// less than 1500
correction =MIN_CORRECTION;
//LED4=LED_ON;
}
else
{
//LED3=LED_OFF;
position = (index << 9) + (com_data[index+1]-com_data[index-1]) ;
perror=error; //stores previous error
error = 1536 - position;
//PID
correction = (int)((int)((kp*error)/DIV100) + (int)((ki *iterm)/DIV1000) + (int)(kd*(error-perror)/DIV1000));
//anti wind up circutary
if (correction >= MAX_CORRECTION)
{
correction = MAX_CORRECTION;
}
else if (correction <= MIN_CORRECTION)
{
correction = MIN_CORRECTION;
}
else
{
// limits bound on iterm.
if (iterm>=ITERM_LIMIT)
{
//To buffer the fast responses of propotional control, we have the integral control.
iterm = ITERM_LIMIT;
//LED4=~LED4;
}
else if (iterm<=-ITERM_LIMIT)
{
//To buffer the fast responses of propotional control, we have the integral control.
iterm = -ITERM_LIMIT;
//LED4=~LED4;
}
else
{
iterm += error/20;
}
}
}
/* PID of dc motor should be applied based on the result of the pid of SERVO ie ""correction""
As the frequency of action of DC PID is slow,, 65 ms, so the lag is enough to drive the car out of track..
DIRECT ACCESS TO DC MOTOR SHOULD BE MADE .. while settiing thr """setpoint"""
*/
// correction will be handled from the guards also...
if(correction > SHARP_TURN || (-correction) > SHARP_TURN)
{
//dc_motor_speed(10);
setpoint=2;
}
else if ((correction>MIDDLE_TURN && correction <SHARP_TURN) || (correction < -MIDDLE_TURN && correction >-SHARP_TURN))
{
//dc_motor_speed(20);
setpoint=3;
}
else
{
//dc_motor_speed(50);
setpoint =4;
}
servo_output = (unsigned int)(SET_POINT-correction);
servo_set(servo_output);
}
}
}
