//===============================================================
// Name : StateClose
// Purpose : Close all peripherals and put device in sleep mode
//===============================================================
void StateClose(void)
{
INTDisableInterrupts(); // Disable all interrupts of the system.
// Wdt.Disable();
LED_ALL_OFF();
I2c.Close(I2C4);
// DRIVE B
//==========================================================
if (USE_DRIVE_B == 1)
{
Pwm.Close(PWM_2);
Pwm.Close(PWM_3);
InputCapture.Close(IC2);
InputCapture.Close(IC4);
}
//==========================================================
// DRIVE A
//==========================================================
if (USE_DRIVE_A == 1)
{
Pwm.Close(PWM_4);
Pwm.Close(PWM_5);
InputCapture.Close(IC1);
InputCapture.Close(IC3);
}
//==========================================================
Spi.Close(SPI4);
// Can.Close(CAN1);
Uart.Close(UART6);
Timer.Close(TIMER_1);
Timer.Close(TIMER_2);
Timer.Close(TIMER_3);
Timer.Close(TIMER_5);
// OSCCONSET = 0x10; // Sleep mode
}
void testLoop()
{
u8 errorCount = 0,test_current_level;
u32 tempSysTick;
u16 tCurrentAdc,minCurrent,maxCurrent;
//gTestMode = TEST_AA_BATTERY;
Start:
if(gTestMode == TEST_AAA_BATTERY)
{
gAdc_Current_Level_1_Min = ADC_CURRENT_LEVEL_1_MIN_AAA;
gAdc_Current_Level_1_Max = ADC_CURRENT_LEVEL_1_MAX_AAA;
gAdc_Current_Level_2_Min = ADC_CURRENT_LEVEL_2_MIN_AAA;
gAdc_Current_Level_2_Max = ADC_CURRENT_LEVEL_2_MAX_AAA;
gAdc_Current_Level_3_Min = ADC_CURRENT_LEVEL_3_MIN_AAA;
gAdc_Current_Level_3_Max = ADC_CURRENT_LEVEL_3_MAX_AAA;
gAdc_Start_Battery_Charging = ADC_START_BATTERY_CHARGING;
}
else
{
gAdc_Current_Level_1_Min = ADC_CURRENT_LEVEL_1_MIN;
gAdc_Current_Level_1_Max = ADC_CURRENT_LEVEL_1_MAX;
gAdc_Current_Level_2_Min = ADC_CURRENT_LEVEL_2_MIN;
gAdc_Current_Level_2_Max = ADC_CURRENT_LEVEL_2_MAX;
gAdc_Current_Level_3_Min = ADC_CURRENT_LEVEL_3_MIN;
gAdc_Current_Level_3_Max = ADC_CURRENT_LEVEL_3_MAX;
gAdc_Start_Battery_Charging = ADC_START_BATTERY_CHARGING;
}
//test_pos_now = TEST_CHANNEL_2;
do{
switch(test_pos_now)
{
case TEST_CHANNEL_1:
CLOSE_CHANNEL_1();break;
case TEST_CHANNEL_2:
CLOSE_CHANNEL_2();break;
case TEST_CHANNEL_3:
CLOSE_CHANNEL_3();break;
case TEST_CHANNEL_4:
CLOSE_CHANNEL_4();break;
default:
dumpHandler();
goto endpos;
break;
}
//wait for ZERO_STATE---->DETECT_STATE
//while(getAverage(test_pos_now) > ADC_START_BATTERY_DETECT)
// ClrWdt();
getSysTick();
tempSysTick = nowSysTick;
do
{
tCurrentAdc = getAverage(test_pos_now);
if(tCurrentAdc > ADC_NO_CURRENT)
{
if(tCurrentAdc > gAdc_Current_Level_1_Min)
{
stepNow = 4;
goto battery_detect;
}
else
break;
}
#if 1
getSysTick();
if(nowSysTick < tempSysTick || (nowSysTick-tempSysTick) >= 12)
{
dumpHandler();
goto endpos;
}
#endif
ClrWdt();
}while(1);
getSysTick();
tempSysTick = nowSysTick;
while(getAverage(test_pos_now) > ADC_NO_CURRENT)
{
getSysTick();
if(nowSysTick < tempSysTick || (nowSysTick-tempSysTick) >= MAX_TIME_BATTERY_DEAD)
{
dumpHandler();
goto endpos;
}
ClrWdt();
}
stepNow++;
//ok now battery state is BATTERY_DETECT
//time wait for battery detect
tempSysTick = nowSysTick;
while(getAverage(test_pos_now) < gAdc_Start_Battery_Charging)
{
getSysTick();
if(nowSysTick < tempSysTick || (nowSysTick-tempSysTick) >= MAX_TIME_TO_BATTERY_DETECT)
{
dumpHandler();
goto endpos;
}
}
stepNow++;
if((nowSysTick-tempSysTick) < MIN_TIME_TO_BATTERY_DETECT)
{
dumpHandler();
goto endpos;
}
stepNow++;
//BATTERY_DETECT_STATE
battery_detect:
getSysTick();
tempSysTick = nowSysTick;
delay_ms(10);
do
{
tCurrentAdc = getAverage(test_pos_now);
if(tCurrentAdc < gAdc_Current_Level_1_Min|| tCurrentAdc > gAdc_Current_Level_1_Max) // CURRENT_LEVEL_1
{
errorCount++;
if(errorCount > 3)
{
dumpHandler();
goto endpos;
}
}
getSysTick();
if(nowSysTick < tempSysTick || (nowSysTick-tempSysTick) >= MAX_TIME_DURING_BATTERY_DETECT)
{
dumpHandler();
goto endpos;
}
//delay_ms(10);
ClrWdt();
}while(tCurrentAdc > gAdc_Current_Level_1_Min);
stepNow++;
if((nowSysTick-tempSysTick) < MIN_TIME_DURING_BATTERY_DETECT)
{
dumpHandler();
goto endpos;
}
stepNow++;
//wait for no current
tempSysTick = nowSysTick;
do{
tCurrentAdc = getAverage(test_pos_now);
ClrWdt();
getSysTick();
if(nowSysTick < tempSysTick || (nowSysTick-tempSysTick) >=4)
{
dumpHandler();
goto endpos;
}
}while(tCurrentAdc > ADC_NO_CURRENT);
stepNow++;
//ok, now battery state is BATTERY_NORMAL_CHARGING
//time wait for battery normal charging
for(test_current_level = 1; test_current_level <=4; test_current_level++)
{
errorCount = 0;
switch(test_current_level)
{
case 1:
minCurrent = gAdc_Current_Level_3_Min;
maxCurrent = gAdc_Current_Level_3_Max;
break;
case 2:
minCurrent = gAdc_Current_Level_2_Min;
maxCurrent = gAdc_Current_Level_2_Max;
break;
case 3:
case 4:
minCurrent = gAdc_Current_Level_1_Min;
maxCurrent = gAdc_Current_Level_1_Max;
break;
default:
dumpHandler();
goto endpos;
break;
}
getSysTick();
tempSysTick = nowSysTick;
while(getAverage(test_pos_now) < gAdc_Start_Battery_Charging)
{
getSysTick();
if(nowSysTick < tempSysTick || (nowSysTick-tempSysTick) >= MAX_TIME_TO_BATTERY_NORMAL_CHARGING)
{
dumpHandler();
goto endpos;
}
}
stepNow++;
if((nowSysTick-tempSysTick) < MIN_TIME_TO_BATTERY_NORMAL_CHARGING_SPEC)
{
dumpHandler();
goto endpos;
}
stepNow++;
//BATTERY_NORMAL_CHARGING
getSysTick();
tempSysTick = nowSysTick;
delay_ms(10);
do{
tCurrentAdc = getAverage(test_pos_now);
if(tCurrentAdc < minCurrent || tCurrentAdc > maxCurrent)
{
errorCount++;
if(errorCount > 5)
{
dumpHandler();
goto endpos;
}
}
getSysTick();
if(nowSysTick < tempSysTick || (nowSysTick-tempSysTick) >= MAX_TIME_DURING_BATTERY_NORMAL_CHARGING)
{
dumpHandler();
goto endpos;
}
ClrWdt();
}while(tCurrentAdc > minCurrent);
if((nowSysTick-tempSysTick) < MIN_TIME_DURING_BATTERY_NORMAL_CHARGING)
{
dumpHandler();
goto endpos;
}
tempSysTick = nowSysTick;
do{
tCurrentAdc = getAverage(test_pos_now);
ClrWdt();
getSysTick();
if(nowSysTick < tempSysTick || (nowSysTick-tempSysTick) >=4)
{
dumpHandler();
goto endpos;
}
}while(tCurrentAdc > ADC_NO_CURRENT);
#ifdef ENABLE_FAST_TEST_MODE
if(test_current_level == 2)
{
if(gTestMode == TEST_AA_BATTERY)
{
if(test_pos_now != TEST_CHANNEL_1)
break;
}
else if(gTestMode == TEST_AAA_BATTERY)
{
if(test_pos_now != TEST_CHANNEL_1)
break;
}
#endif
}
}
switch(test_pos_now)
{
case TEST_CHANNEL_1:
OPEN_CHANNEL_1();
LED3_G_ON();
break;
case TEST_CHANNEL_2:
OPEN_CHANNEL_2();
LED4_G_ON();
break;
case TEST_CHANNEL_3:
OPEN_CHANNEL_3();
LED1_G_ON();
break;
case TEST_CHANNEL_4:
OPEN_CHANNEL_4();
LED2_G_ON();
break;
default:
dumpHandler();
goto endpos;
break;
}
endpos:
delay_ms(20);
test_pos_now++;
}while(test_pos_now<= TEST_CHANNEL_4);
if(isError)
{
while(1)
{
NOP();
ClrWdt();
}
}
getSysTick();
tempSysTick = nowSysTick;
while(GET_BATTERY_STATUS() == gTestMode)
{
ClrWdt();
getSysTick();
if((nowSysTick-tempSysTick) >= 200)
LED_ALL_OFF();
}
LED_ALL_OFF();
//CLOSE_CHANNEL_1();
//CLOSE_CHANNEL_2();
//CLOSE_CHANNEL_3();
//CLOSE_CHANNEL_4();
while(1)
{
NOP();
ClrWdt();
}
gTestMode = GET_BATTERY_STATUS();
errorCount = 0;
test_pos_now = TEST_CHANNEL_1;
goto Start;
}
int main(void) {
//***************************************************************************************//
//*****************ARRANCAMOS LA CONFIGURACION DEL PIC **********************************//
//***************************************************************************************//
Init_Hw();
Delay1msT1(0); //Configuramos el puertos y quitamos analogicos
Init_Pll();
Delay1msT1(0); //configuramos el pll que abiasmos arracamos sin él
Init_PWM();
Delay1msT1(0); //configuramos el PWM
init_interr_extern();
Delay1msT1(0); //configuramos las interrupciones externas
Init_Bluetooh();
Delay1msT1(0); //Configuramos el serial-Bluetooth
cargar_datos_ajuste();
Init_I2C();
Delay1msT1(0); //incializamos el I2c
set_inicial();
Delay1msT1(0); //Configuramos la incialicacion de sensor
getAngle_init();
Delay1msT1(0); //Incializamos el filtro kalman
set_inicial();
Delay1msT1(0); //Incializamos el acelerometro
LED_ALL_ON();
for (i = 0; i < 2500; i++) Delay_Nop(2000);
LED_ALL_OFF();
//***************************************************************************************//
//***************************************************************************************//
//*****************CALIBRAMOS EL ACELEROMETRO Y GIROSCOPO *******************************//
// get_calibrado_acelerometro(5, 50); //cojemos los valres por defecto del giroscopo y aceletometro
LED_ALL_OFF();
//***************************************************************************************//
//*****************INICIALIZAMOS EL PID Y LAS VARIABLES ********************************//
// pid_dsp_configuracion();
//***************************************************************************************//
LED_ALL_OFF();
//*****************ARRANCAMOS INTERRUPCION DEL BUCLE PRINCIPAL *************************//
SetupT3ForXmsPID(Tsample); //configuramos la interrupcion principal
SetupT4For20msPID(); //configuramos la interrupcion principal
StartInterrup4(); //incializamos la interrupcion
StartInterrup3(); //incializamos la interrupcion
enviar_mensaje("------------------------------------------------------");
//***************************************************************************************//
while (1) {
}
}
