/*=====================================================================================================*/
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();
}
