int main(void) {
/* Init system clock for maximum system speed */
TM_RCC_InitSystem();
/* Init HAL layer */
HAL_Init();
/* Init leds */
TM_DISCO_LedInit();
/* Init button */
TM_DISCO_ButtonInit();
/* For pinouts, check TM_MPU6050 library */
/* Try to init MPU6050, device address is 0xD0, AD0 pin is set to low */
if (TM_MPU6050_Init(&MPU6050, TM_MPU6050_Device_0, TM_MPU6050_Accelerometer_8G, TM_MPU6050_Gyroscope_250s) == TM_MPU6050_Result_Ok) {
/* Green LED on */
TM_DISCO_LedOn(LED_GREEN);
}
while (1) {
/* Read everything from device */
TM_MPU6050_ReadAll(&MPU6050);
/* Raw data are available for use as needed */
//MPU6050.Accelerometer_X;
//MPU6050.Accelerometer_Y;
//MPU6050.Accelerometer_Z;
//MPU6050.Gyroscope_X;
//MPU6050.Gyroscope_Y;
//MPU6050.Gyroscope_Z;
//MPU6050.Temperature;
/* Delay a little */
Delayms(1);
}
}
void MAIN_PROG (uint16_t time)
{
static uint16_t ticker = 0;
if ((uint16_t)(ticker+time)== GetTicker())
{
ticker = GetTicker();
//Encoder section
ENC_PROCESSING (&Left);
ENC_PROCESSING (&Right);
Linear.velo_raw = (Left.velo_fb - Right.velo_fb)/ 2.0f;
Smooth_filter (Linear.velo_raw,Linear.velo_filted,0.1);
PID_VELO_RUN.feedback = Linear.velo_filted[0];
PID_VELO_HOLD.feedback = Linear.velo_filted[0];
Linear.posi_raw = (Left.posi_fb - Right.posi_fb)/ 2.0f;
Smooth_filter (Linear.posi_raw,Linear.posi_filted,0.1);
PID_POSI.feedback = Linear.posi_filted[0];
/*MPU 1*/
/* Read all data */
MPU6050_Status = TM_MPU6050_ReadAll(&MPU6050_Data0);
//Pitch
/* Complementary filter*/
Pitch.raw = (0.98f * (Pitch.raw - MPU6050_Data0.Gyroscope_Y * dt)) + (0.02f * (atan2f(MPU6050_Data0.Accelerometer_X, MPU6050_Data0.Accelerometer_Z) * 180.0f/PI));
Smooth_filter (Pitch.raw,Pitch.filted_theta,0.25);
PID_TILT.feedback = Pitch.filted_theta[0] - Pitch.theta_offset;
//Yaw
// Yaw.raw = - MPU6050_Data0.Gyroscope_Z;
// /* HighPass filter*/
// Yaw.filted_theta[0] = HPF(Yaw.raw,10,f_s);
// PID_TURN.feedback = Yaw.filted_theta[0] - Yaw.theta_offset;
if ((PID_TILT.feedback < 45) && (PID_TILT.feedback > -45))
{
//Preprocessing
if ((setspeed-PID_VELO_RUN.set_point) > 0)
{
PID_VELO_RUN.set_point += 0.5f;
}
if ((setspeed-PID_VELO_RUN.set_point) < 0)
{
PID_VELO_RUN.set_point -= 0.5f;
}
if (PID_VELO_RUN.set_point == 0)
{
//PID POSITION
PID (&PID_POSI,0.34);
PID_VELO_HOLD.set_point = PID_POSI.filted_U[0] * MAX_VELO;
//PID VELOCITY
PID (&PID_VELO_HOLD,0.34);
PID_TILT.set_point = -PID_VELO_HOLD.filted_U[0] * MAX_TILT;
}
else
{
//PID VELOCITY
PID (&PID_VELO_RUN,0.34);
PID_TILT.set_point = -PID_VELO_RUN.filted_U[0] * MAX_TILT;
}
//PID TILTING
PID (&PID_TILT,0.36);
//Turning
//PID (&PID_TURN,0.12);
U_offset = -setturn*TURN_OFFSET;
//FUSION
Left_motor.PWM = PID_TILT.filted_U[0] + U_offset;
Right_motor.PWM = PID_TILT.filted_U[0] - U_offset;
//PWM section
PWM (&Left_motor);
PWM (&Right_motor);
}
else
{
TIM_SetCompare1(TIM4,0);
TIM_SetCompare2(TIM4,0);
TIM_SetCompare3(TIM4,0);
TIM_SetCompare4(TIM4,0);
GPIO_ResetBits(GPIOD,GPIO_Pin_12);
GPIO_ResetBits(GPIOD,GPIO_Pin_13);
GPIO_ResetBits(GPIOD,GPIO_Pin_14);
GPIO_ResetBits(GPIOD,GPIO_Pin_15);
ResetPID (&PID_TILT);
ResetPID (&PID_VELO_HOLD);
ResetPID (&PID_VELO_RUN);
ResetPID (&PID_POSI);
}
}
}
int main(void) {
TM_MPU6050_t MPU6050_Data0;
TM_MPU6050_t MPU6050_Data1;
uint8_t sensor1 = 0, sensor2 = 0;
char str[120];
/* Initialize system */
SystemInit();
/* Initialize delay */
TM_DELAY_Init();
/* Initialize USART, TX: PB6 */
TM_USART_Init(USART1, TM_USART_PinsPack_2, 115200);
/* Initialize MPU6050 sensor 0, address = 0xD0, AD0 pin on sensor is low */
if (TM_MPU6050_Init(&MPU6050_Data0, TM_MPU6050_Device_0, TM_MPU6050_Accelerometer_8G, TM_MPU6050_Gyroscope_250s) == TM_MPU6050_Result_Ok) {
/* Display message to user */
TM_USART_Puts(USART1, "MPU6050 sensor 0 is ready to use!\n");
/* Sensor 1 OK */
sensor1 = 1;
}
/* Initialize MPU6050 sensor 1, address = 0xD2, AD0 pin on sensor is high */
if (TM_MPU6050_Init(&MPU6050_Data1, TM_MPU6050_Device_1, TM_MPU6050_Accelerometer_8G, TM_MPU6050_Gyroscope_250s) == TM_MPU6050_Result_Ok) {
/* Display message to user */
TM_USART_Puts(USART1, "MPU6050 sensor 1 is ready to use!\n");
/* Sensor 2 OK */
sensor2 = 1;
}
while (1) {
/* Every 500ms */
if (TM_DELAY_Time() >= 500) {
/* Reset time */
TM_DELAY_SetTime(0);
/* If sensor 1 is connected */
if (sensor1) {
/* Read all data from sensor 1 */
TM_MPU6050_ReadAll(&MPU6050_Data0);
/* Format data */
sprintf(str, "1. Accelerometer\n- X:%d\n- Y:%d\n- Z:%d\nGyroscope\n- X:%d\n- Y:%d\n- Z:%d\nTemperature\n- %3.4f\n\n\n",
MPU6050_Data0.Accelerometer_X,
MPU6050_Data0.Accelerometer_Y,
MPU6050_Data0.Accelerometer_Z,
MPU6050_Data0.Gyroscope_X,
MPU6050_Data0.Gyroscope_Y,
MPU6050_Data0.Gyroscope_Z,
MPU6050_Data0.Temperature
);
/* Show to usart */
TM_USART_Puts(USART1, str);
}
/* If sensor 2 is connected */
if (sensor2) {
/* Read all data from sensor 1 */
TM_MPU6050_ReadAll(&MPU6050_Data1);
/* Format data */
sprintf(str, "2. Accelerometer\n- X:%d\n- Y:%d\n- Z:%d\nGyroscope\n- X:%d\n- Y:%d\n- Z:%d\nTemperature\n- %3.4f\n\n\n",
MPU6050_Data1.Accelerometer_X,
MPU6050_Data1.Accelerometer_Y,
MPU6050_Data1.Accelerometer_Z,
MPU6050_Data1.Gyroscope_X,
MPU6050_Data1.Gyroscope_Y,
MPU6050_Data1.Gyroscope_Z,
MPU6050_Data1.Temperature
);
/* Show to usart */
TM_USART_Puts(USART1, str);
}
}
}
}
