//在【原创】智能小船的驱动--from华-电子产品世界论坛原帖，有些不详细，所以注释和整理了一下帖子，方便有硬件的测评、学习，
//
//智能小船的驱动
//CV捕捉到目标后给我的一个信号反馈，然后算出目标离小船的角度，后将数据进行运算，控制小船原地旋转通过mpu6050导致小船达到目标角度为了精确加入了pid算法
#include <stdio.h>
#include "stm32f10x.h"
#include <math.h>
#include "MPU6050.h"
// Device header
uint8_t ID;
//定义用于存放ID号的变量
int16_t AX, AV, AZ, GX, GY, GZ;
//定义用于存放各个数据的变量
double calculate_angle(double opposite, double adjacent)
{
return (3.1415926/180.0)*atan2(opposite,adjacent);
}
int angle()
{
MPU6050_GetData(&AX, &AY, &AZ, &GX, &GY, &GZ);
double a_b;
a=AX-GX;
b=AY-GY;
printf("三角形两边长度：
");
scaf("%lf %lf",a_b,a,b);
if(a==0||b==0)
{
printf("输入错误
");
return 0;
}
double angle = calculate_angle (a_b);
}
printf("对应角度%.2f
",angle);
return 0;
}
int Incremental_PI(int now,int aim)
{
float KP,KI;
KP=0.7;
KI=0.05;
static float Bias,angle,Last_bias;
Bias=now-aim;
angle+=KP*(Bias-Last_bias)+KI*Bias; //增量式PI控制器
Last_bias=Bias; //保存上一次偏差
return angle; //增量输出
}
//如果当旋转角度与误差小于10的时候就直接进入case3，防止惯性，如果当他差值大于10进入case1，小于—10进入case3
void Motor_Stra()
{
// 使用switch语句根据direction的值选择不同的操作
switch(direction)
{
case 1:
// 如果direction为1，设置电机2的速度为10，电机1的速度为-10
Motor_SetSpeed2(10);
Motor_SetSpeed1(-10);
break; // 结束当前case，不再执行后续代码
case 2:
// 如果direction为2，设置电机2的速度为-10，电机1的速度为10
Motor_SetSpeed2(-10);
Motor_SetSpeed1(10);
break; // 结束当前case，不再执行后续代码
case 3:
// 如果direction为3，停止两个电机
Motor_SetSpeed2(0);
Motor_SetSpeed1(0);
break; // 结束当前case，不再执行后续代码
}
}
//然后如果达到目标角度一段时间过后小船进入下一阶段直线行驶在直线行驶的过程中此时用mpu6050自带的姿态角读出值控制电机旋转同时控制舵机让舵机与姿态角反方向旋转控制小船向正确方向旋转
void Motor_ing()
{
Motor_SetSpeed2(50+Yaw-aim); // 设置电机2的速度为50加上Yaw减去aim
Motor_SetSpeed1(50-Yaw-aim); // 设置电机1的速度为50减去Yaw再减去aim
}
//主函数的运行
if(mode==1) // 检查模式是否为1
{
Motor_Strat(); // 调用Motor_Strat函数，可能用于启动电机
OLED_ShowSignedNum(1,7,angle,3); // 在OLED显示屏上显示角度值，位置为(1,7)，显示3位数字
OLED_ShowString(1,"angle"); // 在OLED显示屏上显示字符串"angle"，位置为(1,0)
for(n=1;angle==aim;++n) // 循环直到角度等于目标值
{
if(n>137) // 如果循环次数超过137次
{
mode=2; // 改变模式为2
}
}
}
if(mode==2) // 检查模式是否为2
{
if(mode==2) // 再次检查模式是否为2（这个条件总是为真）
{
Angle_D=aim-Yaw; // 计算角度差值并赋值给Angle_D
OLED_ShowString(1,1,"Yaw"); // 在OLED显示屏上显示字符串"Yaw"，位置为(1,0)
OLED_ShowSignedNum(1,7,Yaw-aim,3); // 在OLED显示屏上显示Yaw与目标值的差值，位置为(1,7)，显示3位数字
Motor_ing(); // 调用Motor_ing函数，可能用于控制电机
if(view>=25) // 如果视图值大于或等于25
{
mode=3; // 改变模式为3
}
}
}
//当检测到目标在画面中占比大于25%停止
//如果不可以调试，试试以下程序,并在检查一下配件，
//以下是对上述代码的逐行注释和整理：
//```c
#include <stdio.h>
#include "stm32f10x.h"
#include <math.h>
#include "MPU6050.h"
// Device header
uint8_t ID; // 定义用于存放ID号的变量
int16_t AX, AV, AZ, GX, GY, GZ; // 定义用于存放各个数据的变量
double calculate_angle(double opposite, double adjacent) {
return (3.1415926/180.0) * atan2(opposite, adjacent);
}
int angle() {
MPU6050_GetData(&AX, &AY, &AZ, &GX, &GY, &GZ); // 获取MPU6050的数据
double a = AX - GX; // 计算差值a
double b = AY - GY; // 计算差值b
printf("三角形两边长度：
");
scaf("%lf %lf", a, b); // 打印a和b的值
if(a == 0 || b == 0) {
printf("输入错误
");
return 0;
}
double angle = calculate_angle(a, b); // 计算角度
printf("对应角度%.2f
", angle); // 打印计算出的角度
return 0;
}
int Incremental_PI(int now, int aim) {
float KP = 0.7;
float KI = 0.05;
static float Bias, angle, Last_bias;
Bias = now - aim; // 计算当前偏差
angle += KP * (Bias - Last_bias) + KI * Bias; // 增量式PI控制器
Last_bias = Bias; // 保存上一次偏差
return angle; // 返回增量输出
}
void Motor_Stra() {
switch(direction) {
case 1:
Motor_SetSpeed2(10); // 如果direction为1，设置电机2的速度为10
Motor_SetSpeed1(-10); // 设置电机1的速度为-10
break; // 结束当前case，不再执行后续代码
case 2:
Motor_SetSpeed2(-10); // 如果direction为2，设置电机2的速度为-10
Motor_SetSpeed1(10); // 设置电机1的速度为10
break; // 结束当前case，不再执行后续代码
case 3:
Motor_SetSpeed2(0); // 如果direction为3，停止两个电机
Motor_SetSpeed1(0);
break; // 结束当前case，不再执行后续代码
}
}
void Motor_ing() {
Motor_SetSpeed2(50 + Yaw - aim); // 设置电机2的速度为50加上Yaw减去aim
Motor_SetSpeed1(50 - Yaw - aim); // 设置电机1的速度为50减去Yaw再减去aim
}
int main() {
if(mode == 1) { // 检查模式是否为1
Motor_Strat(); // 调用Motor_Strat函数，可能用于启动电机
OLED_ShowSignedNum(1, 7, angle, 3); // 在OLED显示屏上显示角度值，位置为(1,7)，显示3位数字
OLED_ShowString(1, "angle"); // 在OLED显示屏上显示字符串"angle"，位置为(1,0)
for(n = 1; angle == aim; ++n) { // 循环直到角度等于目标值
if(n > 137) { // 如果循环次数超过137次
mode = 2; // 改变模式为2
}
}
}
if(mode == 2) { // 检查模式是否为2
if(mode == 2) { // 再次检查模式是否为2（这个条件总是为真）
Angle_D = aim - Yaw; // 计算角度差值并赋值给Angle_D
OLED_ShowString(1, 1, "Yaw"); // 在OLED显示屏上显示字符串"Yaw"，位置为(1,0)
OLED_ShowSignedNum(1, 7, Yaw - aim, 3); // 在OLED显示屏上显示Yaw与目标值的差值，位置为(1,7)，显示3位数字
Motor_ing(); // 调用Motor_ing函数，可能用于控制电机
if(view >= 25) { // 如果视图值大于或等于25
mode = 3; // 改变模式为3
}
}
}
}
//```
/* 用到哪些硬件？
从代码中可以看出，以下硬件被使用：
1. STM32F10x：这是基于ARM Cortex-M3内核的微控制器，用于处理各种传感器数据和控制电机。
2. MPU6050：六轴陀螺仪和加速度计模块，用于获取姿态数据（如角速度和加速度）。
3. 电机驱动模块：用于控制小船上的电机，包括`Motor_SetSpeed2`和`Motor_SetSpeed1`函数。
4. 舵机：用于控制方向，代码中提到了`Motor_Strat`和`Motor_ing`函数，这些函数可能涉及舵机的控制。
5. OLED显示屏：用于显示角度和其他信息，通过`OLED_ShowSignedNum`和`OLED_ShowString`函数进行控制。
6. 摄像头：用于捕捉目标图像，并通过图像处理算法计算目标的位置和大小。
这些硬件共同工作，实现智能小船的驱动和控制功能。*/
//整理完毕， |
楼主
标题置顶
标题高亮
