init:全链路闭环控制调通

project/user/src/main.c

@@ -39,20 +39,82 @@
 // 本例程是开源库空工程 可用作移植或者测试各类内外设
 
 // **************************** 代码区域 ****************************
+#include "Motor.h"
+#include "zf_device_key.h"
+#include "Task.h"
+#include "IMU.h"
+#include "Beep.h"
+#include "GrayscaleSensor.h"
+#include "MotorEncoder.h"
 
+/*
+定时器资源分配：
+G0 右电机PWM
+G7 左电机PWM
+A0 100Hz任务
+A1 50Hz任务
+G6 20Hz任务
+*/
+/*
+中断等级分配:
+0 GPIOA_INT_IRQn 外部中断模拟四倍频正交编码器
+1 TIMA0_INT_IRQn 100Hz任务
+1 TIMA1_INT_IRQn 50Hz任务
+1 UART2_INT_IRQn Command串口接收中断
+7 TIMG6_INT_IRQn 20Hz任务
+*/
 int main (void)
 {
     clock_init(SYSTEM_CLOCK_80M);   // 时钟配置及系统初始化<务必保留>
     debug_init();					// 调试串口信息初始化
 	// 此处编写用户代码 例如外设初始化代码等
+    beep_init(50);
+    motor_init();
+    key_init(50);//按键初始化，参数为扫描周期50ms
+    imu_init(200);// IMU 初始化，参数为采样频率 200Hz 需要与定时器中断频率一致
+    imu_calibrate_gyro(); // IMU 陀螺仪零偏校准 需要在静止状态下调用
+    grayscale_sensor_init();//灰度传感器初始化
+    motor_encoder_init();//电机编码器初始化
 
+    task_init();
 
-
+    beep_on_time(500);//初始化完成，蜂鸣器响 500ms
     // 此处编写用户代码 例如外设初始化代码等
-    while(true)
+    while(1)
     {
         // 此处编写需要循环执行的代码
-
+        system_delay_ms(100);
+        char str[20] = {0};
+        sprintf(str,"%d,%d,%d\n", (int)(imu_angle.yaw*100),(int)(yaw_ref*100),(int)(grayscale_sensor_position*100));
+        debug_send_buffer((const uint8 *)str,strlen(str));
+        if(KEY_SHORT_PRESS == key_get_state(KEY_1))
+        {
+            while(KEY_SHORT_PRESS == key_get_state(KEY_1));
+            // 等待按键释放
+            grayscale_sensor_set_white();
+            beep_on_time(200);
+        }
+        else if(KEY_SHORT_PRESS == key_get_state(KEY_2))
+        {
+            while(KEY_SHORT_PRESS == key_get_state(KEY_2));
+            // 等待按键释放
+            grayscale_sensor_set_black();
+            beep_on_time(200);
+        }
+        else if(KEY_SHORT_PRESS == key_get_state(KEY_3))
+        {
+            while(KEY_SHORT_PRESS == key_get_state(KEY_3));
+            // 等待按键释放
+            set_yaw_ref(yaw_ref + 5.0f);
+            beep_on_time(200);
+        }
+         else if(KEY_SHORT_PRESS == key_get_state(KEY_4))
+        {
+            while(KEY_SHORT_PRESS == key_get_state(KEY_4));
+            // 等待按键释放
+            set_yaw_ref(yaw_ref - 5.0f);
+            beep_on_time(200);
+        }
 
         // 此处编写需要循环执行的代码
     }