#include "task_manage.h"
#include "encoder.h"
#include "pwm.h"
#include "led.h"
#include "rgb.h"
#include "buzz.h"
#include "usart1.h"
#include "usart2.h"
#include "vin.h"
#include "mpu6050.h"
#include "servo.h"

int16_t acc[3], gyro[3];
float temperature;
float angle=90;
void task_1_main(void) { 
    Encoder_Update_All();
    printf("M0: %ld (%d) | M1: %ld (%d) | M2: %ld (%d) | M3: %ld (%d)\r\n",
            g_encoder[0].total_count, g_encoder[0].speed,
            g_encoder[1].total_count, g_encoder[1].speed,
            g_encoder[2].total_count, g_encoder[2].speed,
            g_encoder[3].total_count, g_encoder[3].speed);
    printf("VIN: %fV\r\n",Get_Vin_Voltage());

    
    MPU6050_GetAccData(acc);
    printf("acceleration: X=%d, Y=%d, Z=%d\r\n", acc[0], acc[1], acc[2]);
        
        
    MPU6050_GetGyroData(gyro);
    printf("gyroscope: X=%d, Y=%d, Z=%d\r\n", gyro[0], gyro[1], gyro[2]);
    
        
    temperature = MPU6050_GetTempValue();
    printf("temperature: %.2f\r\n", temperature);
    angle+=30;
    if(360<angle)angle=20;
    Servo_360_Set_Angle(angle);
        
    osDelay(2000); // 100ms读取一次
    
}
