void setup()
{
Serial.begin(115200);
Serial.println("started!");
strip1.begin();
strip1.show();
blinkLED(4, 250);
initMPU();
RGB.control(true);
RGB.color(0, 0, 0);
logger.begin();
Spark.function("setskipvalues", setSkipValues);
Spark.function("setaccllimit", setAccelLimit);
}
void setDLPFConfig(uint8_t source)
{
initMPU(MPU6050Addr, MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, source);
}
void setClockSource(uint8_t source)
{
initMPU(MPU6050Addr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, source);
}
void setFullScaleGyroRange(uint8_t range)
{
initMPU(MPU6050Addr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, range);
}
uint8_t setFullScaleAccelRange(uint8_t range)
{
return initMPU(MPU6050Addr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, range);
}
static void main_init(void){
GPIO_InitTypeDef ledGPIOInit;
int i;
SystemInit();
HAL_Init();
/* Configure the system clock to 168 MHz */
//SystemClock_Config();
std_init();
__GPIOD_CLK_ENABLE();
__GPIOA_CLK_ENABLE();
ledGPIOInit.Pin = GPIO_PIN_12 | GPIO_PIN_13 | GPIO_PIN_14 | GPIO_PIN_15;
ledGPIOInit.Mode = GPIO_MODE_OUTPUT_PP;
ledGPIOInit.Pull = GPIO_NOPULL;
ledGPIOInit.Speed = GPIO_SPEED_FREQ_MEDIUM;
HAL_GPIO_Init(GPIOD,&ledGPIOInit);
ledGPIOInit.Pin = GPIO_PIN_0;
ledGPIOInit.Mode = GPIO_MODE_INPUT;
ledGPIOInit.Pull = GPIO_PULLDOWN;
ledGPIOInit.Speed = GPIO_SPEED_FREQ_MEDIUM;
HAL_GPIO_Init(GPIOA,&ledGPIOInit);
HAL_Delay(10);
pwm_init();
for( i = 0; i < 4; i++){
power[i] = 0;
}
HAL_Delay(20);
for( i = 0; i < 4; i++){
power[i] = 1000;
}
HAL_Delay(20);
for( i = 0; i < 4; i++){
power[i] = 0;
}
if(initMPU()){
puts("Failed MPU");
}
Get_Gyro_Offset_Start();
HAL_Delay(1000);
if(Get_Gyro_Offset_Stopp()<900){
puts("Failed Gyrooffset");
Error_Handler();
}
delayTime = HAL_GetTick();
pidDataX = &pidDataXObj;
pidDataY = &pidDataYObj;
pid_init(pidDataX,1,0,0,0.003f,9000,900);
pid_init(pidDataY,1,0,0,0.003f,9000,900);
init2Kalman(&kalman2X,90);
init2Kalman(&kalman2Y,90);
puts("Init + Selfcheck okay!");
}