/*******************************************************************************
* Function: pcu_dma_init
*
* Parameters:
* mtd: IN: mtd_info structure
* pcu_dmaConf: IN: pcu_dma configuration enum
*
* Description:
* Depending on the pcu_dma configuration enum value, this function initializes
* the PCU_DMA itself, as well as sets the function pointers that are to be
* used by brcmnand35xxx_page to access the device.
*
* Returns: nothing
*******************************************************************************/
void pcu_dma_init(struct mtd_info* mtd, pcu_dma_configuration_t pcu_dmaConf)
{
struct brcmnand35xxx_chip *this = mtd->priv;
pcu_dmaData.mtd = mtd;
// Clear the interrupt for next time
PFLASH_PCU_DMA_INTR2_CLEAR();
pcu_dma_buf = kmalloc (mtd->writesize, GFP_KERNEL);
if (!pcu_dma_buf) {
panic("pcu_dmainit: could not allocate buffer memory\n");
}
switch(pcu_dmaConf)
{
case PCU_DMA_TASKLET:
this->pcu_dma_write = pcu_dma_tasklet_write;
this->pcu_dma_read = pcu_dma_tasklet_read;
this->pcu_dma_wait_for_completion = NULL;
this->pcu_dma_read_get_data = pcu_dma_tasklet_read_get_data;
break;
default:
printk(KERN_ERR "%s DMA Mode not supported \n",__FUNCTION__);
break;
}
if (pcu_dmaConf == PCU_DMA_TASKLET)
{
ISR_init();
}
}
void init_main()
{
/*Initialize modules*/
timer_init();
led_init();
/*Initialize peripherals*/
USART_Init(1);
TIMERS_Init();
EEPROM_Init();
ws2812Init();
ledStringInit();
ledEffectInit();
adcInit();
ExtIntInit();
ISR_init();
}
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 app_init() {
ports_init();
ACTOR1_init();
ACTOR2_init();
ISR_init();
}
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);
}
}
}
