void init_MiniMU9()
{
I2C_Init();
delay(1500);
Accel_Init();
Compass_Init();
Gyro_Init();
delay(20);
for(int i=0;i<32;i++) // We take some readings...
{
Read_Gyro();
Read_Accel();
for(int y=0; y<6; y++) // Cumulate values
AN_OFFSET[y] += AN[y];
delay(20);
}
for(int y=0; y<6; y++)
AN_OFFSET[y] = AN_OFFSET[y]/32;
AN_OFFSET[5]-=GRAVITY*SENSOR_SIGN[5];
delay(2000);
timer=millis();
delay(20);
counter=0;
}
/*
** ===================================================================
** Method : PE_low_level_init (component MK20DX128EX5)
**
** Description :
** Initializes beans and provides common register initialization.
** The method is called automatically as a part of the
** application initialization code.
** This method is internal. It is used by Processor Expert only.
** ===================================================================
*/
void PE_low_level_init(void)
{
#ifdef PEX_RTOS_INIT
PEX_RTOS_INIT(); /* Initialization of the selected RTOS. Macro is defined by the RTOS component. */
#endif
/* {MQXLite RTOS Adapter} Set new interrupt vector (function handler and ISR parameter) */
/* Initialization of the SIM module */
/* PORTA_PCR4: ISF=0,MUX=7 */
PORTA_PCR4 = (uint32_t)((PORTA_PCR4 & (uint32_t)~(uint32_t)(
PORT_PCR_ISF_MASK
)) | (uint32_t)(
PORT_PCR_MUX(0x07)
));
/* Initialization of the RCM module */
/* RCM_RPFW: RSTFLTSEL=0 */
RCM_RPFW &= (uint8_t)~(uint8_t)(RCM_RPFW_RSTFLTSEL(0x1F));
/* RCM_RPFC: RSTFLTSS=0,RSTFLTSRW=0 */
RCM_RPFC &= (uint8_t)~(uint8_t)(
RCM_RPFC_RSTFLTSS_MASK |
RCM_RPFC_RSTFLTSRW(0x03)
);
/* Initialization of the FTFL_FlashConfig module */
/* Initialization of the PMC module */
/* PMC_REGSC: ACKISO=0,BGBE=0 */
PMC_REGSC &= (uint8_t)~(uint8_t)(
PMC_REGSC_ACKISO_MASK |
PMC_REGSC_BGBE_MASK
);
/* PMC_LVDSC1: LVDACK=1,LVDIE=0,LVDRE=1,LVDV=0 */
PMC_LVDSC1 = (uint8_t)((PMC_LVDSC1 & (uint8_t)~(uint8_t)(
PMC_LVDSC1_LVDIE_MASK |
PMC_LVDSC1_LVDV(0x03)
)) | (uint8_t)(
PMC_LVDSC1_LVDACK_MASK |
PMC_LVDSC1_LVDRE_MASK
));
/* PMC_LVDSC2: LVWACK=1,LVWIE=0,LVWV=0 */
PMC_LVDSC2 = (uint8_t)((PMC_LVDSC2 & (uint8_t)~(uint8_t)(
PMC_LVDSC2_LVWIE_MASK |
PMC_LVDSC2_LVWV(0x03)
)) | (uint8_t)(
PMC_LVDSC2_LVWACK_MASK
));
/* SMC_PMPROT: ??=0,??=0,AVLP=0,??=0,ALLS=0,??=0,AVLLS=0,??=0 */
SMC_PMPROT = 0x00U; /* Setup Power mode protection register */
/* Common initialization of the CPU registers */
/* NVICIP8: PRI8=0 */
NVICIP8 = NVIC_IP_PRI8(0x00);
/* ### InternalI2C "Accel" init code ... */
Accel_Init();
/* ### Asynchro serial "Inhr1" init code ... */
Inhr1_Init();
/* ### "Term1" init code ... */
}
void main(void)
{
char counting_loops = 0;
// initialize board
Sys_Init();
putchar(' ');
All_Init();
Accel_Init(); // Provided function in i2c.h for accelerometer use
PCA0CP2 = 0xFFFF - PW_NEUT;
setup_count = 25;
while (setup_count);
LCD_calibrate_steering();
printf("\rSteering Calibrated\n");
lcd_print("Steering Calibrated\n");
LCD_prompts();
setup_count = 20;
prev_h_counts = h_counts; //start the checking flag
while(1)
{
if(!SSgain && calibrate_flag)
{
calibrate_accel();
calibrate_flag = 0;
//printf("calibrated once\n\r");
}
else if (SSgain){
get_four_readings(); //if the flag is down then we update the car
//printf("running damn car");
Run_Car();
}
counting_loops = counting_loops ? counting_loops-1 : 5;
//printf("once through loop %d \n\r",counting_loops);
if(!counting_loops)
{
Info_LCD_print();
//X accel. Y accel. DPW, SPW, total_time
printf("%d, %d, %d, %d, %d\n\r", gx, gy, motor_pw, r, total_time);
}
}
}
void setupImu()
{
// Init sensors
delay(50); // Give sensors enough time to start
I2C_Init();
Accel_Init();
Magn_Init();
Gyro_Init();
// Read sensors, init DCM algorithm
delay(20); // Give sensors enough time to collect data
reset_sensor_fusion();
}
/*!
* @brief Initialisation thread. runs once.
*/
void InitThread(void *data)
{
for (;;)
{
OS_SemaphoreWait(InitSemaphore, 0);
Random_Init();
//Switches mate
Switch_Init(S1Callback, (void *) 0, S2Callback, (void *) 0);
Toggle_Init(ToggleModeFinished);
Game_Init(GameModeFinished);
Touch_Init();
//Initialize all the modules
LEDs_Init();
I2C_Init(100000, MODULE_CLOCK);
Accel_Init(&AccelSetup);
PIT_Init(MODULE_CLOCK, &PitCallback, (void *) 0);
PIT_Set(500000000, bFALSE);
PIT_Enable(bTRUE);
Packet_Init(BAUD_RATE, MODULE_CLOCK);
Flash_Init();
CMD_Init();
//Best to do this one last
//TODO: disabled for yellow
RTC_Init((void (*)(void*))OS_SemaphoreSignal, (void *) RtcSemaphore);
Timer_Init();
Timer_Set(&PacketTimer);
Timer_Set(&AccTimer);
CMD_SpecialGetStartupValues();
LEDs_On(LED_ORANGE);
}
}