//=============================================================== // 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) { } }