机器人进阶学习(一)---基于stm32的底层搭建,与上位机通讯及base_control编写
来源:互联网 发布:卡哇伊字体软件 编辑:程序博客网 时间:2024/06/04 23:22
最近一直忙于底层设计,以及上位机的搭建,现在终于告一段落,决定把整个搭建过程写传来。方便日后查阅,同时回馈各路博客上的援助。
底层:
单片机:stm32f103zet6
电机:额定电压24v,60w,的maxon motor,减速比169:1,减速后50r/m
编码器:HEDL 5540#A02 的500线编码器
通讯方式:串口
小车控制思想
控制电机转动及调速
电机的控制我们分为两部分,一部分为电机转动方向的控制,另一个为电机转速的控制。
电机转动的方向可以通过GPIO的引脚控制驱动上的IN1-1N4,如IN_1=1,IN_2=0 时,电机正转;IN_1=0,IN_B=2时,电机反转;IN_1=0,IN_2=0 时,电机停止。
电机速度的控制则需要一个PWM输出引脚,与驱动上的使能引脚相连,如对于电机1,可以通过一段时间内控制ENA=1,调节占空比调节速度。
PID控制
如果我们想控制小车以一米每秒的速度做直线,但由于地面的摩擦阻力的影响,会造成左右轮速度与我们想控制的速度不同,所以会走不直,这时我们就需要加入PID控制。
PID控制的思想就是我实时的把轮子真正的速度采集回来和控制的速度对比,差则补,多则减。
这样基本就可以实现理想控制。
小车转弯控制。
一般我们要是想控制小车以多少的速度前进或者后退,我们只需要PID控制两个轮子的速度一致就可以基本做到。如果要控制小车以确定的速度以及固定的角度转弯,则需要了解下差速轮的运动控制。下面从三个公式开始简单说下,大家最好自己推导下。
在这里,用v表示线速度,w表示角速度,r表示半径。
在这里,首先要说明几个坐标系
1. 里程计坐标系,是以机器人上电时刻为原点,进行坐标和角度的累加。
2. 机器人坐标系,是以机器人的中心轴为z轴,以逆时针旋转方向为旋转坐标系的正方向,以右手坐标系为参考系。
我们现在用极限的思想去解决运动距离以及角度的描述:
当我们在极短的单位时间内移动很短的距离,转动很小的角度时,根据微元思想,轮子的运动可以看作直线。
∆d=(∆d1+∆d2)/2 //距离变化
∆Ɵ=∆d1-∆d2)/L //角度变化,L为轴距
做第二个轮子反向延长线,可以看到等腰三角形,两个内角之和等于一个不相邻的外角,所以得到一个∆Ɵ/2的变化角
这样可以得到里程计坐标系下的变化值。
∆x= ∆d*cos(Ɵ+ ∆Ɵ/2)
∆y= ∆d*sin(Ɵ+ ∆Ɵ/2)
要求∆d,就要求∆d1,∆d2
我们的编码器是500线的,与169:1的减速器相连,通过A,B相输入,一个周期检测上升沿,下降沿一共四个,这样电机转一圈产生M=500*169*4 个脉冲。∆d1,∆d2可以通过∆t时间内,编码器产生的脉冲计算。若在∆t时间内,左轮产生脉冲个数Nl,右轮产生脉冲个数为Nr,轮子直径为D,
∆d1=Nl*3.14*D /M,∆d2=Nr*3.14*D/M
∆d=(Nr+Nl)*3.14D/2M,∆Ɵ=(Nr-Nl)*3.14D/L*M
基本上所有的差速轮的里程计坐标下的数据计算都是基于上述思想。
stm32 上的程序如下:github: https://github.com/ncnynl/ros-car-stm32.git
(1)main.c 接收和发送串口数据,控制电机
/*********************************************** 说明 ******************************************************************* 1.串口接收* (1)内容:小车左右轮速度,单位:mm/s(所有数据都为 float型,float型占4字节)* (2)格式:10字节 [右轮速度4字节][左轮速度4字节][结束符"\r\n"2字节]** 2.串口发送* (1)内容:里程计(x,y坐标、线速度、角速度和方向角,单位依次为:mm,mm,mm/s,rad/s,rad,所有数据都为float型,float型占4字节)* (2)格式:21字节 [x坐标4字节][y坐标4字节][方向角4字节][线速度4字节][角速度4字节][结束符"\n"1字节]*************************************************************************************************************************/#include "stm32f10x.h"#include "stm32f10x_it.h"#include "delay.h"#include "nvic_conf.h"#include "rcc_conf.h"#include "usart.h"#include "encoder.h"#include "contact.h"#include "gpio_conf.h"#include "tim2_5_6.h"#include "odometry.h"#include "tim2_5_6.h"#include "stdbool.h"#include <stdio.h>#include "string.h"#include "math.h"/*********************************************** 输出 *****************************************************************/char odometry_data[21]={0}; //发送给串口的里程计数据数组float odometry_right=0,odometry_left=0;//串口得到的左右轮速度/*********************************************** 输入 *****************************************************************/extern float position_x,position_y,oriention,velocity_linear,velocity_angular; //计算得到的里程计数值extern u8 USART_RX_BUF[USART_REC_LEN]; //串口接收缓冲,最大USART_REC_LEN个字节.extern u16 USART_RX_STA; //串口接收状态标记 extern float Milemeter_L_Motor,Milemeter_R_Motor; //dt时间内的左右轮速度,用于里程计计算/*********************************************** 变量 *****************************************************************/u8 main_sta=0; //用作处理主函数各种if,去掉多余的flag(1打印里程计)(2调用计算里程计数据函数)(3串口接收成功)(4串口接收失败)union recieveData //接收到的数据{ float d; //左右轮速度 unsigned char data[4];}leftdata,rightdata; //接收的左右轮数据union odometry //里程计数据共用体{ float odoemtry_float; unsigned char odometry_char[4];}x_data,y_data,theta_data,vel_linear,vel_angular; //要发布的里程计数据,分别为:X,Y方向移动的距离,当前角度,线速度,角速度/****************************************************************************************************************/ int main(void){ u8 t=0;u8 i=0,j=0,m=0;RCC_Configuration(); //系统时钟配置 NVIC_Configuration(); //中断优先级配置GPIO_Configuration(); //电机方向控制引脚配置USART1_Config(); //串口1配置TIM2_PWM_Init(); //PWM输出初始化ENC_Init(); //电机处理初始化TIM5_Configuration(); //速度计算定时器初始化TIM1_Configuration(); //里程计发布定时器初始化while (1){ if(main_sta&0x01)//执行发送里程计数据步骤 { //里程计数据获取 x_data.odoemtry_float=position_x;//单位mm y_data.odoemtry_float=position_y;//单位mm theta_data.odoemtry_float=oriention;//单位rad vel_linear.odoemtry_float=velocity_linear;//单位mm/s vel_angular.odoemtry_float=velocity_angular;//单位rad/s //将所有里程计数据存到要发送的数组 for(j=0;j<4;j++) { odometry_data[j]=x_data.odometry_char[j]; odometry_data[j+4]=y_data.odometry_char[j]; odometry_data[j+8]=theta_data.odometry_char[j]; odometry_data[j+12]=vel_linear.odometry_char[j]; odometry_data[j+16]=vel_angular.odometry_char[j]; } odometry_data[20]='\n';//添加结束符 //发送数据要串口 for(i=0;i<21;i++) { USART_ClearFlag(USART1,USART_FLAG_TC); //在发送第一个数据前加此句,解决第一个数据不能正常发送的问题 USART_SendData(USART1,odometry_data[i]);//发送一个字节到串口 while(USART_GetFlagStatus(USART1, USART_FLAG_TC) == RESET); //等待发送结束 } main_sta&=0xFE;//执行计算里程计数据步骤 } if(main_sta&0x02)//执行计算里程计数据步骤 { odometry(Milemeter_R_Motor,Milemeter_L_Motor);//计算里程计 main_sta&=0xFD;//执行发送里程计数据步骤 } if(main_sta&0x08) //当发送指令没有正确接收时 { USART_ClearFlag(USART1,USART_FLAG_TC); //在发送第一个数据前加此句,解决第一个数据不能正常发送的问题 for(m=0;m<3;m++) { USART_SendData(USART1,0x00); while(USART_GetFlagStatus(USART1, USART_FLAG_TC) == RESET); } USART_SendData(USART1,'\n'); while(USART_GetFlagStatus(USART1, USART_FLAG_TC) == RESET); main_sta&=0xF7; } if(USART_RX_STA&0x8000) // 串口1接收函数 { //接收左右轮速度 for(t=0;t<4;t++) { rightdata.data[t]=USART_RX_BUF[t]; leftdata.data[t]=USART_RX_BUF[t+4]; } //储存左右轮速度 odometry_right=rightdata.d;//单位mm/s odometry_left=leftdata.d;//单位mm/s USART_RX_STA=0;//清楚接收标志位 } car_control(rightdata.d,leftdata.d); //将接收到的左右轮速度赋给小车 }//end_while}//end main/*********************************************END OF FILE**************************************************/2.odometry.c 里程计计算#include "odometry.h"/*********************************************** 输出 *****************************************************************/float position_x=0,position_y=0,oriention=0,velocity_linear=0,velocity_angular=0;/*********************************************** 输入 *****************************************************************/extern float odometry_right,odometry_left;//串口得到的左右轮速度/*********************************************** 变量 *****************************************************************/float wheel_interval= 268.0859f;// 272.0f; // 1.0146//float wheel_interval=276.089f; //轴距校正值=原轴距/0.987float multiplier=4.0f; //倍频数float deceleration_ratio=90.0f; //减速比float wheel_diameter=100.0f; //轮子直径,单位mmfloat pi_1_2=1.570796f; //π/2float pi=3.141593f; //πfloat pi_3_2=4.712389f; //π*3/2float pi_2_1=6.283186f; //π*2float dt=0.005f; //采样时间间隔5msfloat line_number=4096.0f; //码盘线数float oriention_interval=0; //dt时间内方向变化值float sin_=0; //角度计算值float cos_=0;float delta_distance=0,delta_oriention=0; //采样时间间隔内运动的距离float const_frame=0,const_angle=0,distance_sum=0,distance_diff=0;float oriention_1=0;u8 once=1;/****************************************************************************************************************///里程计计算函数void odometry(float right,float left){ if(once) //常数仅计算一次 { const_frame=wheel_diameter*pi/(line_number*multiplier*deceleration_ratio); const_angle=const_frame/wheel_interval; once=0; } distance_sum = 0.5f*(right+left);//在很短的时间内,小车行驶的路程为两轮速度和 distance_diff = right-left;//在很短的时间内,小车行驶的角度为两轮速度差 //根据左右轮的方向,纠正短时间内,小车行驶的路程和角度量的正负 if((odometry_right>0)&&(odometry_left>0)) //左右均正 { delta_distance = distance_sum; delta_oriention = distance_diff; } else if((odometry_right<0)&&(odometry_left<0)) //左右均负 { delta_distance = -distance_sum; delta_oriention = -distance_diff; } else if((odometry_right<0)&&(odometry_left>0)) //左正右负 { delta_distance = -distance_diff; delta_oriention = -2.0f*distance_sum; } else if((odometry_right>0)&&(odometry_left<0)) //左负右正 { delta_distance = distance_diff; delta_oriention = 2.0f*distance_sum; } else { delta_distance=0; delta_oriention=0; } oriention_interval = delta_oriention * const_angle;//采样时间内走的角度 oriention = oriention + oriention_interval;//计算出里程计方向角 oriention_1 = oriention + 0.5f * oriention_interval;//里程计方向角数据位数变化,用于三角函数计算 sin_ = sin(oriention_1);//计算出采样时间内y坐标 cos_ = cos(oriention_1);//计算出采样时间内x坐标 position_x = position_x + delta_distance * cos_ * const_frame;//计算出里程计x坐标 position_y = position_y + delta_distance * sin_ * const_frame;//计算出里程计y坐标 velocity_linear = delta_distance*const_frame / dt;//计算出里程计线速度 velocity_angular = oriention_interval / dt;//计算出里程计角速度 //方向角角度纠正 if(oriention > pi) { oriention -= pi_2_1; } else { if(oriention < -pi) { oriention += pi_2_1; } }}3.PID.c PID处理函数#include "PID.h"extern int span;float MaxValue=9;//输出最大限幅float MinValue=-9;//输出最小限幅float OutputValue;//PID输出暂存变量,用于积分饱和抑制float PID_calculate(struct PID *Control,float CurrentValue_left )//位置PID计算B{ float Value_Kp;//比例分量 float Value_Ki;//积分分量 float Value_Kd;//微分分量 Control->error_0 = Control->OwenValue - CurrentValue_left + 0*span;//基波分量,Control->OwenValue为想要的速度,CurrentValue_left为电机真实速度 Value_Kp = Control->Kp * Control->error_0 ; Control->Sum_error += Control->error_0; /***********************积分饱和抑制********************************************/ OutputValue = Control->OutputValue; if(OutputValue>5 || OutputValue<-5) { Control->Ki = 0; } /*******************************************************************/ Value_Ki = Control->Ki * Control->Sum_error; Value_Kd = Control->Kd * ( Control->error_0 - Control->error_1); Control->error_1 = Control->error_0;//保存一次谐波 Control->OutputValue = Value_Kp + Value_Ki + Value_Kd;//输出值计算,注意加减 //限幅 if( Control->OutputValue > MaxValue) Control->OutputValue = MaxValue; if (Control->OutputValue < MinValue) Control->OutputValue = MinValue; return (Control->OutputValue) ;}4.encoder.c 电机编码处理#include "encoder.h"/****************************************************************************************************************/s32 hSpeed_Buffer2[SPEED_BUFFER_SIZE]={0}, hSpeed_Buffer1[SPEED_BUFFER_SIZE]={0};//左右轮速度缓存数组static unsigned int hRot_Speed2;//电机A平均转速缓存static unsigned int hRot_Speed1;//电机B平均转速缓存unsigned int Speed2=0; //电机A平均转速 r/min,PID调节unsigned int Speed1=0; //电机B平均转速 r/min,PID调节static volatile u16 hEncoder_Timer_Overflow1;//电机B编码数采集 static volatile u16 hEncoder_Timer_Overflow2;//电机A编码数采集//float A_REMP_PLUS;//电机APID调节后的PWM值缓存float pulse = 0;//电机A PID调节后的PWM值缓存float pulse1 = 0;//电机B PID调节后的PWM值缓存int span;//采集回来的左右轮速度差值static bool bIs_First_Measurement2 = true;//电机A以清除速度缓存数组标志位static bool bIs_First_Measurement1 = true;//电机B以清除速度缓存数组标志位struct PID Control_left ={0.01,0.1,0.75,0,0,0,0,0,0};//左轮PID参数,适于新电机4096struct PID Control_right ={0.01,0.1,0.75,0,0,0,0,0,0};//右轮PID参数,适于新电机4096/****************************************************************************************************************/s32 hPrevious_angle2, hPrevious_angle1;/****************************************************************************************************************/void ENC_Init2(void)//电机A码盘采集定时器,TIM4编码器模式{ TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; TIM_ICInitTypeDef TIM_ICInitStructure; GPIO_InitTypeDef GPIO_InitStructure; RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE); RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); GPIO_StructInit(&GPIO_InitStructure); GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_Init(GPIOB, &GPIO_InitStructure); TIM_DeInit(ENCODER2_TIMER); TIM_TimeBaseStructInit(&TIM_TimeBaseStructure); TIM_TimeBaseStructure.TIM_Prescaler = 0; TIM_TimeBaseStructure.TIM_Period = (4*ENCODER2_PPR)-1; TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; TIM_TimeBaseInit(ENCODER2_TIMER, &TIM_TimeBaseStructure); TIM_EncoderInterfaceConfig(ENCODER2_TIMER, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising); TIM_ICStructInit(&TIM_ICInitStructure); TIM_ICInitStructure.TIM_ICFilter = ICx_FILTER; TIM_ICInit(ENCODER2_TIMER, &TIM_ICInitStructure); TIM_ClearFlag(ENCODER2_TIMER, TIM_FLAG_Update); TIM_ITConfig(ENCODER2_TIMER, TIM_IT_Update, ENABLE); TIM_Cmd(ENCODER2_TIMER, ENABLE); }void ENC_Init1(void)//电机B码盘采集定时器,TIM3编码器模式{ TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; TIM_ICInitTypeDef TIM_ICInitStructure; GPIO_InitTypeDef GPIO_InitStructure; RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE); RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); GPIO_StructInit(&GPIO_InitStructure); GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_Init(GPIOA, &GPIO_InitStructure); TIM_DeInit(ENCODER1_TIMER); TIM_TimeBaseStructInit(&TIM_TimeBaseStructure); TIM_TimeBaseStructure.TIM_Prescaler = 0; TIM_TimeBaseStructure.TIM_Period = (4*ENCODER1_PPR)-1; TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; TIM_TimeBaseInit(ENCODER1_TIMER, &TIM_TimeBaseStructure); TIM_EncoderInterfaceConfig(ENCODER1_TIMER, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising); TIM_ICStructInit(&TIM_ICInitStructure); TIM_ICInitStructure.TIM_ICFilter = ICx_FILTER; TIM_ICInit(ENCODER1_TIMER, &TIM_ICInitStructure); TIM_ClearFlag(ENCODER1_TIMER, TIM_FLAG_Update); TIM_ITConfig(ENCODER1_TIMER, TIM_IT_Update, ENABLE); TIM_Cmd(ENCODER1_TIMER, ENABLE); }/****************************************************************************************************************/s16 ENC_Calc_Rot_Speed2(void)//计算电机A的编码数{ s32 wDelta_angle; u16 hEnc_Timer_Overflow_sample_one; u16 hCurrent_angle_sample_one; s32 temp; s16 haux; if (!bIs_First_Measurement2)//电机A以清除速度缓存数组 { hEnc_Timer_Overflow_sample_one = hEncoder_Timer_Overflow2; hCurrent_angle_sample_one = ENCODER2_TIMER->CNT; hEncoder_Timer_Overflow2 = 0; haux = ENCODER2_TIMER->CNT; if ( (ENCODER2_TIMER->CR1 & TIM_CounterMode_Down) == TIM_CounterMode_Down) { // encoder timer down-counting 反转的速度计算 wDelta_angle = (s32)((hEnc_Timer_Overflow_sample_one) * (4*ENCODER2_PPR) -(hCurrent_angle_sample_one - hPrevious_angle2)); } else { //encoder timer up-counting 正转的速度计算 wDelta_angle = (s32)(hCurrent_angle_sample_one - hPrevious_angle2 + (hEnc_Timer_Overflow_sample_one) * (4*ENCODER2_PPR)); } temp=wDelta_angle; } else { bIs_First_Measurement2 = false;//电机A以清除速度缓存数组标志位 temp = 0; hEncoder_Timer_Overflow2 = 0; haux = ENCODER2_TIMER->CNT; } hPrevious_angle2 = haux; return((s16) temp);}s16 ENC_Calc_Rot_Speed1(void)//计算电机B的编码数{ s32 wDelta_angle; u16 hEnc_Timer_Overflow_sample_one; u16 hCurrent_angle_sample_one; s32 temp; s16 haux; if (!bIs_First_Measurement1)//电机B以清除速度缓存数组 { hEnc_Timer_Overflow_sample_one = hEncoder_Timer_Overflow1; //得到采样时间内的编码数 hCurrent_angle_sample_one = ENCODER1_TIMER->CNT; hEncoder_Timer_Overflow1 = 0;//清除脉冲数累加 haux = ENCODER1_TIMER->CNT; if ( (ENCODER1_TIMER->CR1 & TIM_CounterMode_Down) == TIM_CounterMode_Down) { // encoder timer down-counting 反转的速度计算 wDelta_angle = (s32)((hEnc_Timer_Overflow_sample_one) * (4*ENCODER1_PPR) -(hCurrent_angle_sample_one - hPrevious_angle1)); } else { //encoder timer up-counting 正转的速度计算 wDelta_angle = (s32)(hCurrent_angle_sample_one - hPrevious_angle1 + (hEnc_Timer_Overflow_sample_one) * (4*ENCODER1_PPR)); } temp=wDelta_angle; } else { bIs_First_Measurement1 = false;//电机B以清除速度缓存数组标志位 temp = 0; hEncoder_Timer_Overflow1 = 0; haux = ENCODER1_TIMER->CNT; } hPrevious_angle1 = haux; return((s16) temp);}/****************************************************************************************************************/void ENC_Clear_Speed_Buffer(void)//速度存储器清零{ u32 i; //清除左右轮速度缓存数组 for (i=0;i<SPEED_BUFFER_SIZE;i++) { hSpeed_Buffer2[i] = 0; hSpeed_Buffer1[i] = 0; } bIs_First_Measurement2 = true;//电机A以清除速度缓存数组标志位 bIs_First_Measurement1 = true;//电机B以清除速度缓存数组标志位}void ENC_Calc_Average_Speed(void)//计算三次电机的平均编码数{ u32 i; signed long long wtemp3=0; signed long long wtemp4=0; //累加缓存次数内的速度值 for (i=0;i<SPEED_BUFFER_SIZE;i++) { wtemp4 += hSpeed_Buffer2[i]; wtemp3 += hSpeed_Buffer1[i]; } //取平均,平均脉冲数单位为 个/s wtemp3 /= (SPEED_BUFFER_SIZE); wtemp4 /= (SPEED_BUFFER_SIZE); //平均脉冲数 个/s //将平均脉冲数单位转为 r/min wtemp3 = (wtemp3 * SPEED_SAMPLING_FREQ)*60/(4*ENCODER1_PPR); wtemp4 = (wtemp4 * SPEED_SAMPLING_FREQ)*60/(4*ENCODER2_PPR); hRot_Speed2= ((s16)(wtemp4));//平均转速 r/min hRot_Speed1= ((s16)(wtemp3));//平均转速 r/min Speed2=hRot_Speed2;//平均转速 r/min Speed1=hRot_Speed1;//平均转速 r/min}/****************************************************************************************************************/void Gain2(void)//设置电机A PID调节 PA2{ //static float pulse = 0; span=1*(Speed1-Speed2);//采集回来的左右轮速度差值 pulse= pulse + PID_calculate(&Control_right,hRot_Speed2);//PID调节 //pwm幅度抑制 if(pulse > 3600) pulse = 3600; if(pulse < 0) pulse = 0; //A_REMP_PLUS=pulse;//电机APID调节后的PWM值缓存}void Gain1(void)//设置电机B PID调节 PA1{ //static float pulse1 = 0; span=1*(Speed2-Speed1);//采集回来的左右轮速度差值 pulse1= pulse1 + PID_calculate(&Control_left,hRot_Speed1);//PID调节 ////pwm 幅度抑制 if(pulse1 > 3600) pulse1 = 3600; if(pulse1 < 0) pulse1 = 0; TIM2->CCR2 = pulse1;//电机B赋值PWM //TIM2->CCR3 = A_REMP_PLUS;//电机A赋值PWM TIM2->CCR3 = pulse;//电机A赋值PWM}/****************************************************************************************************************/void ENC_Init(void)//电机处理初始化{ ENC_Init2(); //设置电机A TIM4编码器模式PB6 PB7 右电机 ENC_Init1(); //设置电机B TIM3编码器模式PA6 PA7 左电机 ENC_Clear_Speed_Buffer();//速度存储器清零}/****************************************************************************************************************/void TIM4_IRQHandler (void)//执行TIM4(电机A编码器采集)计数中断{ TIM_ClearFlag(ENCODER2_TIMER, TIM_FLAG_Update); if (hEncoder_Timer_Overflow2 != U16_MAX)//不超范围 { hEncoder_Timer_Overflow2++; //脉冲数累加 }}void TIM3_IRQHandler (void)//执行TIM3(电机B编码器采集)计数中断{ TIM_ClearFlag(ENCODER1_TIMER, TIM_FLAG_Update); if (hEncoder_Timer_Overflow1 != U16_MAX)//不超范围 { hEncoder_Timer_Overflow1++; //脉冲数累加 }}5.contact.c 电机控制函数#include "contact.h"/*********************************************** 输出 *****************************************************************//*********************************************** 输入 *****************************************************************/extern struct PID Control_left;//左轮PID参数,适于新电机4096extern struct PID Control_right;//右轮PID参数,适于新电机4096/*********************************************** 变量 *****************************************************************//*******************************************************************************************************************/void LeftMovingSpeedW(unsigned int val)//左轮方向和速度控制函数{ if(val>10000) { GPIO_SetBits(GPIOC, GPIO_Pin_6); GPIO_ResetBits(GPIOC, GPIO_Pin_7); Control_left.OwenValue=(val-10000);//PID调节的目标编码数 } else if(val<10000) { GPIO_SetBits(GPIOC, GPIO_Pin_7); GPIO_ResetBits(GPIOC, GPIO_Pin_6); Control_left.OwenValue=(10000-val);//PID调节的目标编码数 } else { GPIO_SetBits(GPIOC, GPIO_Pin_6); GPIO_SetBits(GPIOC, GPIO_Pin_7); Control_left.OwenValue=0;//PID调节的目标编码数 } }void RightMovingSpeedW(unsigned int val2)//右轮方向和速度控制函数{ if(val2>10000) { /* motor A 正转*/ GPIO_SetBits(GPIOC, GPIO_Pin_10); GPIO_ResetBits(GPIOC, GPIO_Pin_11); Control_right.OwenValue=(val2-10000);//PID调节的目标编码数 } else if(val2<10000) { /* motor A 反转*/ GPIO_SetBits(GPIOC, GPIO_Pin_11); GPIO_ResetBits(GPIOC, GPIO_Pin_10); Control_right.OwenValue=(10000-val2);//PID调节的目标编码数 } else { GPIO_SetBits(GPIOC, GPIO_Pin_10); GPIO_SetBits(GPIOC, GPIO_Pin_11); Control_right.OwenValue=0;//PID调节的目标编码数 } }void car_control(float rightspeed,float leftspeed)//小车速度转化和控制函数{ float k2=17.179; //速度转换比例,转/分钟 //将从串口接收到的速度转换成实际控制小车的速度?还是PWM? int right_speed=(int)k2*rightspeed; int left_speed=(int)k2*leftspeed; RightMovingSpeedW(right_speed+10000); LeftMovingSpeedW(left_speed+10000);}//void Contact_Init(void)//左右轮方向和速度初始化//{// LeftMovingSpeedW(12000); //电机B// RightMovingSpeedW(12000);//电机A //}
主要参考:
http://www.ncnynl.com/archives/201703/1416.html
http://blog.csdn.net/david_han008/article/details/78573616
- 机器人进阶学习(一)---基于stm32的底层搭建,与上位机通讯及base_control编写
- 机器人进阶学习(二)--上位机上的程序搭建
- 机器人技术—基于DELPHI的数据采集与分析类上位机软件的编写
- STM32与上位机串口通讯的学习笔记(简明的数据帧设计方法)
- 上位机与PLC通讯及OPC数据采集实践一
- 基于VC++和MFC的上位机与PLC的通讯系统
- 基于OPC 技术的上位机与S7-200 系列PLC通讯的实现
- VS2008编写tcp客户端上位机与STM32单片机通信(结构体)
- 一、c++上位机与WiFi小车通讯
- vc--基于mfc的上位机设计(一)
- 基于C#+51上位机通讯程序
- CAN 学习笔记一_【基于STM32的CANopen通讯协议的实现】
- 求救 基于RS485的PC机与多下位机通讯时上位机奇偶校验位的设置 20分
- QT学习——QT5串口编写的上位机
- 用VB编写的上位机与嵌入式LINUX下位机的网络通信(TCP)
- 基于Linux的ARM与上位机文件传输
- STM32单片机上位机程序代码(供参考)(基于C#开发)
- 基于stm32的解魔方机器人
- 完全卸载
- Android studio 华为调试不打印崩溃日志
- Jsp中的静态引入和动态引入
- 程序、编译器和操作系统
- 编译Hi3516a osdrv目录
- 机器人进阶学习(一)---基于stm32的底层搭建,与上位机通讯及base_control编写
- CSS设置图片转向
- 机器学习之支持向量机SVM Support Vector Machine (六) 高斯核调参
- 解决JSONObject.fromObject数字为null时被转换为0
- 移动开发-----自定义View(圆环)
- HTML 中引入调用另一个 HTML 的方法
- 像Apache Storm一样简单的分布式图计算
- 中文乱码
- 关于centos 7 中service iptables save 指令使用失败的结局方案