void init_platform( void )
{
MicoGpioInitialize( (mico_gpio_t)MICO_SYS_LED, OUTPUT_PUSH_PULL );
MicoGpioOutputLow( (mico_gpio_t)MICO_SYS_LED );
MicoGpioInitialize( (mico_gpio_t)MICO_RF_LED, OUTPUT_OPEN_DRAIN_NO_PULL );
MicoGpioOutputHigh( (mico_gpio_t)MICO_RF_LED );
// MicoGpioInitialize((mico_gpio_t)BOOT_SEL, INPUT_PULL_UP);
//MicoGpioInitialize((mico_gpio_t)MFG_SEL, INPUT_PULL_UP);
// Initialise EasyLink buttons
MicoGpioInitialize( (mico_gpio_t)EasyLink_BUTTON, INPUT_PULL_UP );
mico_init_timer(&_button_EL_timer, RestoreDefault_TimeOut, _button_EL_Timeout_handler, NULL);
MicoGpioEnableIRQ( (mico_gpio_t)EasyLink_BUTTON, IRQ_TRIGGER_BOTH_EDGES, _button_EL_irq_handler, NULL );
//MicoFlashInitialize( MICO_SPI_FLASH );
#ifdef USE_MiCOKit_EXT
dc_motor_init( );
dc_motor_set( 0 );
rgb_led_init();
rgb_led_open(0, 0, 0);
#endif
}
OSStatus user_modules_init(void)
{
OSStatus err = kUnknownErr;
// init DC Motor(GPIO)
dc_motor_init();
dc_motor_set(0); // off
// init RGB LED(P9813)
hsb2rgb_led_init();
hsb2rgb_led_open(0, 0, 0); // off
// init OLED
OLED_Init();
//OLED_Clear();
LCD_Clear(0x00);
OLED_ShowString(20,0,"M X C H I P");
OLED_ShowString(20,3,(uint8_t*)DEFAULT_DEVICE_NAME);
OLED_ShowString(0,6,"T: 0C H: 0%");
// init Light sensor(ADC)
light_sensor_init();
// init infrared sensor(ADC)
infrared_reflective_init();
// init user key1 && key2
user_key1_init();
user_key2_init();
err = kNoErr;
return err;
}
void init_platform( void )
{
button_init_t init;
MicoGpioInitialize( (mico_gpio_t)MICO_SYS_LED, OUTPUT_PUSH_PULL );
MicoGpioOutputLow( (mico_gpio_t)MICO_SYS_LED );
MicoGpioInitialize( (mico_gpio_t)MICO_RF_LED, OUTPUT_OPEN_DRAIN_NO_PULL );
MicoGpioOutputHigh( (mico_gpio_t)MICO_RF_LED );
MicoGpioInitialize((mico_gpio_t)BOOT_SEL, INPUT_PULL_UP);
MicoGpioInitialize((mico_gpio_t)MFG_SEL, INPUT_PULL_UP);
init.gpio = EasyLink_BUTTON;
init.pressed_func = PlatformEasyLinkButtonClickedCallback;
init.long_pressed_func = PlatformEasyLinkButtonLongPressedCallback;
init.long_pressed_timeout = 5000;
button_init( IOBUTTON_EASYLINK, init );
#ifdef USE_MiCOKit_EXT
dc_motor_init( );
dc_motor_set( 0 );
rgb_led_init();
rgb_led_open(0, 0, 0);
#endif
}
void init_block(void)
{
g_block_type = CLASS_MOTOR;
g_block_sub_type = BLOCK_SINGLE_DC_MOTOR;
dc_motor_init();
uart0_recv_attach(sysex_process_online, sysex_process_offline);
set_rgb_led(0, 0, 128);
}
void init_platform( void )
{
MicoGpioInitialize( (mico_gpio_t)MICO_SYS_LED, OUTPUT_PUSH_PULL );
MicoGpioOutputLow( (mico_gpio_t)MICO_SYS_LED );
MicoGpioInitialize( (mico_gpio_t)MICO_RF_LED, OUTPUT_OPEN_DRAIN_NO_PULL );
MicoGpioOutputHigh( (mico_gpio_t)MICO_RF_LED );
// Initialise EasyLink buttons
MicoGpioInitialize( (mico_gpio_t)EasyLink_BUTTON, INPUT_HIGH_IMPEDANCE );
mico_init_timer(&_button_EL_timer, RestoreDefault_TimeOut, _button_EL_Timeout_handler, NULL);
MicoGpioEnableIRQ( (mico_gpio_t)EasyLink_BUTTON, IRQ_TRIGGER_BOTH_EDGES, _button_EL_irq_handler, NULL );
#ifdef USE_MiCOKit_EXT
dc_motor_init( );
dc_motor_set( 0 );
#endif
}
void init_platform_bootloader( void )
{
MicoGpioInitialize( (mico_gpio_t)MICO_SYS_LED, OUTPUT_PUSH_PULL );
MicoGpioOutputLow( (mico_gpio_t)MICO_SYS_LED );
MicoGpioInitialize( (mico_gpio_t)MICO_RF_LED, OUTPUT_OPEN_DRAIN_NO_PULL );
MicoGpioOutputHigh( (mico_gpio_t)MICO_RF_LED );
MicoGpioInitialize((mico_gpio_t)BOOT_SEL, INPUT_PULL_UP);
MicoGpioInitialize((mico_gpio_t)MFG_SEL, INPUT_HIGH_IMPEDANCE);
#ifdef USE_MiCOKit_EXT
dc_motor_init( );
dc_motor_set( 0 );
rgb_led_init();
rgb_led_open(0, 0, 0);
#endif
}
void NORETURN dc_motor_testRun(void)
{
dc_motor_init();
/*
* Assign the configuration to motor.
*/
dc_motor_setup(MOTOR, &motor);
while (1)
{
/*
* Using enable and disable
*/
dc_motor_setDir(MOTOR, 1);
dc_motor_setSpeed(MOTOR, 10000);
dc_motor_enable(MOTOR, true);
timer_delay(500);
dc_motor_enable(MOTOR, false);
dc_motor_setDir(MOTOR, 0);
dc_motor_setSpeed(MOTOR, 60000);
dc_motor_enable(MOTOR, true);
timer_delay(150);
dc_motor_enable(MOTOR, false);
/*
* Using timer
*/
dc_motor_setDir(MOTOR, 1);
dc_motor_setSpeed(MOTOR, 60000);
dc_motor_startTimer(MOTOR, 150);
dc_motor_waitStop(MOTOR);
dc_motor_setDir(MOTOR, 0);
dc_motor_setSpeed(MOTOR, 10000);
dc_motor_startTimer(MOTOR, 500);
dc_motor_waitStop(MOTOR);
}
}
//------------------------------------- API ------------------------------------
OSStatus user_modules_init(void)
{
OSStatus err = kUnknownErr;
char oled_show_line[16] = {'\0'}; // max char each line
// init DC Motor(GPIO)
dc_motor_init();
dc_motor_set(0); // off
// init RGB LED(P9813)
rgb_led_init();
rgb_led_open(0, 0, 0); // off
// init OLED
OLED_Init();
OLED_Clear();
snprintf(oled_show_line, 16, "%s", (uint8_t*)DEV_KIT_MANUFACTURER);
OLED_ShowString(0,0,(uint8_t*)oled_show_line);
memset(oled_show_line, '\0', 16);
snprintf(oled_show_line, 16, "%s", (uint8_t*)DEV_KIT_NAME);
OLED_ShowString(0,3,(uint8_t*)oled_show_line);
OLED_ShowString(0,6,"Starting... ");
// init Light sensor(ADC)
light_sensor_init();
// init infrared sensor(ADC)
infrared_reflective_init();
// init user key1 && key2
user_key1_init();
user_key2_init();
err = temp_hum_sensor_init();
return err;
}
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 init_platform_bootloader( void )
{
CRC8_Context crc;
OSStatus err = kNoErr;
mico_logic_partition_t *rf_partition = MicoFlashGetInfo( MICO_PARTITION_RF_FIRMWARE );
MicoGpioInitialize( (mico_gpio_t)MICO_SYS_LED, OUTPUT_PUSH_PULL );
MicoGpioOutputLow( (mico_gpio_t)MICO_SYS_LED );
MicoGpioInitialize( (mico_gpio_t)MICO_RF_LED, OUTPUT_OPEN_DRAIN_NO_PULL );
MicoGpioOutputHigh( (mico_gpio_t)MICO_RF_LED );
MicoGpioInitialize((mico_gpio_t)BOOT_SEL, INPUT_PULL_UP);
MicoGpioInitialize((mico_gpio_t)MFG_SEL, INPUT_PULL_UP);
#ifdef USE_MiCOKit_EXT
dc_motor_init( );
dc_motor_set( 0 );
rgb_led_init();
rgb_led_open(0, 0, 0);
#endif
/* Specific operations used in EMW3165 production */
#define NEED_RF_DRIVER_COPY_BASE ((uint32_t)0x08008000)
#define TEMP_RF_DRIVER_BASE ((uint32_t)0x08040000)
#define TEMP_RF_DRIVER_END ((uint32_t)0x0807FFFF)
const uint8_t isDriverNeedCopy = *(uint8_t *)(NEED_RF_DRIVER_COPY_BASE);
const uint32_t totalLength = rf_partition->partition_length;
const uint8_t crcResult = *(uint8_t *)(TEMP_RF_DRIVER_END);
uint8_t targetCrcResult = 0;
uint32_t copyLength;
uint32_t destStartAddress_tmp = rf_partition->partition_start_addr;
uint32_t sourceStartAddress_tmp = TEMP_RF_DRIVER_BASE;
uint32_t i;
if ( isDriverNeedCopy != 0x0 )
return;
platform_log( "Bootloader start to copy RF driver..." );
/* Copy RF driver to SPI flash */
err = platform_flash_init( &platform_flash_peripherals[ MICO_FLASH_SPI ] );
require_noerr(err, exit);
err = platform_flash_init( &platform_flash_peripherals[ MICO_FLASH_EMBEDDED ] );
require_noerr(err, exit);
err = platform_flash_erase( &platform_flash_peripherals[ MICO_FLASH_SPI ],
rf_partition->partition_start_addr, rf_partition->partition_start_addr + rf_partition->partition_length - 1 );
require_noerr(err, exit);
platform_log( "Time: %d", mico_get_time_no_os() );
for(i = 0; i <= totalLength/SizePerRW; i++){
if( i == totalLength/SizePerRW ){
if(totalLength%SizePerRW)
copyLength = totalLength%SizePerRW;
else
break;
}else{
copyLength = SizePerRW;
}
printf(".");
err = platform_flash_read( &platform_flash_peripherals[ MICO_FLASH_EMBEDDED ], &sourceStartAddress_tmp, data , copyLength );
require_noerr( err, exit );
err = platform_flash_write( &platform_flash_peripherals[ MICO_FLASH_SPI ], &destStartAddress_tmp, data, copyLength );
require_noerr(err, exit);
}
printf("\r\n");
/* Check CRC-8 check-sum */
platform_log( "Bootloader start to verify RF driver..." );
sourceStartAddress_tmp = TEMP_RF_DRIVER_BASE;
destStartAddress_tmp = rf_partition->partition_start_addr;
CRC8_Init( &crc );
for(i = 0; i <= totalLength/SizePerRW; i++){
if( i == totalLength/SizePerRW ){
if(totalLength%SizePerRW)
copyLength = totalLength%SizePerRW;
else
break;
}else{
copyLength = SizePerRW;
}
printf(".");
err = platform_flash_read( &platform_flash_peripherals[ MICO_FLASH_SPI ], &destStartAddress_tmp, data, copyLength );
require_noerr( err, exit );
CRC8_Update( &crc, data, copyLength);
}
CRC8_Final( &crc, &targetCrcResult );
printf("\r\n");
//require_string( crcResult == targetCrcResult, exit, "Check-sum error" );
if( crcResult != targetCrcResult ){
platform_log("Check-sum error");
while(1);
}
/* Clear RF driver from temperary storage */
platform_log("Bootloader start to clear RF driver temporary storage...");
/* Clear copy tag */
err = platform_flash_erase( &platform_flash_peripherals[ MICO_FLASH_EMBEDDED ], NEED_RF_DRIVER_COPY_BASE, NEED_RF_DRIVER_COPY_BASE);
require_noerr(err, exit);
exit:
return;
}
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);
}
}
}
}
//------------------------------------- API ------------------------------------
OSStatus micokit_STmems_init(void)
{
OSStatus err = kUnknownErr;
#if defined(MICOKIT_STMEMS_KEY1)||defined(MICOKIT_STMEMS_KEY2)
button_init_t init;
#endif
//init RGB LED(P9813)
rgb_led_init();
rgb_led_close(); // off
dc_motor_init();
dc_motor_set(0); // off
// init OLED
OLED_Init();
OLED_ShowString(OLED_DISPLAY_COLUMN_START, OLED_DISPLAY_ROW_1, (uint8_t*)MODEL);
OLED_ShowString(OLED_DISPLAY_COLUMN_START, OLED_DISPLAY_ROW_2, (uint8_t*)"MiCO ");
OLED_ShowString(OLED_DISPLAY_COLUMN_START, OLED_DISPLAY_ROW_3, (uint8_t*)" Starting... ");
OLED_ShowString(OLED_DISPLAY_COLUMN_START, OLED_DISPLAY_ROW_4, (uint8_t*)" ");
#ifdef MICOKIT_STMEMS_KEY1
init.gpio = MICOKIT_STMEMS_KEY1;
init.pressed_func = micokit_STmems_key1_clicked_callback;
init.long_pressed_func = NULL;
init.long_pressed_timeout = 5000;
button_init( IOBUTTON_USER_1, init);
#endif
#ifdef MICOKIT_STMEMS_KEY2
init.gpio = MICOKIT_STMEMS_KEY2;
init.pressed_func = micokit_STmems_key2_clicked_callback;
init.long_pressed_func = NULL;
init.long_pressed_timeout = 5000;
button_init( IOBUTTON_USER_2, init);
#endif
/*init HTS221 */
err = hts221_sensor_init();
require_noerr_string( err, exit, "ERROR: Unable to Init HTS221" );
/*init UVIS25 */
err = uvis25_sensor_init();
require_noerr_string( err, exit, "ERROR: Unable to Init UVIS25" );
/*init LSM9DS1_ACC_GYR */
err = lsm9ds1_acc_gyr_sensor_init();
require_noerr_string( err, exit, "ERROR: Unable to Init LSM9DS1_ACC_GYR" );
err = lsm9ds1_mag_sensor_init();
require_noerr_string( err, exit, "ERROR: Unable to Init LSM9DS1_MAG" );
/*init LPS25HB */
err = lps25hb_sensor_init();
require_noerr_string( err, exit, "ERROR: Unable to Init LPS25HB" );
light_sensor_init();
exit:
return err;
}
int
main (void)
{
// This isn't what we're testing exactly, but we need to know if its
// working or not to interpret other results.
term_io_init ();
PFP ("\n");
PFP ("\n");
PFP ("term_io_init() worked.\n");
PFP ("\n");
dc_motor_init ();
PFP ("Finished dc_motor_init().\n");
PFP ("\n");
// Ramp motor speeds up and down continually, measuring load at each step.
// To be cute, motors A and B will be run in opposite directions :)
{
int8_t spd = 0; // Speed setting (for motor A)
int8_t ss = 20; // Step size (in % duty cycle)
int8_t ssgn = 1; // Step sign
double tps = 2000.0; // Time Per Step (in ms)
int8_t max_spd = 100; // Maximum speed setting
int8_t min_spd = -100; // Minimum speed setting (full speed reverse)
for ( ; ; ) {
dc_motor_set_speed (DC_MOTOR_CHANNEL_A, spd);
PFP ("Set motor A speed to %hhi\n", spd);
dc_motor_set_speed (DC_MOTOR_CHANNEL_B, -spd);
PFP ("Set motor B speed to %hhi\n", -spd);
// Run motors at this speed (in opposite directions) for tps
// milliseconds, measuring load current half way through.
_delay_ms (tps / 2.0);
int mapa = 1000; // Milliamps Per Amp
// NOTE: we can't output floating point values using PFP() without
// first SETTING AVRLIBC_PRINTF_LDFLAGS as described in generic.mk.
PFP (
"DC motor A load current: %i mA\n",
(int) (mapa * dc_motor_load_current (DC_MOTOR_CHANNEL_A)) );
PFP (
"DC motor B load current: %i mA\n",
(int) (mapa * dc_motor_load_current (DC_MOTOR_CHANNEL_B)) );
_delay_ms (tps / 2.0);
PFP ("\n");
// Calculate motor speed to set on next iteration
spd += ssgn * ss;
if ( spd > max_spd ) {
spd = max_spd;
ssgn = -1;
}
else if ( spd < min_spd ) {
spd = min_spd;
ssgn = 1;
}
}
}
}
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);
}
}
}
