目录
先列举出需要学习的内容：元器件、电路板设计、机械结构设计、MCU、C语言、Python
调整了代码让小车自己兜圈。后面加了视觉寻迹的功能。有时间考虑做下基于视觉避障的优化。
这是小车的顶部视角图片。
相关知识点
https://blog.csdn.net/LANXIAMAO/article/details/132794104?spm=1001.2014.3001.5502
#include "usart3.h"
u8 Usart3_Receive;
u8 mode_data[8];
u8 six_data_stop[3]={0X59,0X59,0X59}; //停止数据样本
u8 six_data_start[3]={0X58,0X58,0X58}; //启动数据样本
/**************************************************************************
函数功能：串口3初始化
入口参数：pclk2:PCLK2 时钟频率(Mhz) bound:波特率
返回 值：无
**************************************************************************/
void uart3_init(u32 pclk2,u32 bound)
{
float temp;
u16 mantissa;
u16 fraction;
temp=(float)(pclk2*1000000)/(bound*16);//得到USARTDIV
mantissa=temp; //得到整数部分
fraction=(temp-mantissa)*16; //得到小数部分
mantissa<<=4;
mantissa+=fraction;
RCC->APB2ENR|=1<<3; //使能PORTB口时钟
RCC->APB1ENR|=1<<18; //使能串口时钟
GPIOB->CRH&=0XFFFF00FF;
GPIOB->CRH|=0X00008B00;//IO状态设置
GPIOB->ODR|=1<<10;
RCC->APB1RSTR|=1<<18; //复位串口1
RCC->APB1RSTR&=~(1<<18);//停止复位
//波特率设置
USART3->BRR=mantissa; // 波特率设置
USART3->CR1|=0X200C; //1位停止,无校验位.
//使能接收中断
USART3->CR1|=1<<8; //PE中断使能
USART3->CR1|=1<<5; //接收缓冲区非空中断使能
MY_NVIC_Init(2,1,USART3_IRQn,2);//组2，最低优先级
}
/**************************************************************************
函数功能：串口3接收中断
入口参数：无
返回 值：无
**************************************************************************/
void USART3_IRQHandler(void)
{
if(USART3->SR&(1<<5))//接收到数据
{
static int uart_receive=0;//蓝牙接收相关变量
uart_receive=USART3->DR;
mode_data[0]=uart_receive;
// if(mode_data[0]==six_data_start[0] &&mode_data[1]==six_data_start[1] &&mode_data[2]==six_data_start[2] )
// {
// Flag_Stop=0; //3击低速挡 小车关闭电机
// mode_data[0]=0; mode_data[1]=0; mode_data[2]=0;
// }
// if(mode_data[0]==six_data_stop[0] &&mode_data[1]==six_data_stop[1] &&mode_data[2]==six_data_stop[2] )
// {
// Flag_Stop=1; //3击高速挡 小车启动电机
// mode_data[0]=0; mode_data[1]=0; mode_data[2]=0;
// }
// if(uart_receive>10) //默认使用app为：MiniBalanceV3.5 因为MiniBalanceV3.5的遥控指令为A~H 其HEX都大于10
// {
// if(uart_receive==0x5A) Flag_Qian=0,Flag_Hou=0,Flag_Left=0,Flag_Right=0;//刹车
// else if(uart_receive==0x41) Flag_Qian=1,Flag_Hou=0,Flag_Left=0,Flag_Right=0;//前
// else if(uart_receive==0x45) Flag_Qian=0,Flag_Hou=1,Flag_Left=0,Flag_Right=0;//后
// else if(uart_receive==0x42||uart_receive==0x43||uart_receive==0x44)
// Flag_Qian=0,Flag_Hou=0,Flag_Left=0,Flag_Right=1; //左
// else if(uart_receive==0x46||uart_receive==0x47||uart_receive==0x48) //右
// Flag_Qian=0,Flag_Hou=0,Flag_Left=1,Flag_Right=0;
// else Flag_Qian=0,Flag_Hou=0,Flag_Left=0,Flag_Right=0;//刹车
// }
// if(uart_receive<10) //备用app为：MiniBalanceV1.0 因为MiniBalanceV1.0的遥控指令为A~H 其HEX都小于10
// {
// Flag_sudu=1;//切换至高速档
// if(uart_receive==0x00) Flag_Qian=0,Flag_Hou=0,Flag_Left=0,Flag_Right=0;//刹车
// else if(uart_receive==0x01) Flag_Qian=1,Flag_Hou=0,Flag_Left=0,Flag_Right=0;//前
// else if(uart_receive==0x05) Flag_Qian=0,Flag_Hou=1,Flag_Left=0,Flag_Right=0;//后
// else if(uart_receive==0x02||uart_receive==0x03||uart_receive==0x04)
// Flag_Qian=0,Flag_Hou=0,Flag_Left=0,Flag_Right=1; //左
// else if(uart_receive==0x06||uart_receive==0x07||uart_receive==0x08) //右
// Flag_Qian=0,Flag_Hou=0,Flag_Left=1,Flag_Right=0;
// else Flag_Qian=0,Flag_Hou=0,Flag_Left=0,Flag_Right=0;//刹车
// }
mode_data[2]=mode_data[1];
mode_data[1]=mode_data[0];
}
}
#include "control.h"
#include "show.h"
#include "usart.h"
#include "stmflash.h"
#include "stdio.h"
float Center_Gravity; //机械中值x轴方向机械中值 可以将小车PWM关闭然后观察小车的x轴平衡时屏幕显示的角度（即为机械中值），每个小车由于电池和惯量轮安装位置不同会有些许差异
float Center_Gra_Sart=90.0; //开机设定机械中值
int Encoder_x=0; //横向电机编码器变量
int Moto_x,Moto_y; //电机PWM变量
float Voltage; //电池电压变量
float Angle_Balance_x; //横向角度
float Gyro_Balance_x=0; //横向角加速度
int Balance_Pwm_x=0,velocity_Pwm_x=0; //横向电机PWM分量
char t=0;
char ANGLE[4];
char GRO[5];
char SPEED[4];
int TIM1_UP_IRQHandler(void) //所有的控制代码都在这里面 TIM1控制的10ms定时中断
{
if(TIM1->SR&0X0001)
{
u8 key_value;
TIM1->SR&=~(1<<0); //清除定时器1中断标志位
Get_Angle(); //获取侧向角度
Voltage=Get_battery_volt(); //获取当前电池电压
Encoder_x=-Read_Encoder(2); //读取编码器值
Balance_Pwm_x=balance_x(Angle_Balance_x,Gyro_Balance_x-40); //角度闭环控制
velocity_Pwm_x=velocity_x(Encoder_x); //速度闭环控制
if(Angle_Balance_x<120&&Angle_Balance_x>60)Moto_x=Balance_Pwm_x+velocity_Pwm_x; //计算电机最终PWM
else Moto_x=0;
Moto_y=remot_moto*60; //前进电机赋值
Xianfu_Pwm(); //PWM限幅
//Set_Pwm(0,0); //赋值给PWM寄存器*/
Set_Pwm(Moto_x,Moto_y); //赋值给PWM寄存器*/
Led_Flash(10); //系统运行状态指示灯
if(t==0) //每隔20ms发出一次舵机脉冲
{
SERVER_PWM=1;
delay_us(-remot_angle*5+1250);//舵机引脚控制
SERVER_PWM=0;
t=1;
}
else //每隔20ms上传一次姿态信息
{
Usart2_SendString("ag:", 3);
sprintf(ANGLE,"%04d",(int)(Angle_Balance_x*100));
Usart2_SendString((unsigned char *)ANGLE, 4);
Usart2_SendString("gr:", 3);
sprintf(GRO,"%05d",(int)Gyro_Balance_x);
Usart2_SendString((unsigned char *)GRO, 5);
Usart2_SendString("sp:", 3);
sprintf(SPEED,"%04d",Encoder_x);
Usart2_SendString((unsigned char *)SPEED, 4);
t=0;
}
key_value=key_read();
if(key_value==3) //两个按键同时被按下，进入参数调节状态
{
Set_Pwm(0,0); //关闭电机
while(key_value==3)key_value=key_read();
fill_picture(0x00); //OLED清屏
OLED_ShowString(5,10,"SET_UP",12);
OLED_ShowString(10,4,"Entering...",12);
delay_ms(1000);
fill_picture(0x00); //OLED清屏
while(1)
{
Get_Angle(); //获取角度
Voltage=Get_battery_volt(); //获取电压
Encoder_x=Read_Encoder(2); //更新编码器位置信息
key_value=key_read();
if(key_value==1)
{
Center_Gra_Sart=Center_Gra_Sart-0.1;
if(Center_Gra_Sart<70)Center_Gra_Sart=70;
oled_show();
delay_ms(200);
}
else if(key_value==2)
{
Center_Gra_Sart=Center_Gra_Sart+0.1;
if(Center_Gra_Sart>110)Center_Gra_Sart=110;
oled_show();
delay_ms(200);
}
else if(key_value==3)
{
fill_picture(0x00); //OLED清屏
OLED_ShowString(5,3,"Parameters",12);
OLED_ShowString(10,4,"Saving...",12);
datatemp[0]=((int)(Center_Gra_Sart*10))/100;
datatemp[1]=(((int)(Center_Gra_Sart*10))%100)/10;
datatemp[2]=((int)(Center_Gra_Sart*10))%10;
STMFLASH_Write(FLASH_SAVE_ADDR,(u16*)datatemp,4);
delay_ms(1000);delay_ms(1000);
fill_picture(0x00); //OLED清屏
break;
}
oled_show();
delay_ms(5);
}
}
}
return 0;
}
int balance_x(float Angle,float gyro)//倾角PD控制 入口参数：角度 返回 值：倾角控制PWM
{
float Balance_KP=350,Balance_KI=0,Balance_KD=-2;
float Bias; //倾角偏差
static float D_Bias,Integral_bias; //PID相关变量
int balance; //PWM返回值
Bias=Angle-Center_Gravity; //求出平衡的角度中值 和机械相关
Integral_bias+=Bias;
if(Integral_bias>30000)Integral_bias=30000;
if(Integral_bias<-30000)Integral_bias=-30000;
D_Bias=gyro; //求出偏差的微分 进行微分控制
balance=Balance_KP*Bias+Balance_KI*Integral_bias+D_Bias*Balance_KD; //===计算倾角控制的电机PWM PD控制
return balance;
}
int velocity_x(int encoder) //位置式PID控制器 入口参数：编码器测量位置信息，目标位置 返回 值：电机PWM
{
float Position_KP=50,Position_KI=0.02,Position_KD=0;
static float Pwm,Integral_bias,Last_Bias,Encoder;
Encoder *= 0.65; //一阶低通滤波器
Encoder += encoder*0.35; //一阶低通滤波器
Integral_bias+=Encoder; //求出偏差的积分
if(Integral_bias>20000)Integral_bias=20000;
if(Integral_bias<-20000)Integral_bias=-20000;
Pwm=Position_KP*Encoder+Position_KI*Integral_bias+Position_KD*(Encoder-Last_Bias); //位置式PID控制器
Last_Bias=Encoder; //保存上一次偏差
return Pwm; //增量输出
}
void Set_Pwm(int motox,int motoy)
{
if(motox<0) DIR=0; //无刷电机PWM赋值
else DIR=1;
if(motox<50&&motox>-50)EN=0; //pwm很低时直接关闭电机使能，防止电机休眠！
else EN=1;
PWMX=myabs(motox)+100;
if(motoy<0){PWMB=0;PWMA=myabs(motoy);}
else {PWMB=myabs(motoy);PWMA=0;}
}
void Xianfu_Pwm(void)
{
int Amplitude_x=6900,Amplitude_y=6900; //===PWM满幅是7200 限制在6900
if(Moto_x<-Amplitude_x) Moto_x=-Amplitude_x;
if(Moto_x>Amplitude_x) Moto_x=Amplitude_x;
if(Moto_y<-Amplitude_y) Moto_y=-Amplitude_y;
if(Moto_y>Amplitude_y) Moto_y=Amplitude_y;
}
int myabs(int a)
{
int temp;
if(a<0) temp=-a;
else temp=a;
return temp;
}
void Yijielvbo_X(float angle_m, float gyro_m) //一阶互补滤波 入口参数：加速度、角速度
{
Angle_Balance_x = 0.1 * angle_m+ (1-0.1) * (Angle_Balance_x + gyro_m * 0.005);
}
void Get_Angle(void)
{
u8 bufa[6],bufg[6];
float Accel_Y,Accel_X,Accel_Z,Gyro_Y,Gyro_X,Gyro_Z;
MPU_Read_Len(MPU_ADDR,MPU_ACCEL_XOUTH_REG,6,bufa);
MPU_Read_Len(MPU_ADDR,MPU_GYRO_XOUTH_REG,6,bufg);
sensor.acc.origin.y = ((((int16_t)bufa[0]) << 8) | bufa[1]);
sensor.acc.origin.x = ((((int16_t)bufa[2]) << 8) | bufa[3]);
sensor.acc.origin.z = ((((int16_t)bufa[4]) << 8) | bufa[5]);
sensor.gyro.origin.y = ((((int16_t)bufg[0]) << 8)| bufg[1]);
sensor.gyro.origin.x = ((((int16_t)bufg[2]) << 8)| bufg[3]);
sensor.gyro.origin.z = ((((int16_t)bufg[4]) << 8)| bufg[5]);
// sensor.acc.origin.x = ((((int16_t)bufa[0]) << 8) | bufa[1]);
// sensor.acc.origin.y = ((((int16_t)bufa[2]) << 8) | bufa[3]);
// sensor.acc.origin.z = ((((int16_t)bufa[4]) << 8) | bufa[5]);
// sensor.gyro.origin.x = ((((int16_t)bufg[0]) << 8)| bufg[1]);
// sensor.gyro.origin.y = ((((int16_t)bufg[2]) << 8)| bufg[3]);
// sensor.gyro.origin.z = ((((int16_t)bufg[4]) << 8)| bufg[5]);
Gyro_Y=sensor.gyro.origin.y; //读取X轴陀螺仪
Gyro_Z=sensor.gyro.origin.z; //读取Z轴陀螺仪
Accel_X=sensor.acc.origin.x; //读取Y轴加速度计
Accel_Z=sensor.acc.origin.z; //读取Z轴加速度计
if(Gyro_Y>32768) Gyro_Y-=65536; //数据类型转换 也可通过short强制类型转换
if(Gyro_Z>32768) Gyro_Z-=65536; //数据类型转换
if(Accel_X>32768) Accel_X-=65536; //数据类型转换
if(Accel_Z>32768) Accel_Z-=65536; //数据类型转换
// Gyro_Balance_y=-Gyro_Y; //更新平衡角速度
Accel_Y=atan2(Accel_X,Accel_Z)*180/PI; //计算倾角
Gyro_Y=Gyro_Y/16.4; //陀螺仪量程转换
Gyro_X=sensor.gyro.origin.x; //读取X轴陀螺仪
Accel_Y=sensor.acc.origin.y; //读取Y轴加速度计
if(Gyro_X>32768) Gyro_X-=65536; //数据类型转换 也可通过short强制类型转换
if(Accel_Y>32768) Accel_Y-=65536; //数据类型转换
Gyro_Balance_x=-Gyro_X; //更新平衡角速度
Accel_X= (atan2(Accel_Z , Accel_Y)) * 180 / PI; //计算倾角
Gyro_X=Gyro_X/16.4; //陀螺仪量程转换
Yijielvbo_X(Accel_X,Gyro_X);
}
某鱼上买了个openmv4 的dao版二手，正版有点贵。模块核心MCU是STM32 能跑python是因为装了micro python固件。可以插存储卡，将代码放到存储卡根目录就会被读取执行。
直接上代码，代码需要根据实际情况调整，不能直接用
#THRESHOLD =(13, 51, -11, 42, -81, -18) # Grayscale threshold for dark things...
THRESHOLD =(0, 10, 0, 10, -20, 10) # Grayscale threshold for dark things...
import sensor, image, time, pyb
from pyb import LED
from pid import PID
from pyb import UART
rho_pid = PID(p=0.8, i=0) #距离
#rho_pid = PID(p=0, i=0) #距离
theta_pid = PID(p=0.1, i=0) #角度
#theta_pid = PID(p=0, i=0) #角度
LED(1).on()
LED(2).on()
LED(3).on()
led = pyb.LED(4)
led.on()
sensor.reset()
sensor.set_vflip(True)
sensor.set_hmirror(True)
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQQVGA) # 80x60 (4,800 pixels) - O(N^2) max = 2,3040,000.
#sensor.set_windowing([0,20,80,40])
sensor.skip_frames(time = 2000) # WARNING: If you use QQVGA it may take seconds
clock = time.clock() # to process a frame sometimes.
uart = UART(3, 115200) #初始化串口3
arr=[0x30,0x30,0x30,0x30,0x0d,0x0a] #要发送的数组
while(True):
clock.tick()
img = sensor.snapshot().binary([THRESHOLD],invert = False) #得到二值化图像
line = img.get_regression([(100,100)], robust = True) #从图象中得到线
if (line): #如果含有线
rho_err = abs(line.rho())-img.width()/2 #求出距离偏差
if line.theta()>90:
theta_err = line.theta()-180
else:
theta_err = line.theta() #求出角度偏差
img.draw_line(line.line(), color = 127) #在图像中画出直线
print(rho_err,line.magnitude(),rho_err) #打印偏差信息
if line.magnitude()>8: #如果线的长度大于8
#if -40<b_err<40 and -30<t_err<30:
rho_output = rho_pid.get_pid(rho_err,1)
theta_output = theta_pid.get_pid(theta_err,1)
output = rho_output+theta_output
if output>99:
output=99
if output<-99:
output=-99
if abs(output)>10:
arr[3]='1' #前进级别
elif abs(output)>7:
arr[3]='2' #前进级别
elif abs(output)>5:
arr[3]='3' #前进级别
elif abs(output)>3:
arr[3]='3' #前进级别
elif abs(output)>1:
arr[3]='4' #前进级别
else:
arr[3]='5' #前进级别
output=output+100
arr[0]=int(output/100) #转弯百位
arr[1]=int(output%100/10) #转弯十位
arr[2]=int(output%10) #转弯个位
print('you send:',str(arr))
uart.write(str(arr[0]))
uart.write(str(arr[1]))
uart.write(str(arr[2]))
uart.write(str(arr[3]))
uart.write('\r')
uart.write('\n')
else: #否则停止前进
print('you send:1,0,0,0，')
uart.write('1')
uart.write('0')
uart.write('0')
uart.write('0')
uart.write('\r')
uart.write('\n')
else: #如果没有找到线就停止前进
print('you send noline:1,0,0,0，')
uart.write('1')
uart.write('0')
uart.write('0')
uart.write('0')
uart.write('\r')
uart.write('\n')
pass
#print(clock.fps())
from pyb import millis
from math import pi, isnan
class PID:
_kp = _ki = _kd = _integrator = _imax = 0
_last_error = _last_derivative = _last_t = 0
_RC = 1/(2 * pi * 20)
def __init__(self, p=0, i=0, d=0, imax=0):
self._kp = float(p)
self._ki = float(i)
self._kd = float(d)
self._imax = abs(imax)
self._last_derivative = float('nan')
def get_pid(self, error, scaler):
tnow = millis()
dt = tnow - self._last_t
output = 0
if self._last_t == 0 or dt > 1000:
dt = 0
self.reset_I()
self._last_t = tnow
delta_time = float(dt) / float(1000)
output += error * self._kp
if abs(self._kd) > 0 and dt > 0:
if isnan(self._last_derivative):
derivative = 0
self._last_derivative = 0
else:
derivative = (error - self._last_error) / delta_time
derivative = self._last_derivative + \
((delta_time / (self._RC + delta_time)) * \
(derivative - self._last_derivative))
self._last_error = error
self._last_derivative = derivative
output += self._kd * derivative
output *= scaler
if abs(self._ki) > 0 and dt > 0:
self._integrator += (error * self._ki) * scaler * delta_time
if self._integrator < -self._imax: self._integrator = -self._imax
elif self._integrator > self._imax: self._integrator = self._imax
output += self._integrator
return output
def reset_I(self):
self._integrator = 0
self._last_derivative = float('nan')
注：部分内容来自网络，文章仅供学习，如有侵权请联系删除,