#include "moebus.h"
struct rt_device_pwm* pwm_device;
/*
y
^
|
|
x<-----
*/
struct motor
{
int pin_in1;
int pin_in2;
int pwm_channel;
char* encoder_name;
float spd;
rt_device_t encoder_device;
float cur_spd;
unsigned long prev_time;
long prev_encoder_ticks;
rt_bool_t encoder_reverse;
int pwm;
float bias;
};
struct motor motors[4]=
{
{LF_IN1,LF_IN2,LF_CHANNEL,"pulse2",0,RT_NULL,0,0,0,RT_FALSE,0,0},
{RF_IN1,RF_IN2,RF_CHANNEL,"pulse3",0,RT_NULL,0,0,0,RT_TRUE,0,0},
{LR_IN1,LR_IN2,LR_CHANNEL,"pulse4",0,RT_NULL,0,0,0,RT_FALSE,0,0},
{RR_IN1,RR_IN2,RR_CHANNEL,"pulse5",0,RT_NULL,0,0,0,RT_TRUE,0,0}
};
void Motor_Init(void)
{
pwm_device=(struct rt_device_pwm*)rt_device_find(PWM_NAME);
rt_int8_t index;
for(index=0;index<4;index++)
{
//设置PWM
rt_pin_mode(motors[index].pin_in1,PIN_MODE_OUTPUT);
rt_pin_mode(motors[index].pin_in2,PIN_MODE_OUTPUT);
rt_pwm_set(pwm_device,motors[index].pwm_channel,PWM_PERIOD,0);
rt_pwm_enable(pwm_device,motors[index].pwm_channel);
//设置编码器
motors[index].encoder_device=rt_device_find(motors[index].encoder_name);
if(motors[index].encoder_device!=RT_NULL)
rt_device_open(motors[index].encoder_device,RT_DEVICE_OFLAG_RDONLY);
}
}
//单位是m/s ,r/s
velocities_t get_velocities()
{
velocities_t vel;
vel.linear_y=(motors[0].cur_spd+motors[1].cur_spd+motors[2].cur_spd+motors[3].cur_spd)/4; //rpm
vel.linear_y=vel.linear_y/SDRM_PARAM/1000; //m/s
vel.linear_x=(-motors[0].cur_spd+motors[1].cur_spd+motors[2].cur_spd-motors[3].cur_spd)/4; //rpm
vel.linear_x=vel.linear_x/SDRM_PARAM/1000; //m/s
vel.angular_z=(-motors[0].cur_spd+motors[1].cur_spd-motors[2].cur_spd+motors[3].cur_spd)/4;
vel.angular_z=vel.angular_z/SDRM_PARAM; //r/s
vel.angular_z=vel.angular_z/(a_PARAMETER+b_PARAMETER);
return vel;
}
//单位是rpm
static void get_spd()
{
unsigned long current_time=rt_tick_get();
long current_encoder_ticks=0;
for(unsigned short index=0;index<4;index++)
{
unsigned long dt=current_time-motors[index].prev_time; //单位是ms
float dtm=dt/60000.0; //单位是分钟
rt_device_read(motors[index].encoder_device,0,¤t_encoder_ticks,1);
current_encoder_ticks=motors[index].encoder_reverse?-current_encoder_ticks:current_encoder_ticks;
long delta_ticks=current_encoder_ticks-motors[index].prev_encoder_ticks;
float current_spd=(float)delta_ticks/PULSE_PER_ROTATE/dtm;
if(current_spd-motors[index].cur_spd>500||motors[index].cur_spd-current_spd>500)
current_spd=motors[index].cur_spd;
else
motors[index].cur_spd=current_spd;
motors[index].prev_encoder_ticks=current_encoder_ticks;
motors[index].prev_time=current_time;
}
}
void set_spd(float x_vel, float y_vel, float z_rota) //mmps
{
//关闭全局中断确保pid线程不会得到错误的速度值
int level=rt_hw_interrupt_disable();
z_rota *= (a_PARAMETER + b_PARAMETER);
motors[0].spd = confine((y_vel - x_vel - z_rota) * SDRM_PARAM, -MAX_RPM, MAX_RPM); //rpm
motors[1].spd = confine((y_vel + x_vel + z_rota) * SDRM_PARAM, -MAX_RPM, MAX_RPM); //rpm
motors[2].spd = confine((y_vel + x_vel - z_rota) * SDRM_PARAM, -MAX_RPM, MAX_RPM); //rpm
motors[3].spd = confine((y_vel - x_vel + z_rota) * SDRM_PARAM, -MAX_RPM, MAX_RPM); //rpm
rt_hw_interrupt_enable(level);
}
void set_pwm(void)
{
rt_int8_t index;
for(index=0;index<4;index++)
{
if(motors[index].pwm > 0)
{
rt_pin_write(motors[index].pin_in1,PIN_LOW);
rt_pin_write(motors[index].pin_in2,PIN_HIGH);
rt_pwm_set(pwm_device, motors[index].pwm_channel, PWM_PERIOD, motors[index].pwm);
}
else if(motors[index].pwm < 0)
{
rt_pin_write(motors[index].pin_in1, PIN_HIGH);
rt_pin_write(motors[index].pin_in2, PIN_LOW);
rt_pwm_set(pwm_device, motors[index].pwm_channel, PWM_PERIOD,-motors[index].pwm);
}
else
{
rt_pin_write(motors[index].pin_in1, PIN_LOW);
rt_pin_write(motors[index].pin_in2, PIN_LOW);
rt_pwm_set(pwm_device, motors[index].pwm_channel, PWM_PERIOD, 0);
}
}
}
void pid_update(void)
{
for(unsigned short index=0;index<4;index++)
{
float bias=motors[index].spd-motors[index].cur_spd;
motors[index].pwm+=(int)(Kp*(bias-motors[index].bias)+Ki*bias);
motors[index].bias=bias;
motors[index].pwm=confine(motors[index].pwm,-MAX_PWM,MAX_PWM);
}
}
rt_timer_t PID_Timer;
static void PID_control(void *parameter)
{
get_spd();
pid_update();
set_pwm(); //PID控制器
}
int car_init()
{
PID_Timer=rt_timer_create("PID_Timer",PID_control,RT_NULL,10,RT_TIMER_FLAG_PERIODIC);
if(PID_Timer!=NULL)
rt_timer_start(PID_Timer);
Motor_Init();
return 0;
}
INIT_APP_EXPORT(car_init);
static int car_control_demo(int argc, char **argv)
{
int result=0;
if(argc!=5)
{
rt_kprintf("Usage: car_control_demo x_l y_l z_r");
result=-RT_ERROR;
goto _exit;
}
float x=(float)atoi(argv[2]);
float y=(float)atoi(argv[3]);
float z=(float)atoi(argv[4]);
set_spd(x,y,z);
_exit:
return result;
}
MSH_CMD_EXPORT(car_control_demo,car x_l y_l z_r);
没有合适的资源?快使用搜索试试~ 我知道了~
温馨提示
单片机设计,工具源码,适合毕业设计、课程设计作业,所有源码均经过严格测试,可以直接运行,可以放心下载使用。有任何使用问题欢迎随时与博主沟通,第一时间进行解答! 软件开发设计:PHP、QT、应用软件开发、系统软件开发、移动应用开发、网站开发C++、Java、python、web、C#等语言的项目开发与学习资料 硬件与设备:单片机、EDA、proteus、RTOS、包括计算机硬件、服务器、网络设备、存储设备、移动设备等 操作系统:LInux、IOS、树莓派、安卓开发、微机操作系统、网络操作系统、分布式操作系统等。此外,还有嵌入式操作系统、智能操作系统等。 云计算与大数据:数据集、包括云计算平台、大数据分析、人工智能、机器学习等,云计算是一种基于互联网的计算方式,通过这种方式,共享的软硬件资源和信息可以按需提供给计算机和其他设备。
资源推荐
资源详情
资源评论
收起资源包目录
基于rt-thread系统的STM32麦克纳姆轮ROS地盘.zip (15个子文件)
cm
applications
moebus.c 5KB
rotate.h 287B
mag.c 2KB
DataScope_DP.C 3KB
mpu6050.h 329B
ros.cpp 7KB
kprintf_func.h 140B
kprintf_fun.c 317B
DataScope_DP.h 410B
math_def.h 261B
mpu6050.c 1KB
moebus.h 2KB
main.c 655B
rotate.c 2KB
mag.h 208B
共 15 条
- 1
资源评论
妄北y
- 粉丝: 1w+
- 资源: 1万+
下载权益
C知道特权
VIP文章
课程特权
开通VIP
上传资源 快速赚钱
- 我的内容管理 展开
- 我的资源 快来上传第一个资源
- 我的收益 登录查看自己的收益
- 我的积分 登录查看自己的积分
- 我的C币 登录后查看C币余额
- 我的收藏
- 我的下载
- 下载帮助
最新资源
- Picasso_v3.1 2.ipa
- chromedriver-mac-arm64.zip
- 蓝zapro.apk
- chromedriver-linux64.zip
- UCAS研一深度学习实验-MNIST手写数字识别python源码+详细注释(高分项目)
- 基于Python和PyTorch框架完成的一个手写数字识别实验源码(带MINIST手写数字数据集)+详细注释(高分项目)
- 基于Matlab在MNIST数据集上利用CNN完成手写体数字识别任务,并实现单层CNN反向传播算法+源代码+文档说明(高分项目)
- NVIDIA驱动、CUDA和Pytorch及其依赖
- 基于SVM多特征融合的微表情识别python源码+项目说明+详细注释(高分课程设计)
- html动态爱心代码一(附源码)
资源上传下载、课程学习等过程中有任何疑问或建议,欢迎提出宝贵意见哦~我们会及时处理!
点击此处反馈
安全验证
文档复制为VIP权益,开通VIP直接复制
信息提交成功