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); } } } }