#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 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: %2.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]);
    
     printf("T: %2.f\r\n",MPU6050_GetTempValue());
    
    angle+=30;
    if(270<angle)angle=20;
    Servo_270_Set_Angle(angle);
        
    osDelay(2000); // 100ms读取一次
    
}

void task_2_beatdata(void){
    int16_t temp_int = (int16_t)(MPU6050_GetTempValue() * 100.0f);
    uint8_t temperature_low = (uint8_t)(temp_int & 0xFF);        // 0x03
    uint8_t temperature_high = (uint8_t)((temp_int >> 8) & 0xFF); // 0x0A

    int16_t vin_int = (int16_t)(Get_Vin_Voltage()* 100.0f);
    uint8_t vin_low = (uint8_t)(vin_int & 0xFF);
    uint8_t vin_high = (uint8_t)((vin_int >> 8) & 0xFF);

    if(beat_status == BEAT_START) USART2_Send_Command(0x00,temperature_high,temperature_low,vin_high,vin_low);
    osDelay(3000); // 100ms读取一次
}
