btl143/QDAC-firmware
4
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge branch 'master' of http://git.isthmus.tk:441/btl143/firmware_violet_zf

Browse Source
This commit is contained in:
CaoWangrenbo
2023-12-24 22:21:15 +08:00
parent 3ea8776a2b 240ada9388
commit 519a7506e7
7 changed files with 310 additions and 65 deletions
Download Patch File Download Diff File Expand all files Collapse all files

192
app/by_imu.c Normal file
View File

@@ -0,0 +1,192 @@
#include "by_imu.h"
#include "zf_common_headfile.h"
#include <math.h>
#define delta_T 0.002f // 2ms 计算一次
float I_ex, I_ey, I_ez; // 误差积分
quater_param_t Q_info = {1, 0, 0}; // 全局四元数
euler_param_t eulerAngle; // 欧拉角
icm_param_t icm_data;
gyro_param_t GyroOffset;
bool GyroOffset_init = 0;
float param_Kp = 0.17; // 加速度计的收敛速率比例增益 0.17
float param_Ki = 0.004; // 陀螺仪收敛速率的积分增益 0.004
float fast_sqrt(float x)
{
float halfx = 0.5f * x;
float y = x;
long i = *(long *)&y;
i = 0x5f3759df - (i >> 1);
y = *(float *)&i;
y = y * (1.5f - (halfx * y * y));
return y;
}
void gyroOffset_init(void) /////////陀螺仪零飘初始化
{
float xdata_temp = 0;
float ydata_temp = 0;
float zdata_temp = 0;
imu660ra_get_gyro();
xdata_temp = imu660ra_gyro_x;
ydata_temp = imu660ra_gyro_y;
zdata_temp = imu660ra_gyro_z;
GyroOffset.Xdata = 0;
GyroOffset.Ydata = 0;
GyroOffset.Zdata = 0;
for (uint16_t i = 0; i < 5000; i++) {
imu660ra_get_gyro();
xdata_temp = imu660ra_gyro_x * 0.3f + xdata_temp * 0.7f;
ydata_temp = imu660ra_gyro_y * 0.3f + ydata_temp * 0.7f;
zdata_temp = imu660ra_gyro_z * 0.3f + zdata_temp * 0.7f;
GyroOffset.Xdata += xdata_temp;
GyroOffset.Ydata += ydata_temp;
GyroOffset.Zdata += zdata_temp;
system_delay_ms(5);
}
GyroOffset.Xdata /= 5000.0f;
GyroOffset.Ydata /= 5000.0f;
GyroOffset.Zdata /= 5000.0f;
// GyroOffset.Xdata = -1.034f;
// GyroOffset.Ydata = -2.21f;
// GyroOffset.Zdata = -3.15f;
GyroOffset_init = 1;
}
// 转化为实际物理值
void ICM_getValues()
{
#define alpha 0.2738f
// icm_data.acc_x = imu660ra_acc_x * alpha + icm_data.acc_x * (1 - alpha);
// icm_data.acc_y = imu660ra_acc_y * alpha + icm_data.acc_y * (1 - alpha);
// icm_data.acc_z = imu660ra_acc_z * alpha + icm_data.acc_z * (1 - alpha);
icm_data.acc_x = imu660ra_acc_x;
icm_data.acc_y = imu660ra_acc_y;
icm_data.acc_z = imu660ra_acc_z;
icm_data.gyro_x = icm_data.gyro_x * alpha + ((imu660ra_gyro_x - GyroOffset.Xdata) / 939.65f) * (1 - alpha);
icm_data.gyro_y = icm_data.gyro_y * alpha + ((imu660ra_gyro_y - GyroOffset.Ydata) / 939.65f) * (1 - alpha);
icm_data.gyro_z = icm_data.gyro_z * alpha + ((imu660ra_gyro_z - GyroOffset.Zdata) / 939.65f) * (1 - alpha);
}
// 互补滤波
void ICM_AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az)
{
float halfT = 0.5 * delta_T;
float vx, vy, vz; // 当前的机体坐标系上的重力单位向量
float ex, ey, ez; // 四元数计算值与加速度计测量值的误差
float q0 = Q_info.q0;
float q1 = Q_info.q1;
float q2 = Q_info.q2;
float q3 = Q_info.q3;
float q0q0 = q0 * q0;
float q0q1 = q0 * q1;
float q0q2 = q0 * q2;
// float q0q3 = q0 * q3;
float q1q1 = q1 * q1;
// float q1q2 = q1 * q2;
float q1q3 = q1 * q3;
float q2q2 = q2 * q2;
float q2q3 = q2 * q3;
float q3q3 = q3 * q3;
// float delta_2 = 0;
// 对加速度数据进行归一化 得到单位加速度
float norm = fast_sqrt(ax * ax + ay * ay + az * az);
ax = ax * norm;
ay = ay * norm;
az = az * norm;
// 根据当前四元数的姿态值来估算出各重力分量。用于和加速计实际测量出来的各重力分量进行对比，从而实现对四轴姿态的修正
vx = 2.0f * (q1q3 - q0q2);
vy = 2.0f * (q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3;
// vz = 1.0f - 2.0f * q1q1 - 2.0f * q2q2;
// vz = (q0*q0-0.5f+q3 * q3) * 2;
// 叉积来计算估算的重力和实际测量的重力这两个重力向量之间的误差。
ex = ay * vz - az * vy;
ey = az * vx - ax * vz;
ez = ax * vy - ay * vx;
// 用叉乘误差来做 PI 修正陀螺零偏，
// 通过调节 param_Kp，param_Ki 两个参数，
// 可以控制加速度计修正陀螺仪积分姿态的速度。
I_ex += halfT * ex; // integral error scaled by Ki
I_ey += halfT * ey;
I_ez += halfT * ez;
gx = gx + param_Kp * ex + param_Ki * I_ex;
gy = gy + param_Kp * ey + param_Ki * I_ey;
gz = gz + param_Kp * ez + param_Ki * I_ez;
/*数据修正完成，下面是四元数微分方程*/
// 四元数微分方程，其中 halfT 为测量周期的 1/2，gx gy gz 为陀螺仪角速度，以下都是已知量，这里使用了一阶龙哥库塔求解四元数微分方程
q0 = q0 + (-q1 * gx - q2 * gy - q3 * gz) * halfT;
q1 = q1 + (q0 * gx + q2 * gz - q3 * gy) * halfT;
q2 = q2 + (q0 * gy - q1 * gz + q3 * gx) * halfT;
q3 = q3 + (q0 * gz + q1 * gy - q2 * gx) * halfT;
// delta_2=(2*halfT*gx)*(2*halfT*gx)+(2*halfT*gy)*(2*halfT*gy)+(2*halfT*gz)*(2*halfT*gz);
// 整合四元数率 四元数微分方程 四元数更新算法，二阶毕卡法
// q0 = (1-delta_2/8)*q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
// q1 = (1-delta_2/8)*q1 + (q0*gx + q2*gz - q3*gy)*halfT;
// q2 = (1-delta_2/8)*q2 + (q0*gy - q1*gz + q3*gx)*halfT;
// q3 = (1-delta_2/8)*q3 + (q0*gz + q1*gy - q2*gx)*halfT
// normalise quaternion
norm = fast_sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
Q_info.q0 = q0 * norm;
Q_info.q1 = q1 * norm;
Q_info.q2 = q2 * norm;
Q_info.q3 = q3 * norm;
}
void ICM_getEulerianAngles(void)
{
// 采集陀螺仪数据
imu660ra_get_acc();
imu660ra_get_gyro();
imu660ra_get_temperature();
ICM_getValues();
ICM_AHRSupdate(icm_data.gyro_x, icm_data.gyro_y, icm_data.gyro_z, icm_data.acc_x, icm_data.acc_y, icm_data.acc_z);
float q0 = Q_info.q0;
float q1 = Q_info.q1;
float q2 = Q_info.q2;
float q3 = Q_info.q3;
// 四元数计算欧拉角
eulerAngle.pitch = asin(-2 * q1 * q3 + 2 * q0 * q2) * 180 / M_PI; // pitch
eulerAngle.roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 180 / M_PI; // roll
eulerAngle.yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2 * q2 - 2 * q3 * q3 + 1) * 180 / M_PI; // yaw
/* 姿态限制*/
if (eulerAngle.roll > 90 || eulerAngle.roll < -90) {
if (eulerAngle.pitch > 0) {
eulerAngle.pitch = 180 - eulerAngle.pitch;
}
if (eulerAngle.pitch < 0) {
eulerAngle.pitch = -(180 + eulerAngle.pitch);
}
}
if (eulerAngle.yaw > 360) {
eulerAngle.yaw -= 360;
} else if (eulerAngle.yaw < 0) {
eulerAngle.yaw += 360;
}
}

47
app/by_imu.h Normal file
View File

@@ -0,0 +1,47 @@
#ifndef _BY_IMU_H__
#define _BY_IMU_H__
#include <stdio.h>
#include "ch32v30x.h"
typedef struct {
float gyro_x;
float gyro_y;
float gyro_z;
float acc_x;
float acc_y;
float acc_z;
} icm_param_t;
typedef struct {
float q0;
float q1;
float q2;
float q3;
} quater_param_t;
typedef struct {
float pitch; //俯仰角
float roll; //偏航角
float yaw; //翻滚角
} euler_param_t;
typedef struct {
float Xdata;
float Ydata;
float Zdata;
} gyro_param_t;
extern icm_param_t icm_data;
extern euler_param_t eulerAngle;
extern gyro_param_t GyroOffset;
void gyroOffset_init(void);
float fast_sqrt(float x);
void ICM_AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az);
void ICM_getValues();
void ICM_getEulerianAngles(void);
#endif

34
app/by_pt_button.c
View File

@@ -1,6 +1,8 @@
#include "by_pt_button.h" #include "by_pt_button.h"
#include "zf_common_headfile.h" #include "zf_common_headfile.h"
#include "by_imu.h"
uint8_t potate_button; uint8_t potate_button;
void by_gpio_init(void) void by_gpio_init(void)
{ {
gpio_init(E10, GPI, GPIO_HIGH, GPI_PULL_UP); gpio_init(E10, GPI, GPIO_HIGH, GPI_PULL_UP);
@@ -21,7 +23,31 @@ uint8_t by_get_pb_statu(void)
void by_ips_show(void) void by_ips_show(void)
{ {
ips114_show_string(0, 0, "button statu:"); ips200_show_string(0, 0, "button statu:");
ips114_show_uint(104, 0, by_get_pb_statu(), 1); // ips200_show_uint(104, 0, by_get_pb_statu(), 1);
switch (by_get_pb_statu()) {
} case 1:
ips200_show_string(104, 0, "press");
break;
case 2:
ips200_show_string(104, 0, "up ");
break;
case 3:
ips200_show_string(104, 0, "down ");
break;
default:
ips200_show_string(104, 0, " ");
break;
}
// 按钮
ips200_show_string(0, 16, "imu:");
ips200_show_float(46, 32, eulerAngle.pitch, 3, 2);
ips200_show_float(46, 48, eulerAngle.roll, 3, 2);
ips200_show_float(46, 64, eulerAngle.yaw, 3, 2);
// ips200_show_float(46 , 32, icm_data.gyro_x, 2, 2);
// ips200_show_float(106, 32, icm_data.gyro_y, 2, 2);
// ips200_show_float(166, 32, icm_data.gyro_z, 2, 2);
ips200_show_float(46, 80, imu660ra_temperature, 2, 2);
// printf("%d,%d,%d\n", (int16_t)(icm_data.gyro_x * 10), (int16_t)(icm_data.gyro_y * 10), (int16_t)(icm_data.gyro_z * 10));
}

4
app/by_pt_button.h
View File

@@ -8,8 +8,10 @@
#define POTATE_BUTTOM_BACKWARD 3 #define POTATE_BUTTOM_BACKWARD 3
extern uint8_t potate_button; extern uint8_t potate_button;
extern void by_exit_init(void); extern void by_exit_init(void);
extern void by_gpio_init(void); extern void by_gpio_init(void);
extern uint8_t by_get_pb_statu(void); extern uint8_t by_get_statu(void);
extern void by_ips_show(void); extern void by_ips_show(void);
#endif #endif

7
app/isr.c
View File

@@ -195,14 +195,14 @@ void EXTI9_5_IRQHandler(void)
EXTI_ClearITPendingBit(EXTI_Line8); EXTI_ClearITPendingBit(EXTI_Line8);
} }
if (SET == EXTI_GetITStatus(EXTI_Line9)) { if (SET == EXTI_GetITStatus(EXTI_Line9)) {
EXTI_ClearITPendingBit(EXTI_Line9);
if (SET == gpio_get_level(E10)) { if (SET == gpio_get_level(E10)) {
potate_button = POTATE_BUTTOM_BACKWARD; potate_button = POTATE_BUTTOM_BACKWARD;
} else { } else {
potate_button = POTATE_BUTTOM_FOREWARD; potate_button = POTATE_BUTTOM_FOREWARD;
} }
EXTI_ClearITPendingBit(EXTI_Line9);
} }
} }
@@ -217,12 +217,12 @@ void EXTI15_10_IRQHandler(void)
EXTI_ClearITPendingBit(EXTI_Line10); EXTI_ClearITPendingBit(EXTI_Line10);
} }
if (SET == EXTI_GetITStatus(EXTI_Line11)) { if (SET == EXTI_GetITStatus(EXTI_Line11)) {
EXTI_ClearITPendingBit(EXTI_Line11);
system_delay_us(200); system_delay_us(200);
if (SET == !gpio_get_level(E11)) { if (SET == !gpio_get_level(E11)) {
potate_button = POTATE_BUTTOM_PRESS; potate_button = POTATE_BUTTOM_PRESS;
} }
EXTI_ClearITPendingBit(EXTI_Line11);
} }
if (SET == EXTI_GetITStatus(EXTI_Line12)) { if (SET == EXTI_GetITStatus(EXTI_Line12)) {
EXTI_ClearITPendingBit(EXTI_Line12); EXTI_ClearITPendingBit(EXTI_Line12);
@@ -289,6 +289,7 @@ void TIM5_IRQHandler(void)
void TIM6_IRQHandler(void) void TIM6_IRQHandler(void)
{ {
if (TIM_GetITStatus(TIM6, TIM_IT_Update) != RESET) { if (TIM_GetITStatus(TIM6, TIM_IT_Update) != RESET) {
ICM_getEulerianAngles();
TIM_ClearITPendingBit(TIM6, TIM_IT_Update); TIM_ClearITPendingBit(TIM6, TIM_IT_Update);
} }
} }

86
app/main.c
View File

@@ -1,36 +1,36 @@
/********************************************************************************************************************* /*********************************************************************************************************************
* CH32V307VCT6 Opensourec Library <EFBFBD><EFBFBD><EFBFBD><EFBFBD>CH32V307VCT6 <EFBFBD><EFBFBD>Դ<EFBFBD>⣩<EFBFBD><EFBFBD>һ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڹٷ<EFBFBD> SDK <20>ӿڵĵ<DAB5><C4B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Դ<EFBFBD><D4B4> * CH32V307VCT6 Opensourec Library ????CH32V307VCT6 ???????????????? SDK ??????????????
* Copyright (c) 2022 SEEKFREE <EFBFBD><EFBFBD><EFBFBD>ɿƼ<EFBFBD> * Copyright (c) 2022 SEEKFREE ?????
* *
* <EFBFBD><EFBFBD><EFBFBD>ļ<EFBFBD><EFBFBD><EFBFBD> CH32V307VCT6 <EFBFBD><EFBFBD>Դ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD><EFBFBD><EFBFBD><EFBFBD> * ??????? CH32V307VCT6 ???????????
* *
* CH32V307VCT6 <EFBFBD><EFBFBD>Դ<EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> * CH32V307VCT6 ????? ?????????
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ը<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ᷢ<EFBFBD><EFBFBD><EFBFBD><EFBFBD> GPL<EFBFBD><EFBFBD>GNU General Public License<EFBFBD><EFBFBD><EFBFBD><EFBFBD> GNU ͨ<EFBFBD>ù<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>֤<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> * ?????????????????????????? GPL??GNU General Public License???? GNU ??<3F><><EFBFBD><EFBFBD>???????????????
* <EFBFBD><EFBFBD> GPL <EFBFBD>ĵ<EFBFBD> 3 <20>棨<EFBFBD><E6A3A8> GPL3.0<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ѡ<EFBFBD><EFBFBD><EFBFBD>ģ<EFBFBD><EFBFBD>κκ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>İ汾<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>·<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>/<2F><><EFBFBD>޸<EFBFBD><DEB8><EFBFBD> * ?? GPL ??? 3 ?<3F><><EFBFBD><EFBFBD>? GPL3.0????????????<3F>ʦ<EFBFBD><CAA6><EFBFBD>????<3F><><EFBFBD><EFBFBD>????<3F><><EFBFBD><EFBFBD>????/???????
* *
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD>Դ<EFBFBD><EFBFBD><EFBFBD>ķ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϣ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܷ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ã<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>δ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>κεı<EFBFBD>֤ * ????????????????????????????????<3F><><EFBFBD><EFBFBD>??????<3F>ʦ<EFBFBD><CAA6><EFBFBD>??
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ի<EFBFBD><EFBFBD>ʺ<EFBFBD><EFBFBD>ض<EFBFBD><EFBFBD><EFBFBD>;<EFBFBD>ı<EFBFBD>֤ * ?????????????????????????????????
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϸ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>μ<EFBFBD> GPL * ?????????<EFBFBD><EFBFBD><EFBFBD><EFBFBD>GPL
* *
* <EFBFBD><EFBFBD>Ӧ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>յ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Դ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͬʱ<EFBFBD>յ<EFBFBD>һ<EFBFBD><EFBFBD> GPL <20>ĸ<EFBFBD><C4B8><EFBFBD> * ?????????????????????????? GPL ?????
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD>У<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><https://www.gnu.org/licenses/> * ?????<3F><><EFBFBD><EFBFBD>?????<https://www.gnu.org/licenses/>
* *
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD>ע<EFBFBD><EFBFBD><EFBFBD><EFBFBD> * ?????????
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD>Դ<EFBFBD><EFBFBD>ʹ<EFBFBD><EFBFBD> GPL3.0 <20><>Դ<EFBFBD><D4B4><EFBFBD><EFBFBD>֤Э<D6A4><D0AD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD>İ汾 * ?????????? GPL3.0 ????????<3F><><EFBFBD><EFBFBD>? ?????????????????<3F><>
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӣ<EFBFBD>İ<EFBFBD><EFBFBD><EFBFBD> libraries/doc <EFBFBD>ļ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>µ<EFBFBD> GPL3_permission_statement.txt <EFBFBD>ļ<EFBFBD><EFBFBD><EFBFBD> * ?????????????? libraries/doc ???????? GPL3_permission_statement.txt ?????
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD>֤<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> libraries <20>ļ<EFBFBD><C4BC><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD>ļ<EFBFBD><C4BC><EFBFBD><EFBFBD>µ<EFBFBD> LICENSE <EFBFBD>ļ<EFBFBD> * ??????????? libraries ??????? ???????????? LICENSE ???
* <EFBFBD><EFBFBD>ӭ<EFBFBD><EFBFBD>λʹ<EFBFBD>ò<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD>޸<EFBFBD><DEB8><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD>뱣<EFBFBD><EBB1A3><EFBFBD><EFBFBD><EFBFBD>ɿƼ<C9BF><C6BC>İ<EFBFBD>Ȩ<EFBFBD><C8A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> * ?????<EFBFBD><EFBFBD><EFBFBD><EFBFBD>?<EFBFBD><EFBFBD><EFBFBD><EFBFBD>?????????? ?????????????????????????????????????????
* *
* <EFBFBD>ļ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> main * ??????? main
* <EFBFBD><EFBFBD>˾<EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD>ɶ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɿƼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>޹<EFBFBD>˾ * ??????? ??????????????
* <EFBFBD>汾<EFBFBD><EFBFBD>Ϣ <EFBFBD>鿴 libraries/doc <EFBFBD>ļ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> version <EFBFBD>ļ<EFBFBD> <20>汾˵<E6B1BE><CBB5> * ?<3F><><EFBFBD><EFBFBD>?? ?? libraries/doc ??????? version ??? ?<3F><><EFBFBD><EFBFBD>??
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> MounRiver Studio V1.8.1 * ???????? MounRiver Studio V1.8.1
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽ̨ CH32V307VCT6 * ?????? CH32V307VCT6
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> https://seekfree.taobao.com/ * ???????? https://seekfree.taobao.com/
* *
* <EFBFBD>޸ļ<EFBFBD>¼ * ?????
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD><EFBFBD><EFBFBD> <EFBFBD><EFBFBD>ע * ???? ???? ???
* 2022-09-15 <EFBFBD><EFBFBD> W first version * 2022-09-15 ?? W first version
********************************************************************************************************************/ ********************************************************************************************************************/
#include "zf_common_headfile.h" #include "zf_common_headfile.h"
#include "gl_headfile.h" #include "gl_headfile.h"
@@ -39,9 +39,10 @@
#include "cw_servo.h" #include "cw_servo.h"
#include "./page/cw_page.h" #include "./page/cw_page.h"
uint8_t (*Img_Gray)[MT9V03X_W]; // <20><><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD> MT9V03X_W <20>е<EFBFBD> uint8_t <20><><EFBFBD>͵Ķ<CDB5>ά<EFBFBD><CEAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8> uint8_t (*Img_Gray)[MT9V03X_W];
// uint8_t *mt9v03x_image_copy[0]; // <20><><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8> uint8_t <20><><EFBFBD>͵<EFBFBD>һά<D2BB><CEAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8> // uint8_t *mt9v03x_image_copy[0];
int32_t pts_left[PT_MAXLEN][2], pts_right[PT_MAXLEN][2]; int32_t pts_left[PT_MAXLEN][2];
int32_t pts_right[PT_MAXLEN][2];
int32_t pts_left_count, pts_right_count; int32_t pts_left_count, pts_right_count;
float pts_inv_l[PT_MAXLEN][2], pts_inv_r[PT_MAXLEN][2]; float pts_inv_l[PT_MAXLEN][2], pts_inv_r[PT_MAXLEN][2];
int32_t pts_inv_l_count, pts_inv_r_count; int32_t pts_inv_l_count, pts_inv_r_count;
@@ -52,18 +53,15 @@ int32_t pts_resample_left_count, pts_resample_right_count;
float mid_left[PT_MAXLEN][2], mid_right[PT_MAXLEN][2]; float mid_left[PT_MAXLEN][2], mid_right[PT_MAXLEN][2];
int32_t mid_left_count, mid_right_count; int32_t mid_left_count, mid_right_count;
// <20><><EFBFBD>ұ<EFBFBD><D2B1>߾ֲ<DFBE><D6B2>Ƕȱ仯<C8B1><E4BBAF> + <20>Ǽ<EFBFBD><C7BC><EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>
float angle_new_left[PT_MAXLEN]; float angle_new_left[PT_MAXLEN];
float angle_new_right[PT_MAXLEN]; float angle_new_right[PT_MAXLEN];
int angle_new_left_num, angle_new_right_num; int angle_new_left_num, angle_new_right_num;
uint8_t mt9v03x_image_copy[MT9V03X_H][MT9V03X_W]; uint8_t mt9v03x_image_copy[MT9V03X_H][MT9V03X_W];
// <20><><EFBFBD>ұ<EFBFBD><D2B1>߾ֲ<DFBE><D6B2>Ƕȱ仯<C8B1><E4BBAF>
float angle_left[PT_MAXLEN]; float angle_left[PT_MAXLEN];
float angle_right[PT_MAXLEN]; float angle_right[PT_MAXLEN];
int angle_left_num, angle_right_num; int angle_left_num, angle_right_num;
// L <20>ǵ<EFBFBD>
int Lpt0_rpts0s_id, Lpt1_rpts1s_id; int Lpt0_rpts0s_id, Lpt1_rpts1s_id;
bool Lpt0_found, Lpt1_found; bool Lpt0_found, Lpt1_found;
int Lpt1[2], Lpt0[2]; int Lpt1[2], Lpt0[2];
@@ -72,13 +70,10 @@ int Lpt_in0_rpts0s_id, Lpt_in1_rpts1s_id;
bool Lpt_in0_found, Lpt_in1_found; bool Lpt_in0_found, Lpt_in1_found;
int Lpt_in1[2], Lpt_in0[2]; int Lpt_in1[2], Lpt_in0[2];
// <20><>ֱ<EFBFBD><D6B1>
bool is_straight0, is_straight1; bool is_straight0, is_straight1;
// <20><><EFBFBD><EFBFBD>
bool is_turn0, is_turn1; bool is_turn0, is_turn1;
// <20><>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
float rptsn[PT_MAXLEN][2]; float rptsn[PT_MAXLEN][2];
int rptsn_num; int rptsn_num;
float aim_distance; float aim_distance;
@@ -92,36 +87,24 @@ void get_corners();
int main(void) int main(void)
{ {
clock_init(SYSTEM_CLOCK_120M); // <20><>ʼ<EFBFBD><CABC>оƬʱ<C6AC><CAB1> <20><><EFBFBD><EFBFBD>Ƶ<EFBFBD><C6B5>Ϊ 120MHz clock_init(SYSTEM_CLOCK_120M);
debug_init(); // <20><><EFBFBD>ر<EFBFBD><D8B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڳ<EFBFBD>ʼ<EFBFBD><CABC> MPU ʱ<><CAB1> <20><><EFBFBD>Դ<EFBFBD><D4B4><EFBFBD> debug_init();
mt9v03x_init(); mt9v03x_init();
ips200_init(IPS200_TYPE_SPI); ips200_init(IPS200_TYPE_SPI);
by_gpio_init(); by_gpio_init();
by_exit_init(); by_exit_init();
by_pwm_init(); by_pwm_init();
cw_servo_init(); cw_servo_init();
// while (imu660ra_init())
// ;
Page_Init(); Page_Init();
while (1) { while (1) {
Page_Run(); Page_Run();
// while (frame_count < 20) {
// if (mt9v03x_finish_flag) {
// memcpy(mt9v03x_image_copy[0], mt9v03x_image[0], (sizeof(mt9v03x_image_copy) / sizeof(uint8_t)));
// adaptiveThreshold((uint8_t*)mt9v03x_image_copy, (uint8_t*)mt9v03x_image_copy, 188, 120, 7, 8);
// //threshold((uint8_t*)mt9v03x_image_copy, (uint8_t*)mt9v03x_image_copy, MT9V03X_W, MT9V03X_H, 110);
// ips114_show_gray_image(0, 0, mt9v03x_image_copy[0], MT9V03X_W, MT9V03X_H, MT9V03X_W, MT9V03X_H, 0);
// mt9v03x_finish_flag = 0;
// frame_count++;
// }
//}
/************************ ͼ<><CDBC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>غ<EFBFBD><D8BA><EFBFBD> ***************************/
if (mt9v03x_finish_flag) { if (mt9v03x_finish_flag) {
memcpy(mt9v03x_image_copy[0], mt9v03x_image[0], (sizeof(mt9v03x_image_copy) / sizeof(uint8_t))); memcpy(mt9v03x_image_copy[0], mt9v03x_image[0], (sizeof(mt9v03x_image_copy) / sizeof(uint8_t)));
// ips114_show_gray_image(0, 0, mt9v03x_image[0], 188, 120, 188, 120,0);
// Img_Gray = mt9v03x_image;
// mt9v03x_image_copy[0] = Img_Gray[0];
mt9v03x_finish_flag = 0; mt9v03x_finish_flag = 0;
state_type = COMMON_STATE; state_type = COMMON_STATE;
@@ -133,6 +116,5 @@ int main(void)
ElementRun(); ElementRun();
MidLineTrack(); MidLineTrack();
} }
/************************ ͼ<><CDBC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>غ<EFBFBD><D8BA><EFBFBD> ***************************/
} }
} }

5
libraries/zf_device/zf_device_ips200.h
View File

@@ -85,11 +85,6 @@
#endif #endif
// 如果使用的是单排排针的两寸屏幕 SPI 驱动控制引脚 可以修改 // 如果使用的是单排排针的两寸屏幕 SPI 驱动控制引脚 可以修改
// #define IPS200_RST_PIN_SPI (B7 ) // 液晶复位引脚定义
// #define IPS200_DC_PIN_SPI (D7 ) // 液晶命令位引脚定义
// #define IPS200_CS_PIN_SPI (D4 )
// #define IPS200_BLk_PIN_SPI (D0 )
#define IPS200_RST_PIN_SPI (D8 ) // 液晶复位引脚定义 #define IPS200_RST_PIN_SPI (D8 ) // 液晶复位引脚定义
#define IPS200_DC_PIN_SPI (D9 ) // 液晶命令位引脚定义 #define IPS200_DC_PIN_SPI (D9 ) // 液晶命令位引脚定义
#define IPS200_CS_PIN_SPI (D10 ) #define IPS200_CS_PIN_SPI (D10 )