void Setup (void)
{
Menu_EnableAllItems();
Initial_EEPROM_Config_Load();
//Config.QuadFlyingMode = QuadFlyingMode_PLUS;
RX_Init();
if ((Config.RX_mode==RX_mode_SingleMode) && (IS_MISC_SENSOR_SONAR_ENABLED==true))
{
Ultrasonic_Init();
}
// Motors
M1_DIR = OUTPUT;
M2_DIR = OUTPUT;
M3_DIR = OUTPUT;
M4_DIR = OUTPUT;
M1 = 0;
M2 = 0;
M3 = 0;
M4 = 0;
Buzzer_DIR = OUTPUT;
LED_Orange_DIR = OUTPUT;
// Sensors
V_BAT = INPUT;
// Timers
TCCR1A = 0; //Set timer 1 to run at 2.5MHz
TCCR1B = 0;
TCCR1C = 0;
#ifdef TELEMETRY_ENABLED
if (Config.RX_mode==RX_mode_SingleMode)
{
UART_Init(SERIAL_BAUD_RATE); //57600 = 20 115200=10
}
#endif
ADCPort_Init();
Sensors_Init();
KeyBoard_Init();
Timer_Init();
LCD_Init();
LCD_Clear();
Menu_MenuInit();
sei();
//delay_ms(30);
}
void main (void)
{
int reflection,LDs_count,omron_count,angel;
int old_fix,fix,left,right;
unsigned char reflect,status,k;
char old_flag,flag;
WDTCN = 0xDE; // disable watchdog timer
WDTCN = 0xAD;
SYSCLK_Init (); // Initialize system clock to 16MHz
ExtCrystalOsc_Init (); // switch system clock
Port_Init (); // Initialize crossbar and GPIO
SPI0_Init();
LED_Init(); // Initialize OLED
Sensors_Init();
Rudder_Init();
Motor_Init();
Measurement_Init();
Test_Helper_Init();
EA = 1; // Enable global interrupts
CR = 1; // start to output PWM
P31 = 1;
PUT_LINE("OMRON:",omron_count);
PUT_LINE("reflection:",reflection);
PUT_LINE("LDs_count:",LDs_count);
PUT_LINE("Tunnel1:",tunnel_length[0]);
PUT_LINE("Tunnel2:",0);
PUT_LINE("Tunnel3:",0);
//PUT_LINE("angel:",angel);
//PUT_LINE("fix:",fix);
while (1) { // Spin forever
DIRECTION = 0;
PCA0CPH1 = 0xE0;
flag = 0;
while (1) {
// dectecting
reflect = get_LD_reflection();
old_flag = flag;
flag = 0;
// conner emergency
if(reflect < 25){
if(!LEFT3 && RIGHT3)
flag = 1;
if(!RIGHT3 && LEFT3)
flag = -1;
if(flag)
break;
}
// go straight forward:
if(!LEFT1 || !LEFT2 || !LEFT3)
flag = 1;
if(!RIGHT1 && LEFT2)
flag = -1;
if(!RIGHT2 && LEFT3)
flag = -1;
if(!RIGHT3)
flag = -1;
k = 7;
if(reflect > 50){
k = 10;
}
if(reflect > 85){
if(flag > 0)
if(++left > 32766)
{
left = left - right;
right = 0;
}
if(flag < 0)
if(++right > 32766)
{
right = right - left;
left = 0;
}
old_fix = fix;
fix = left - right;
if(fix > 20)
fix = 20;
if(fix < -20)
fix = -20;
fix = ((fix + RUDDER_FIX) + old_fix)/2;
}
//UPDATE_VALUE(5,fix);
if(MAX_REFLECTION > reflect)
angel = MAX_REFLECTION - reflect;
else
angel = 0;
angel = flag * k * angel / 10 + fix;
//UPDATE_VALUE(4,angel);
set_angel(angel);
// debug info
omron_count = Get_OMRON_Count();
reflection = get_LD_reflection();
LDs_count = TL1 + LD2_count;
UPDATE_VALUE(0,omron_count);
UPDATE_VALUE(1,reflection);
UPDATE_VALUE(2,LDs_count/3);
tmp = tunnel_length[1] * 464 / 1683;
UPDATE_VALUE(3,tmp);
tmp = tunnel_length[2] * 464 / 1683;
UPDATE_VALUE(4,tmp);
tmp = tunnel_length[3] * 464 / 1683;
UPDATE_VALUE(5,tmp);
}
reflect = get_LD_reflection();
if(reflect < 10){ // 60 degree
PCA0CPH1 = 0xB8;
set_angel(flag * 127);
Clear_OMRON_Count();
while(omron_count <= 300);
while(1){
if(flag > 0 && !LEFT1) {
while(!RIGHT1);
break;
}
if(flag < 0 && !RIGHT1) {
while(!LEFT1);
break;
}
}
}else{
// 90 degree
DIRECTION = 1;
PCA0CPH1 = 0x38;
Clear_OMRON_Count();
set_angel(-flag * 127);
while(1){
omron_count = Get_OMRON_Count();
UPDATE_VALUE(0,omron_count);
if(omron_count >= 410) //1062
break;
}
DIRECTION = 0;
PCA0CPH1 = 0xB8;
//Clear_OMRON_Count();
set_angel(flag * 127);
// escape from conner
while(1){
if(flag > 0 && !LEFT1){
while(!RIGHT1);
break;
}
if(flag < 0 && !RIGHT1){
while(!LEFT1);
break;
} // if
} // while
} // if else: 60 or 90 degree
} // main loop
} // main
/*******************************************************************************//**
* @implements InitComponents
**********************************************************************************/
RESULT InitComponents(void)
{
// init guard
if(Guard_Init()==FAIL)
return FAIL;
// init utils
if(Utils_Init()==FAIL)
return FAIL;
// init MCU
if(MCU_Init()==FAIL)
return FAIL;
// init scheduler
if(Scheduler_Init()==FAIL)
return FAIL;
// init power management system
#ifdef USE_PWR
if(PWR_Init()==FAIL)
return FAIL;
#endif
// init timers
#ifdef USE_TIMERS
if(Timers_Init()==FAIL)
return FAIL;
#endif
// init LEDs
#ifdef USE_LEDS
if(LEDs_Init()==FAIL)
return FAIL;
#endif
// init buttons
#ifdef USE_BUTTONS
if(Buttons_Init()==FAIL)
return FAIL;
#endif
// init UART
#ifdef USE_UART
if(UART_Init()==FAIL)
return FAIL;
#endif
// init SPI
#ifdef USE_SPI
if(SPI_Init()==FAIL)
return FAIL;
#endif
// init TWI
#ifdef USE_TWI
if(TWI_Init()==FAIL)
return FAIL;
#endif
// init OWI
#ifdef USE_OWI
if(OWI_Init()==FAIL)
return FAIL;
#endif
// init other components
if(InitOther()==FAIL)
return FAIL;
// init sensors
#ifdef USE_SENSORS
if(Sensors_Init()==FAIL)
return FAIL;
#endif
// init NWK
#ifdef USE_NWK
if(NWKLayer_Init()==FAIL)
return FAIL;
#endif
// return success
return SUCCESS;
}
int main()
{
RCC_ClocksTypeDef RCC_Clocks;
GPIO_InitTypeDef GPIO_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
int rf_len = 0;
int rs485_len = 0;
#if DEBUG
// int usart_len = 0;
// char buff_usart[BUFFER_SIZE];
#endif
char buff_rf[BUFFER_SIZE];
char buff_rs485[BUFFER_SIZE];
unsigned int sensors_time_poll = 0;
// int temp_time_poll = 0;
// int sms_test_time = 0;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9;
GPIO_Init(GPIOB, &GPIO_InitStructure);
GPIO_WriteBit(GPIOB, GPIO_Pin_9, Bit_SET); // off
Delay_Init();
Enrf24_init(CE_PIN, CSN_PIN, IRQ_PIN);
Enrf24_begin(1000000, 0); // Defaults 1Mbps, channel 0, max TX power
Enrf24_setTXaddress((void*)enrf24_addr);
Enrf24_setRXaddress((void*)enrf24_addr);
Enrf24_enableRX(); // Start listening
#if DEBUG
USART1_Init(115200);
#endif
DS1307_Init();
Sensors_Init();
RS485_Init(115200);
sim_hal_init(115200);
OutputInit();
if (ThesisInit() == THESIS_FLASH_ERROR)
{
#if DEBUG
USART1_SendStr("\nFlash write error.\n");
#endif
TurnBuzzerOn();
Delay(1000);
}
#if DEBUG
RCC_GetClocksFreq(&RCC_Clocks);
USART1_SendStr("System Clock: ");
USART1_SendNum(RCC_Clocks.SYSCLK_Frequency);
USART1_SendStr("\r\n");
USART1_SendStr("Device ID: ");
USART1_SendByte(__flash_data.id, HEX);
USART1_SendStr("\r\n");
USART1_SendStr("Device Unique Number: ");
USART1_SendByte(__flash_data.unique_number[0], HEX);
USART1_SendByte(__flash_data.unique_number[1], HEX);
USART1_SendByte(__flash_data.unique_number[2], HEX);
USART1_SendByte(__flash_data.unique_number[3], HEX);
USART1_SendStr("\r\n");
#endif
OneWire_Init();
for EVER
{
if (millis() - sensors_time_poll > 100)
{
led_toggle();
Sensors_Poll();
sensors_time_poll = millis();
// buzzer_toggle();
// output_toggle();
if (millis() > 10000)
{
if (sensors.Gas >= GAS_LIMIT)
{
#if DEBUG
USART1_SendStr("Gas detected.\r\n");
USART1_SendStr("Current Gas: ");
USART1_SendFloat(sensors.Gas);
USART1_SendStr("Limited Gas: ");
USART1_SendFloat(GAS_LIMIT);
USART1_SendStr("\r\n");
#endif
TurnBuzzerOn();
TurnSpeakerOn();
TurnRelayOn();
}
else if (sensors.Lighting >= LIGHT_LIMIT)
{
#if DEBUG
USART1_SendStr("Light detected.\r\n");
USART1_SendStr("Current Light: ");
USART1_SendFloat(sensors.Lighting);
USART1_SendStr("Limited Light: ");
USART1_SendFloat(LIGHT_LIMIT);
USART1_SendStr("\r\n");
#endif
TurnBuzzerOn();
TurnSpeakerOn();
TurnRelayOn();
}
else if (sensors.TempC >= TEMPC_LIMIT)
{
#if DEBUG
USART1_SendStr("Tempc detected.\r\n");
USART1_SendStr("Current Tempc: ");
USART1_SendFloat(sensors.TempC);
USART1_SendStr("Limited Tempc: ");
USART1_SendFloat(TEMPC_LIMIT);
USART1_SendStr("\r\n");
#endif
TurnBuzzerOn();
TurnSpeakerOn();
TurnRelayOn();
}
else
{
TurnBuzzerOff();
TurnSpeakerOff();
TurnRelayOff();
}
}
// Sim900_Process();
}
// if (millis() - sms_test_time > 10000)
// {
// Sim900_SendSMS("Hi kieu", "01677880531");
// sms_test_time = millis();
// }
// usart_len = USART1_Available();
//
// // usart: for test
// if (usart_len > 4)
// {
// int i;
// USART1_SendStr("\nUSART1 received packet: \n");
// USART1_GetData(buff_usart, usart_len);
// for (i = 0; i < usart_len; i++)
// USART1_SendByte(buff_usart[i], HEX);
// USART1_SendChar('\n');
// if (ThesisProcess(buff_usart, usart_len) == THESIS_OK)
// {
// memset(buff_usart, 0, usart_len);
// USART1_Flush();
// if (thesis_need_to_send)
// {
// int i;
// USART1_SendStr("\nNeed to send packet: ");
// for (i = 0; i < thesis_msg_len; i++)
// {
// USART1_SendByte(thesis_sent_msg[i], HEX);
// }
// USART1_SendStr("\nNeed to send packet length: ");
// USART1_SendNum(thesis_msg_len);
// USART1_SendStr("\n");
// thesis_msg_len = 0;
// thesis_need_to_send = 0;
// }
// USART1_SendStr("\nPacket processed.\n");
// }
// else if (thesis_errn == THESIS_FLASH_ERROR)
// {
// USART1_SendStr("\n");
// USART1_SendStr(thesis_err_msg);
// USART1_SendStr("\n");
// led_toggle();
// for(;;);
// }
// else if (thesis_errn != THESIS_PACKET_NOT_ENOUGH_LENGTH)
// {
// memset(buff_usart, 0, usart_len);
// USART1_Flush();
// USART1_SendStr("Packet processing fail.\n");
// }
//
// USART1_SendStr("\n");
// USART1_SendStr(thesis_err_msg);
// USART1_SendStr("\n");
// }
// rf
if (Enrf24_available(1))
{
int i;
rf_len = Enrf24_read(buff_rf + rf_len, BUFFER_SIZE - 1 - rf_len);
#if DEBUG
USART1_SendStr("\nRF received packet.\n");
for (i = 0; i < rf_len; i++)
USART1_SendByte(buff_rf[i], HEX);
USART1_SendChar('\n');
#endif
if (ThesisProcess(buff_rf, rf_len) == THESIS_OK)
{
memset(buff_rf, 0, rf_len);
rf_len = 0;
if (thesis_need_to_send)
{
int i;
#if DEBUG
USART1_SendStr("\nNeed to send packet: ");
for (i = 0; i < thesis_msg_len; i++)
{
// Enrf24_write(thesis_sent_msg[i]);
USART1_SendByte(thesis_sent_msg[i], HEX);
}
#endif
Enrf24_write_buff(thesis_sent_msg, thesis_msg_len);
Enrf24_flush();
#if DEBUG
USART1_SendStr("\nNeed to send packet length: ");
USART1_SendNum(thesis_msg_len);
USART1_SendStr("\n");
#endif
thesis_msg_len = 0;
thesis_need_to_send = 0;
}
#if DEBUG
USART1_SendStr("\nPacket processed.\n");
#endif
}
else if (thesis_errn == THESIS_FLASH_ERROR)
{
#if DEBUG
USART1_SendStr("\n");
USART1_SendStr(thesis_err_msg);
USART1_SendStr("\n");
#endif
led_toggle();
for(;;);
}
else if (thesis_errn != THESIS_PACKET_NOT_ENOUGH_LENGTH)
{
memset(buff_rf, 0, rf_len);
// RF_Flush();
rf_len = 0;
#if DEBUG
USART1_SendStr("Packet processing fail.\n");
#endif
}
#if DEBUG
USART1_SendStr("\n");
USART1_SendStr(thesis_err_msg);
USART1_SendStr("\n");
#endif
}
// rs485
rs485_len = RS485_Available();
if (rs485_len > 4)
{
int i;
RS485_GetData(buff_rs485, rs485_len);
#if DEBUG
USART1_SendStr("\nUSART1 received packet: \n");
for (i = 0; i < rs485_len; i++)
USART1_SendByte(buff_rs485[i], HEX);
USART1_SendChar('\n');
#endif
if (ThesisProcess(buff_rs485, rs485_len) == THESIS_OK)
{
memset(buff_rs485, 0, rs485_len);
RS485_Flush();
if (thesis_need_to_send)
{
int i;
#if DEBUG
USART1_SendStr("\nNeed to send packet: ");
#endif
RS485_DIR_Output();
for (i = 0; i < thesis_msg_len; i++)
{
RS485_SendChar(thesis_sent_msg[i]);
#if DEBUG
USART1_SendByte(thesis_sent_msg[i], HEX);
#endif
}
RS485_DIR_Input();
#if DEBUG
USART1_SendStr("\nNeed to send packet length: ");
USART1_SendNum(thesis_msg_len);
USART1_SendStr("\n");
#endif
thesis_msg_len = 0;
thesis_need_to_send = 0;
}
#if DEBUG
USART1_SendStr("\nPacket processed.\n");
#endif
}
else if (thesis_errn == THESIS_FLASH_ERROR)
{
#if DEBUG
USART1_SendStr("\n");
USART1_SendStr(thesis_err_msg);
USART1_SendStr("\n");
#endif
led_toggle();
for(;;);
}
else if (thesis_errn != THESIS_PACKET_NOT_ENOUGH_LENGTH)
{
memset(buff_rs485, 0, rs485_len);
RS485_Flush();
#if DEBUG
USART1_SendStr("Packet processing fail.\n");
#endif
}
#if DEBUG
USART1_SendStr("\n");
USART1_SendStr(thesis_err_msg);
USART1_SendStr("\n");
#endif
}
}
}
void Uc1ArmingLoggerExecutionScopePkg_initRelations( void ) {
Admin_Init(&(itsAdmin), RiCMainTask());
Driver_Init(&(itsDriver), RiCMainTask());
Init(&(itsScanner), RiCMainTask());
Sensors_Init(&(itsSensors), RiCMainTask());
Uc_Uc1ArmingLogger_Init(&(itsUc_Uc1ArmingLogger), RiCMainTask());
{
RiCDefaultReactiveOutbound_setItsIRiCReactive(
&(itsAdmin.pUc_Uc1ArmingLogger.outbound),
&(itsUc_Uc1ArmingLogger.pAdmin.inbound)
);
}{
RiCDefaultReactiveOutbound_setItsIRiCReactive(
&(itsUc_Uc1ArmingLogger.pAdmin.outbound),
&(itsAdmin.pUc_Uc1ArmingLogger.inbound)
);
}
{
RiCDefaultReactiveOutbound_setItsIRiCReactive(
&(itsDriver.pUc_Uc1ArmingLogger.outbound),
&(itsUc_Uc1ArmingLogger.pDriver.inbound)
);
}{
RiCDefaultReactiveOutbound_setItsIRiCReactive(
&(itsUc_Uc1ArmingLogger.pDriver.outbound),
&(itsDriver.pUc_Uc1ArmingLogger.inbound)
);
}
{
RiCDefaultReactiveOutbound_setItsIRiCReactive(
&(itsUc_Uc1ArmingLogger.pScanner.outbound),
&(itsScanner.pUc_Uc1ArmingLogger.inbound)
);
}{
RiCDefaultReactiveOutbound_setItsIRiCReactive(
&(itsScanner.pUc_Uc1ArmingLogger.outbound),
&(itsUc_Uc1ArmingLogger.pScanner.inbound)
);
}
{
RiCDefaultReactiveOutbound_setItsIRiCReactive(
&(itsUc_Uc1ArmingLogger.pSensors.outbound),
&(itsSensors.pUc_Uc1ArmingLogger.inbound)
);
}{
RiCDefaultReactiveOutbound_setItsIRiCReactive(
&(itsSensors.pUc_Uc1ArmingLogger.outbound),
&(itsUc_Uc1ArmingLogger.pSensors.inbound)
);
}
#ifdef _OMINSTRUMENT
RenameGlobalInstances();
#endif /* _OMINSTRUMENT*/
}
