/*=====================================================================================================*/ void Sensor_Init( void ) { Acc.X = 0; Acc.Y = 0; Acc.Z = 0; Acc.OffsetX = +7; Acc.OffsetY = +146; Acc.OffsetZ = -291; Acc.TrueX = 0.0f; Acc.TrueY = 0.0f; Acc.TrueZ = 0.0f; Gyr.X = 0; Gyr.Y = 0; Gyr.Z = 0; Gyr.OffsetX = 0; Gyr.OffsetY = 0; Gyr.OffsetZ = 0; Gyr.TrueX = 0.0f; Gyr.TrueY = 0.0f; Gyr.TrueZ = 0.0f; Mag.X = 0; Mag.Y = 0; Mag.Z = 0; Mag.AdjustX = 0; Mag.AdjustY = 0; Mag.AdjustZ = 0; Mag.TrueX = 0.0f; Mag.TrueY = 0.0f; Mag.TrueZ = 0.0f; Temp.T = 0; Temp.OffsetT = TEMP_OFFSET; Temp.TrueT = 0.0f; Baro.Temp = 0.0f; Baro.Press = 0.0f; Baro.Height = 0.0f; while(MPU9150_Init() != SUCCESS); MS5611_Init(); }
/*=====================================================================================================*/ u8 Sensor_Init(void) { u8 Sta = ERROR; Acc.X = 0; Acc.Y = 0; Acc.Z = 0; Acc.OffsetX = +7; Acc.OffsetY = +146; Acc.OffsetZ = -291; Acc.TrueX = 0.0f; Acc.TrueY = 0.0f; Acc.TrueZ = 0.0f; Gyr.X = 0; Gyr.Y = 0; Gyr.Z = 0; Gyr.OffsetX = 0; Gyr.OffsetY = 0; Gyr.OffsetZ = 0; Gyr.TrueX = 0.0f; Gyr.TrueY = 0.0f; Gyr.TrueZ = 0.0f; Mag.X = 0; Mag.Y = 0; Mag.Z = 0; Mag.AdjustX = 0; Mag.AdjustY = 0; Mag.AdjustZ = 0; Mag.TrueX = 0.0f; Mag.TrueY = 0.0f; Mag.TrueZ = 0.0f; Temp.T = 0; Temp.OffsetT = TEMP_OFFSET; Temp.TrueT = 0.0f; Sta = MPU9150_Init(); MS5611_Init(&Baro); return Sta; }
/** * @brief Main program. * @param None * @retval None */ int main(void) { //SysTick_Config(SystemCoreClock/1000); //1ms interrupt capture counter_sweep=0; /*************Variable Initializations*****************/ /*Example 3600/2=1800-->40K, 3600/3.6=100-->72K, 3600/4=900-->80K, 3600/5=720-->100K*/ PWM_cycle =900; MotorV=16.0f; //(V), motor voltage, now 13.0V power source Ts1=0.001f; //sec, for loop1, 1KHz Ts2=0.01f; //sec, for loop2, 100Hz time1=0.0f; time2=0.0f; pi=3.14159265f; two_pi=6.2831853f; Kc2r=two_pi/2560.0f; //ENCODER RESOLUTION, floating in 32*4*10 freq=34.0f; //Hz, flapping frequency 34.0HZ two_pi_freq=two_pi*freq; Vin1=0; Vin2=0; Ddirect1=1; Ddirect2=1; Hall_1=0x05; Hall_2=0x05; Hall_State1=1; Hall_State2=1; Encoder1=0; Encoder2=0; y1=0.0f; y1_dot=0.0f; y1_ddot=0.0f; y1_save=0.0f; y2=0.0f; y2_dot=0.0f; y2_ddot=0.0f; y2_save=0.0f; yd1=0.0f; yd1_dot=0.0f; yd1_ddot=0.0f; yd2=0.0f; yd2_dot=0.0f; yd2_ddot=0.0f; e1=0.0f; e1_dot=0.0f; e1_int=0.0f; e2=0.0f; e2_dot=0.0f; e2_int=0.0f; flap1=0.0f; d_flap1=0.0f; sigma1=0.5f; phase1=0.0f; flap2=0.0f; d_flap2=0.0f; sigma2=0.5f; phase2=0.0f; /******* body stabilization *******/ flap_cycle_flag=0; Kp_rol=0.1f; //10deg->0.5V Kd_rol=0.0017f; //20deg/s->0.5V Kp_pit=2.0f; //10deg->2V Kd_pit=0.001f; //20deg/s->1V Kp_yaw=0.0f; //10 Kd_yaw=0.0f; //100 delta_Amp=0.0f; delta_Spl=0.0f; delta_Bia=0.0f; /*!!!!!!!!!!!!!! trim condition!!!!!!!!!!!!!!!!!!!*/ //left motor is motor 1, looking at the front of motor board trim_A1 = 16.00f; //(V), value at 12V nominal and 13V power source //right motor is motor 2, looking at the front of motor board trim_A2 = 16.00f; //(V), value at 12V nominal and 13V power source #if WING_KINEMATICS flap1=0.0f*KGR; // rad 30 d_flap1=0.0f/180.0f*pi; // rad sigma1=0.5f; phase1=0.0f; flap2=0.0f*KGR; // rad 30 d_flap2=0.0f/180.0f*pi; // rad sigma2=0.5f; phase2=0.0f; //////////////PID Parameters//////////250/ // Kp=38.8974f; // Kd=0.1338f; // Ki=6028.3f; // Ku = 9.16732471506f; // Kp = 16.10f; // Ki = 0.0f;//550.0f; // Kd = 0.001f; // Ku=0.0f; // Kp = 0.0f; // Ki = 0.0f; // Kd = 0.0f; //////////////PID Parameters//////////200/ //Kp=21.5359f; //Kd=0.1049f; //Ki=3086.5f; /*******LQR Parameters********/ Kss1=0.00015; Kss2=0.012; Fss1=6.1690; //6.1690 Fss2=7.6461; //7.6461 // /*******LQR Parameters********/ // Kss1=0.0001; // Kss2=0.0117; // Fss=8.1690; /******* body stabilization *******/ Kp_rol=3.0f/180.0f*pi; //10deg->5deg in rad Kd_rol=0.0018f; //10deg->5deg in rad Kp_pit=5.0f/180.0f*pi/10.0f; //10deg->5deg in rad Kd_pit=0.0f; //10deg->5deg in rad Kp_yaw=0.0f; //10 Kd_yaw=0.0f; //100 delta_Amp=0.0f; delta_Spl=0.0f; delta_Bia=0.0f; //left motor is motor 1, looking at the front of motor board trim_A1 = 45.0f/180.0f*pi; //(deg) //right motor is motor 2, looking at the front of motor board trim_A2 = 45.0f/180.0f*pi; //(deg) #endif //TO DO: trim_Bia1=0.0f; ////////(pitch) trim bias trim_Bia2=0.0f; //TO DO: trim_Spl1=0.0f; trim_Spl2=0.0f; /**********ENCODER Configuration***********/ #if QEI_DEF Timer1_Encoder1(); Timer4_Encoder2(); /* ENCODER COUNTER # RESET */ TIM_SetCounter(TIM1,0); //TIM4->CNT = 0; Encoder1 counts reset TIM_SetCounter(TIM4,0); //TIM4->CNT = 0; Encoder2 counts reset #endif #if SERIAL_COM serial_flag=0; tx_index = 0; rx_index = 0; Usart3_Configuration(); #endif #if I2C_IMU MPU9150_Init(); #endif /* gpio Configuration */ GPIO_Config(); //define GPIO & PWM with timerS /*************Loop Initializations*****************/ LOOP1_flag=0; LOOP2_flag=0; /*************PWM Initializations*****************/ //DISABLE DRIVE BY DIAG_EN SET 0 GPIO_WriteBit(GPIOB, GPIO_Pin_2 , Bit_RESET);//TEST PIN RESET GPIO_WriteBit(GPIOC, GPIO_Pin_13, Bit_RESET); // Bit-SET = high, bit-reset = low Timer3_PWM1(PWM_cycle); // PWM Driver Input 1 Timer2_PWM2(PWM_cycle); // PWM Driver Input 2 PWM1_Set_SingleEdge( 0, 0, 0); PWM2_Set_SingleEdge( 0, 0, 0); /*************Wait 5sec for setup****************/ delay_ms(5000); //TICKER MIGHT BE A BETTER WAY /*************Timer Initializations****************/ Timer7_Looptimer1_Int(LOOP0_STEP_SIZE); #if TIM_LOOP1 Timer15_Looptimer2_Int(LOOP1_STEP_SIZE); time1=0.0; #endif #if TIM_LOOP2 Timer16_Looptimer3_Int(LOOP2_STEP_SIZE); time2=0.0; #endif /********** Enable drive**************/ // DLAG/EN1 :: 20 :: PB2 // DLAG/EN2 :: 02 :: PC13 GPIO_WriteBit(GPIOB, GPIO_Pin_2 , Bit_SET);//TEST PIN RESET GPIO_WriteBit(GPIOC, GPIO_Pin_13, Bit_SET); // Bit-SET = high, bit-reset = low /* Infinite loop */ while (1) { //Encoder1= TIM_GetCounter(TIM1); //////////Encoder1 test, get position; TIMx->CNT; //Encoder2= TIM_GetCounter(TIM4); //Encoder2 test, get position; TIMx->CNT; takes 10us //PWM1_Set_SingleEdge(0,Vin1,0); //////////PWM test //PWM2_Set_SingleEdge(0,Vin2,0); //////////PWM test if(LOOP1_flag) loop1(); if(LOOP2_flag) loop2(); // PWM1_Set_SingleEdge(Vin1,Vin1,Vin1); //////////PWM test // PWM2_Set_SingleEdge(Vin2,Vin2,Vin2); //////////PWM test // printf("%d \r\n", Encoder1); } }
void MPU9150_Config(void) { MPU9150_StructInit (); MPU9150_Init(); }