RUST下可用的模块
2025年11月12日大约 11 分钟
RUST下可用的模块
在学习rust的过程中，我自己动手谢了一些模块和驱动，在此记录下来
PID控制器
use num_traits::{Num, Signed};
pub enum PidMode {
Position,
Delta,
}
pub struct PidController<T>
where
T: Num + Copy + PartialOrd + Signed,
{
mode: PidMode,
kp: T,
ki: T,
kd: T,
max_out: T,
max_iout: T,
set: T,
feedback: T,
output: T,
p_out: T,
i_out: T,
d_out: T,
d_buf: [T; 3],
error: [T; 3],
}
impl<T> PidController<T>
where
T: Num + Copy + PartialOrd + Signed,
{
pub fn new(mode: PidMode, pid_params: [T; 3], max_out: T, max_iout: T) -> Self {
Self {
mode,
kp: pid_params[0],
ki: pid_params[1],
kd: pid_params[2],
max_out,
max_iout,
set: T::zero(),
feedback: T::zero(),
output: T::zero(),
p_out: T::zero(),
i_out: T::zero(),
d_out: T::zero(),
d_buf: [T::zero(); 3],
error: [T::zero(); 3],
}
}
fn limit(&self, value: T, max: T) -> T {
if value > max {
max
} else if value < -max {
-max
} else {
value
}
}
// pub fn change_setpoint(&mut self, setpoint: T) {
// self.set = setpoint;
// }
// pub fn change_feedback(&mut self, feedback: T) {
// self.feedback = feedback;
// }
// pub fn chenge_pid_params(&mut self, pid_params: [T; 3]) {
// self.kp = pid_params[0];
// self.ki = pid_params[1];
// self.kd = pid_params[2];
// }
pub fn chenge_params(
&mut self,
setpoint: Option<T>,
feedback: Option<T>,
pid_params: Option<[T; 3]>
) {
if let Some(s) = setpoint {
self.set = s;
}
if let Some(f) = feedback {
self.feedback = f;
}
if let Some(pid_params) = pid_params {
self.kp = pid_params[0];
self.ki = pid_params[1];
self.kd = pid_params[2];
}
}
pub fn calc(&mut self, feedback: T, setpoint: T) -> T {
// 更新误差历史
self.error[2] = self.error[1];
self.error[1] = self.error[0];
self.set = setpoint;
self.feedback = feedback;
self.error[0] = setpoint - feedback;
match self.mode {
PidMode::Position => {
// 计算比例项
self.p_out = self.kp * self.error[0];
// 计算积分项并限制
self.i_out = self.i_out + self.ki * self.error[0];
self.i_out = self.limit(self.i_out, self.max_iout);
// 更新微分项缓冲区
self.d_buf[2] = self.d_buf[1];
self.d_buf[1] = self.d_buf[0];
self.d_buf[0] = self.error[0] - self.error[1];
// 计算微分项
self.d_out = self.kd * self.d_buf[0];
// 计算总输出并限制
self.output = self.p_out + self.i_out + self.d_out;
self.output = self.limit(self.output, self.max_out);
}
PidMode::Delta => {
// 计算比例项
self.p_out = self.kp * (self.error[0] - self.error[1]);
// 计算积分项
self.i_out = self.ki * self.error[0];
// 更新微分项缓冲区
self.d_buf[2] = self.d_buf[1];
self.d_buf[1] = self.d_buf[0];
self.d_buf[0] = self.error[0] - (self.error[1] + self.error[1]) + self.error[2];
// 计算微分项
self.d_out = self.kd * self.d_buf[0];
// 计算总输出并限制
self.output = self.output + self.p_out + self.i_out + self.d_out;
self.output = self.limit(self.output, self.max_out);
}
}
return self.output;
}
/// 清除PID控制器状态
pub fn clear(&mut self) {
self.error = [T::zero(); 3];
self.d_buf = [T::zero(); 3];
self.output = T::zero();
self.p_out = T::zero();
self.i_out = T::zero();
self.d_out = T::zero();
self.feedback = T::zero();
self.set = T::zero();
}
}简易OLED驱动
OLED驱动相关文件结构如下：
src/
├── main.rs 你的主程序入口
├── oled.rs
└── oled/
├── oled_font.rs (包含字体数据)
└── ssd1306_driver.rs (SSD1306驱动主文件)oled.rs内容如下：
pub mod ssd1306_driver;
pub mod oled_font;oled_font.rs内容如下：
//! OLED 字体数据
/// 6x8 字体数据 (ASCII 32-127)
pub const FONT6X8: [[u8; 6]; 96] = [
[0x00, 0x00, 0x00, 0x00, 0x00, 0x00], // (space)
[0x00, 0x00, 0x5F, 0x00, 0x00, 0x00], // !
[0x00, 0x07, 0x00, 0x07, 0x00, 0x00], // "
[0x14, 0x7F, 0x14, 0x7F, 0x14, 0x00], // #
[0x24, 0x2A, 0x7F, 0x2A, 0x12, 0x00], // $
[0x23, 0x13, 0x08, 0x64, 0x62, 0x00], // %
[0x36, 0x49, 0x55, 0x22, 0x50, 0x00], // &
[0x00, 0x05, 0x03, 0x00, 0x00, 0x00], // '
[0x00, 0x1C, 0x22, 0x41, 0x00, 0x00], // (
[0x00, 0x41, 0x22, 0x1C, 0x00, 0x00], // )
[0x14, 0x08, 0x3E, 0x08, 0x14, 0x00], // *
[0x08, 0x08, 0x3E, 0x08, 0x08, 0x00], // +
[0x00, 0x50, 0x30, 0x00, 0x00, 0x00], // ,
[0x08, 0x08, 0x08, 0x08, 0x08, 0x00], // -
[0x00, 0x60, 0x60, 0x00, 0x00, 0x00], // .
[0x20, 0x10, 0x08, 0x04, 0x02, 0x00], // /
[0x3E, 0x51, 0x49, 0x45, 0x3E, 0x00], // 0
[0x00, 0x42, 0x7F, 0x40, 0x00, 0x00], // 1
[0x42, 0x61, 0x51, 0x49, 0x46, 0x00], // 2
[0x21, 0x41, 0x45, 0x4B, 0x31, 0x00], // 3
[0x18, 0x14, 0x12, 0x7F, 0x10, 0x00], // 4
[0x27, 0x45, 0x45, 0x45, 0x39, 0x00], // 5
[0x3C, 0x4A, 0x49, 0x49, 0x30, 0x00], // 6
[0x01, 0x71, 0x09, 0x05, 0x03, 0x00], // 7
[0x36, 0x49, 0x49, 0x49, 0x36, 0x00], // 8
[0x06, 0x49, 0x49, 0x29, 0x1E, 0x00], // 9
[0x00, 0x36, 0x36, 0x00, 0x00, 0x00], // :
[0x00, 0x56, 0x36, 0x00, 0x00, 0x00], // ;
[0x08, 0x14, 0x22, 0x41, 0x00, 0x00], // <
[0x14, 0x14, 0x14, 0x14, 0x14, 0x00], // =
[0x00, 0x41, 0x22, 0x14, 0x08, 0x00], // >
[0x02, 0x01, 0x51, 0x09, 0x06, 0x00], // ?
[0x32, 0x49, 0x79, 0x41, 0x3E, 0x00], // @
[0x7E, 0x11, 0x11, 0x11, 0x7E, 0x00], // A
[0x7F, 0x49, 0x49, 0x49, 0x36, 0x00], // B
[0x3E, 0x41, 0x41, 0x41, 0x22, 0x00], // C
[0x7F, 0x41, 0x41, 0x22, 0x1C, 0x00], // D
[0x7F, 0x49, 0x49, 0x49, 0x41, 0x00], // E
[0x7F, 0x09, 0x09, 0x09, 0x01, 0x00], // F
[0x3E, 0x41, 0x49, 0x49, 0x7A, 0x00], // G
[0x7F, 0x08, 0x08, 0x08, 0x7F, 0x00], // H
[0x00, 0x41, 0x7F, 0x41, 0x00, 0x00], // I
[0x20, 0x40, 0x41, 0x3F, 0x01, 0x00], // J
[0x7F, 0x08, 0x14, 0x22, 0x41, 0x00], // K
[0x7F, 0x40, 0x40, 0x40, 0x40, 0x00], // L
[0x7F, 0x02, 0x0C, 0x02, 0x7F, 0x00], // M
[0x7F, 0x04, 0x08, 0x10, 0x7F, 0x00], // N
[0x3E, 0x41, 0x41, 0x41, 0x3E, 0x00], // O
[0x7F, 0x09, 0x09, 0x09, 0x06, 0x00], // P
[0x3E, 0x41, 0x51, 0x21, 0x5E, 0x00], // Q
[0x7F, 0x09, 0x19, 0x29, 0x46, 0x00], // R
[0x46, 0x49, 0x49, 0x49, 0x31, 0x00], // S
[0x01, 0x01, 0x7F, 0x01, 0x01, 0x00], // T
[0x3F, 0x40, 0x40, 0x40, 0x3F, 0x00], // U
[0x1F, 0x20, 0x40, 0x20, 0x1F, 0x00], // V
[0x3F, 0x40, 0x38, 0x40, 0x3F, 0x00], // W
[0x63, 0x14, 0x08, 0x14, 0x63, 0x00], // X
[0x07, 0x08, 0x70, 0x08, 0x07, 0x00], // Y
[0x61, 0x51, 0x49, 0x45, 0x43, 0x00], // Z
[0x00, 0x7F, 0x41, 0x41, 0x00, 0x00], // [
[0x02, 0x04, 0x08, 0x10, 0x20, 0x00], // backslash
[0x00, 0x41, 0x41, 0x7F, 0x00, 0x00], // ]
[0x04, 0x02, 0x01, 0x02, 0x04, 0x00], // ^
[0x40, 0x40, 0x40, 0x40, 0x40, 0x00], // _
[0x00, 0x01, 0x02, 0x04, 0x00, 0x00], // `
[0x20, 0x54, 0x54, 0x54, 0x78, 0x00], // a
[0x7F, 0x48, 0x44, 0x44, 0x38, 0x00], // b
[0x38, 0x44, 0x44, 0x44, 0x20, 0x00], // c
[0x38, 0x44, 0x44, 0x48, 0x7F, 0x00], // d
[0x38, 0x54, 0x54, 0x54, 0x18, 0x00], // e
[0x08, 0x7E, 0x09, 0x01, 0x02, 0x00], // f
[0x0C, 0x52, 0x52, 0x52, 0x3E, 0x00], // g
[0x7F, 0x08, 0x04, 0x04, 0x78, 0x00], // h
[0x00, 0x44, 0x7D, 0x40, 0x00, 0x00], // i
[0x20, 0x40, 0x44, 0x3D, 0x00, 0x00], // j
[0x7F, 0x10, 0x28, 0x44, 0x00, 0x00], // k
[0x00, 0x41, 0x7F, 0x40, 0x00, 0x00], // l
[0x7C, 0x04, 0x18, 0x04, 0x78, 0x00], // m
[0x7C, 0x08, 0x04, 0x04, 0x78, 0x00], // n
[0x38, 0x44, 0x44, 0x44, 0x38, 0x00], // o
[0x7C, 0x14, 0x14, 0x14, 0x08, 0x00], // p
[0x08, 0x14, 0x14, 0x18, 0x7C, 0x00], // q
[0x7C, 0x08, 0x04, 0x04, 0x08, 0x00], // r
[0x48, 0x54, 0x54, 0x54, 0x20, 0x00], // s
[0x04, 0x3F, 0x44, 0x40, 0x20, 0x00], // t
[0x3C, 0x40, 0x40, 0x20, 0x7C, 0x00], // u
[0x1C, 0x20, 0x40, 0x20, 0x1C, 0x00], // v
[0x3C, 0x40, 0x30, 0x40, 0x3C, 0x00], // w
[0x44, 0x28, 0x10, 0x28, 0x44, 0x00], // x
[0x0C, 0x50, 0x50, 0x50, 0x3C, 0x00], // y
[0x44, 0x64, 0x54, 0x4C, 0x44, 0x00], // z
[0x00, 0x08, 0x36, 0x41, 0x00, 0x00], // {
[0x00, 0x00, 0x7F, 0x00, 0x00, 0x00], // |
[0x00, 0x41, 0x36, 0x08, 0x00, 0x00], // }
[0x10, 0x08, 0x08, 0x10, 0x08, 0x00], // ~
[0x00, 0x06, 0x09, 0x09, 0x06, 0x00], // (DEL)
];ssd1306_driver.rs内容如下：
/// 确保embedded-hal = "1.0.0"
use crate::oled::oled_font::FONT6X8;
// 添加驱动类型枚举
#[derive(Clone, Copy, PartialEq)]
pub enum DisplayDriver {
SSD1306,
SH1106,
}
pub struct SSD1306<I: embedded_hal::i2c::I2c> {
i2c: I, // I2C实例
address: u8, // OLED显示屏I2C地址
cmd_addr: u8, // OLED显示屏I2C命令地址
data_addr: u8, // OLED显示屏I2C数据地址
column_offset: u8, // 列偏移量，用于SH1106
driver: DisplayDriver, // 驱动类型
}
impl<I: embedded_hal::i2c::I2c> SSD1306<I> {
const MAX_COLUMN: u8 = 128; // OLED实际列数为128
// 合并构造函数，driver参数有默认值
pub fn new(
i2c: I,
oled_i2c_addr: u8,
i2c_cmd_addr: u8,
i2c_data_addr: u8,
driver: DisplayDriver,
) -> Self {
let column_offset = match driver {
DisplayDriver::SSD1306 => 0,
DisplayDriver::SH1106 => 2, // SH1106需要列偏移2
};
Self {
i2c,
address: oled_i2c_addr,
cmd_addr: i2c_cmd_addr,
data_addr: i2c_data_addr,
column_offset,
driver,
}
}
fn write_command(&mut self, command: u8) -> Result<(), I::Error> {
self.i2c.write(self.address, &[self.cmd_addr, command])
}
fn write_data(&mut self, data: u8) -> Result<(), I::Error> {
self.i2c.write(self.address, &[self.data_addr, data])
}
pub fn init(&mut self) -> Result<(), I::Error> {
// 参考SSD1306数据手册的推荐初始化序列
self.write_command(0xAE)?; // 关闭显示
self.write_command(0xD5)?; // 设置显示时钟分频比/振荡器频率
self.write_command(0x80)?; // 推荐值 0x80
self.write_command(0xA8)?; // 设置多路复用率
self.write_command(0x3F)?; // 1/64
self.write_command(0xD3)?; // 设置显示偏移
self.write_command(0x00)?; // 无偏移
self.write_command(0x40)?; // 设置显示开始行
self.write_command(0x8D)?; // 电荷泵设置
self.write_command(0x14)?; // 开启电荷泵
self.write_command(0x20)?; // 设置内存寻址模式
self.write_command(0x00)?; // 水平寻址模式
self.write_command(0xA1)?; // 设置段重新映射
self.write_command(0xC8)?; // 设置COM输出扫描方向
self.write_command(0xDA)?; // 设置COM引脚硬件配置
self.write_command(0x12)?; //
self.write_command(0x81)?; // 设置对比度控制
self.write_command(0xCF)?; //
self.write_command(0xD9)?; // 设置预充电周期
self.write_command(0xF1)?; //
self.write_command(0xDB)?; // 设置VCOMH去选中电平
self.write_command(0x40)?; //
self.write_command(0xA4)?; // 全部显示映射RAM内容
self.write_command(0xA6)?; // 设置正常显示
self.write_command(0xAF)?; // 开启显示
Ok(())
}
pub fn display_white(&mut self) -> Result<(), I::Error> {
self.fill(0xFF)
}
pub fn clear(&mut self) -> Result<(), I::Error> {
self.fill(0x00)
}
fn fill(&mut self, data: u8) -> Result<(), I::Error> {
for page in 0..8 {
self.write_command(0xB0 + page)?; // 设置页地址
// 应用列偏移
let column_with_offset = 0 + self.column_offset;
self.write_command((column_with_offset & 0x0F) | 0x00)?; // 设置低4位列地址
self.write_command(((column_with_offset & 0xF0) >> 4) | 0x10)?; // 设置高4位列地址
for _ in 0..16 {
// 128列 / 8 = 16次，每次发送8字节
for _ in 0..8 {
self.write_data(data)?;
}
}
}
Ok(())
}
fn set_cursor(&mut self, x: u8, y: u8) -> Result<(), I::Error> {
if x >= Self::MAX_COLUMN || y >= 8 {
return Ok(()); // 超出范围则直接返回
}
self.write_command(0xB0 + y)?; // 设置页地址
// 应用列偏移
let column_with_offset = x + self.column_offset;
self.write_command((column_with_offset & 0x0F) | 0x00)?; // 设置低4位列地址
self.write_command(((column_with_offset & 0xF0) >> 4) | 0x10)?; // 设置高4位列地址
Ok(())
}
/// 显示单个字符
pub fn show_char(&mut self, x: u8, y: u8, chr: char) -> Result<(), I::Error> {
let chr_index = if chr as u8 >= 32 && chr as u8 <= 127 {
(chr as u8 - 32) as usize
} else {
0 // 默认为空格
};
if x > Self::MAX_COLUMN - 6 {
// 如果超出了屏幕的显示宽度，则换到下一行重新开始
self.set_cursor(0, y + 1)?;
} else {
self.set_cursor(x, y)?;
}
// 显示字符
for i in 0..6 {
self.write_data(FONT6X8[chr_index][i])?;
}
Ok(())
}
/// 显示字符串
pub fn show_string(&mut self, mut x: u8, mut y: u8, text: &str) -> Result<(), I::Error> {
for chr in text.chars() {
self.show_char(x, y, chr)?;
x += 6;
if x > Self::MAX_COLUMN - 8 {
x = 0;
y += 1;
}
}
Ok(())
}
/// 显示整数
pub fn show_number(&mut self, mut x: u8, mut y: u8, num: i32) -> Result<(), I::Error> {
// 处理负数
if num < 0 {
self.show_char(x, y, '-')?;
x += 6;
self.show_positive_number(x, y, (-num) as u32)
} else {
self.show_positive_number(x, y, num as u32)
}
}
/// 显示正整数
pub fn show_positive_number(&mut self, mut x: u8, y: u8, num: u32) -> Result<(), I::Error> {
if num == 0 {
self.show_char(x, y, '0')?;
return Ok(());
}
// 将数字转换为字符数组（逆序）
let mut chars = [0u8; 10]; // u32 最多10位数字
let mut len = 0;
let mut n = num;
while n > 0 {
chars[len] = (n % 10) as u8 + b'0';
n /= 10;
len += 1;
}
// 逆序显示字符
for i in (0..len).rev() {
if x > Self::MAX_COLUMN - 6 {
x = 0;
// 注意：这里不增加y值，因为通常我们希望数字在同一行显示
// 如果需要自动换行，可以取消下面两行的注释
// y += 1;
// if y >= 8 { break; } // 防止超出屏幕范围
}
self.show_char(x, y, chars[i] as char)?;
x += 6;
}
Ok(())
}
/// 显示浮点数
pub fn show_float(
&mut self,
mut x: u8,
mut y: u8,
num: f32,
precision: usize,
) -> Result<(), I::Error> {
// 处理负数
if num < 0.0 {
self.show_char(x, y, '-')?;
x += 6;
self.show_positive_float(x, y, -num, precision)
} else {
self.show_positive_float(x, y, num, precision)
}
}
/// 显示正浮点数（内部辅助函数）
fn show_positive_float(
&mut self,
mut x: u8,
y: u8,
num: f32,
precision: usize,
) -> Result<(), I::Error> {
// 显示整数部分
let integer_part = num as u32;
self.show_positive_number(x, y, integer_part)?;
// 计算新的x位置
let mut temp_num = integer_part;
let digit_count = if temp_num == 0 {
1
} else {
let mut count = 0;
while temp_num > 0 {
temp_num /= 10;
count += 1;
}
count
};
x += (digit_count as u8) * 6;
// 如果精度大于0，显示小数点和小数部分
if precision > 0 {
// 检查是否超出屏幕范围
if x > Self::MAX_COLUMN - 6 {
// x = 0;
// y += 1;
return Ok(()); // 或者可以选择换行
}
// 显示小数点
self.show_char(x, y, '.')?;
x += 6;
// 更准确地计算小数部分
let mut fractional_part = num - (integer_part as f32);
// 将小数部分放大到整数
let mut multiplier = 1u32;
for _ in 0..precision {
multiplier *= 10;
}
// 四舍五入处理
let scaled_fraction = ((fractional_part * (multiplier as f32)) + 0.5) as u32;
// 显示小数位数
let mut temp_multiplier = multiplier / 10;
for _ in 0..precision {
if x > Self::MAX_COLUMN - 6 {
return Ok(());
}
let digit = (scaled_fraction / temp_multiplier) % 10;
self.show_char(x, y, (b'0' + digit as u8) as char)?;
x += 6;
temp_multiplier /= 10;
}
}
Ok(())
}
}示例main.rs如下：
#![no_std]
#![no_main]
use core::fmt::Write;
use embassy_executor::Spawner;
use embassy_stm32::Config as BoardClockConfig;
use embassy_stm32::i2c::{Config as I2cConfig, I2c};
use embassy_stm32::time::Hertz;
use embassy_stm32::{bind_interrupts, i2c, peripherals};
use embassy_stm32::usart::{Uart};
use embassy_time::Timer;
use {defmt_rtt as _, panic_probe as _};
use embassy_stm32::gpio::{Input, Level, Output, Pull, Speed};
mod oled;
use oled::ssd1306_driver;
use oled::ssd1306_driver::DisplayDriver;
bind_interrupts!(struct Irqs {
I2C1_EV => i2c::EventInterruptHandler<peripherals::I2C1>;
I2C1_ER => i2c::ErrorInterruptHandler<peripherals::I2C1>;
});
#[embassy_executor::main]
async fn main(_spawner: Spawner) {
let p = embassy_stm32::init(enable72_mhz());
let mut i2c1_config = I2cConfig::default();
i2c1_config.frequency = Hertz(400_000);
i2c1_config.gpio_speed = Speed::VeryHigh;
let i2c = I2c::new(
p.I2C1,
p.PB6,
p.PB7,
Irqs,
p.DMA1_CH6,
p.DMA1_CH7,
i2c1_config,
);
let mut led0 = Output::new(p.PB0, Level::Low, Speed::Low);
let mut key = Input::new(p.PA1, Pull::Up);
// 启动任务
_spawner.spawn(led_toggle_task(led0)).unwrap();
let mut oled = ssd1306_driver::SSD1306::new(i2c, 0x3C, 0x00, 0x40, DisplayDriver::SH1106);
let _ = oled.init(); // 尝试初始化SSD1306
Timer::after_millis(100).await;
let _ = oled.display_white(); // 尝试点亮屏幕
Timer::after_millis(500).await;
let _ = oled.clear(); // 清屏
// 显示字符串
let _ = oled.show_string(0, 0, "Hello Rust!");
let _ = oled.show_string(0, 1, "SSD1306 OLED");
let mut count:f32 = 0.0;
loop {
Timer::after_millis(1).await;
if key.is_low() {
Timer::after_millis(3).await;
while key.is_low() {
Timer::after_millis(1).await;
}
Timer::after_millis(3).await;
count += 0.01;
let _ = oled.show_string(0, 2, "Key Pressed");
Timer::after_millis(500).await;
} else {
let _ = oled.show_string(0, 2, "Key Released");
}
let _ = oled.show_float(0, 3, count, 3);
}
}
fn enable72_mhz() -> BoardClockConfig {
let mut config = BoardClockConfig::default();
{
use embassy_stm32::rcc::*;
config.rcc.hse = Some(Hse {
freq: Hertz(8_000_000),
mode: HseMode::Oscillator,
});
config.rcc.pll = Some(Pll {
src: PllSource::HSE,
prediv: PllPreDiv::DIV1,
mul: PllMul::MUL9,
});
config.rcc.sys = Sysclk::PLL1_P;
config.rcc.ahb_pre = AHBPrescaler::DIV1;
config.rcc.apb1_pre = APBPrescaler::DIV2;
config.rcc.apb2_pre = APBPrescaler::DIV1;
}
return config;
}
#[embassy_executor::task]
async fn led_toggle_task(mut led: Output<'static>) {
loop {
led.toggle();
Timer::after_millis(100).await;
}
}电机控制motor.rs
use super::pid::PidController;
use embassy_stm32::timer::GeneralInstance4Channel;
use embassy_stm32::timer::qei::Qei;
use embassy_stm32::timer::simple_pwm;
pub struct MotorController<'d, T: GeneralInstance4Channel, U: GeneralInstance4Channel> {
pid: PidController<f32>,
encoder: Qei<'d, T>,
pwm_ch1: simple_pwm::SimplePwmChannel<'d, U>,
pwm_ch2: simple_pwm::SimplePwmChannel<'d, U>,
wheel_diameter: f32, // 轮子直径(mm)
pid_update_rate: f32, // 计算pid的频率, 单位s秒
motor_ppr: f32, // 编码器每圈的脉冲数
target_speed: f32, // 目标速度mm/s
current_speed: f32, // 当前速度mm/s
}
impl<'d, T, U> MotorController<'d, T, U>
where
T: GeneralInstance4Channel,
U: GeneralInstance4Channel,
{
pub fn new(
pid: PidController<f32>,
encoder: Qei<'d, T>,
pwm_ch1: simple_pwm::SimplePwmChannel<'d, U>,
pwm_ch2: simple_pwm::SimplePwmChannel<'d, U>,
wheel_diameter: f32,
pid_update_rate: f32,
motor_ppr: f32,
) -> Self {
Self {
pid,
encoder,
pwm_ch1,
pwm_ch2,
wheel_diameter,
pid_update_rate,
motor_ppr,
target_speed: 0.0,
current_speed: 0.0,
}
}
fn get_encoder_cnt_delta(&self) -> i16 {
let cnt_delta = self.encoder.count() as i16;
self.encoder.reset_count();
cnt_delta
}
fn get_speed(&self, encoder_cnt_delta: i16) -> f32 {
use core::f32::consts::PI;
encoder_cnt_delta as f32 / self.motor_ppr / self.pid_update_rate * PI * self.wheel_diameter
}
pub fn set_target_speed(&mut self, speed: f32) {
self.target_speed = speed;
}
pub fn speed_pid_update(&mut self) {
let encoder_cnt_delta = self.get_encoder_cnt_delta();
self.current_speed = self.get_speed(encoder_cnt_delta);
let pid_output = self.pid.calc(self.current_speed, self.target_speed);
if pid_output > 0.0 {
self.pwm_ch1.set_duty_cycle_percent(pid_output as u8);
self.pwm_ch2.set_duty_cycle_percent(0);
} else {
self.pwm_ch1.set_duty_cycle_percent(0);
self.pwm_ch2.set_duty_cycle_percent(pid_output.abs() as u8);
}
}
pub fn speed(&self) -> f32 {
self.current_speed
}
}