3.12节 电机码盘实现里程计实验
本节介绍里程计如何实现
a.实验准备：码盘电机，USB转串口模块，ST-Llink下载器，CHEAPX机器人控制板
b.实验目的：STM32实现里程计的数据采集
c.相关知识点：
里程计，指的是机器人行走的距离，通过统计码盘脉冲值转化而来。
d.编程及运行
(1)初始化
void initEncoder(void) { //LFT ENC A:PB7 B:PB6 //RGT ENC A:PB5 B:PB4 GPIO_InitTypeDef GPIO_InitStruct; EXTI_InitTypeDef EXTI_InitStruct; NVIC_InitTypeDef NVIC_InitStruct; //Config Clock 配置时钟 RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO|RCC_APB2Periph_GPIOB, ENABLE); GPIO_InitStruct.GPIO_Pin = GPIO_Pin_7 | GPIO_Pin_6| GPIO_Pin_5| GPIO_Pin_4; GPIO_InitStruct.GPIO_Mode = GPIO_Mode_IPU;//上拉输入 GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz; GPIO_Init(GPIOB, &GPIO_InitStruct);//传入结构体GPIO_InitStruct,配置GPIO //Config EXTILine 配置外部中断线 //PB7 GPIO_EXTILineConfig(GPIO_PortSourceGPIOB,GPIO_PinSource7); EXTI_InitStruct.EXTI_Line=EXTI_Line7; //外部中断线7 EXTI_InitStruct.EXTI_Mode = EXTI_Mode_Interrupt; //中断模式 EXTI_InitStruct.EXTI_Trigger = EXTI_Trigger_Rising_Falling;//上升沿和下降沿都触发 EXTI_InitStruct.EXTI_LineCmd = ENABLE; //使能中断线 EXTI_Init(&EXTI_InitStruct); //传入结构EXTI_InitStruct,配置外部中断 NVIC_InitStruct.NVIC_IRQChannel = EXTI9_5_IRQn; //外部中断通道9-5 NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority = 0x02; //抢占优先级 NVIC_InitStruct.NVIC_IRQChannelSubPriority = 0x03; //子优先级 NVIC_InitStruct.NVIC_IRQChannelCmd = ENABLE; //使能中断 NVIC_Init(&NVIC_InitStruct); //传入结构NVIC_InitStruct,配置NVIC //PB6 GPIO_EXTILineConfig(GPIO_PortSourceGPIOB,GPIO_PinSource6); EXTI_InitStruct.EXTI_Line=EXTI_Line6; //外部中断线6 EXTI_InitStruct.EXTI_Mode = EXTI_Mode_Interrupt; //中断模式 EXTI_InitStruct.EXTI_Trigger = EXTI_Trigger_Rising_Falling;//上升沿和下降沿都触发 EXTI_InitStruct.EXTI_LineCmd = ENABLE; //使能中断线 EXTI_Init(&EXTI_InitStruct); //传入结构EXTI_InitStruct,配置外部中断 NVIC_InitStruct.NVIC_IRQChannel = EXTI9_5_IRQn; //外部中断通道 9-5 NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority = 0x02; //抢占优先级 NVIC_InitStruct.NVIC_IRQChannelSubPriority = 0x03; //子优先级 NVIC_InitStruct.NVIC_IRQChannelCmd = ENABLE; //使能中断 NVIC_Init(&NVIC_InitStruct); //传入结构NVIC_InitStruct,配置NVIC //PB5 GPIO_EXTILineConfig(GPIO_PortSourceGPIOB,GPIO_PinSource5); EXTI_InitStruct.EXTI_Line=EXTI_Line5; //外部中断线5 EXTI_InitStruct.EXTI_Mode = EXTI_Mode_Interrupt; //中断模式 EXTI_InitStruct.EXTI_Trigger = EXTI_Trigger_Rising_Falling;//上升沿和下降沿都触发 EXTI_InitStruct.EXTI_LineCmd = ENABLE; //使能中断线 EXTI_Init(&EXTI_InitStruct); //传入结构EXTI_InitStruct,配置外部中断 NVIC_InitStruct.NVIC_IRQChannel = EXTI9_5_IRQn; //外部中断通道9-5 NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority = 0x02; //抢占优先级 NVIC_InitStruct.NVIC_IRQChannelSubPriority = 0x03; //子优先级 NVIC_InitStruct.NVIC_IRQChannelCmd = ENABLE; //使能中断 NVIC_Init(&NVIC_InitStruct); //传入结构NVIC_InitStruct,配置NVIC //PB4 GPIO_EXTILineConfig(GPIO_PortSourceGPIOB,GPIO_PinSource4); EXTI_InitStruct.EXTI_Line=EXTI_Line4; //外部中断线4 EXTI_InitStruct.EXTI_Mode = EXTI_Mode_Interrupt; //中断模式 EXTI_InitStruct.EXTI_Trigger = EXTI_Trigger_Rising_Falling;//上升沿和下降沿都触发 EXTI_InitStruct.EXTI_LineCmd = ENABLE; //使能中断线 EXTI_Init(&EXTI_InitStruct); //传入结构EXTI_InitStruct,配置外部中断 NVIC_InitStruct.NVIC_IRQChannel = EXTI4_IRQn; //外部中断通道4 NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority = 0x02; //抢占优先级 NVIC_InitStruct.NVIC_IRQChannelSubPriority = 0x03; //子优先级 NVIC_InitStruct.NVIC_IRQChannelCmd = ENABLE; //使能中断 NVIC_Init(&NVIC_InitStruct); //传入结构NVIC_InitStruct,配置NVIC }
(2)更新函数
void updateEncoder(u8 id) { static u32 lastTimestampUS[2]={0,0}; static float unCountEncoder[2]={0,0}; u32 TimestampUS=0; u32 deltaTimeUS=0; float deltaEncoderMM=0; //Timestamp时间戳，用于计算速度 TimestampUS=9000000-SysTick->VAL; if(TimestampUS<lastTimestampUS[id])deltaTimeUS=TimestampUS+9000000-lastTimestampUS[id]; else deltaTimeUS=TimestampUS-lastTimestampUS[id]; lastTimestampUS[id]=TimestampUS; //码盘同时输出2路相差90度相位的方波如下，因此码盘值应当只相差2。 /* _ _ _ _ _ _ _ _ __________| |_| |_| |_| |_| |_| |_| |_| |_____________________________ _ _ _ _ _ _ _ _ ___________| |_| |_| |_| |_| |_| |_| |_| |____________________________ */ if(Encoder[id][0]>Encoder[id][1]+2)Encoder[id][1]=Encoder[id][0]; else if(Encoder[id][1]>Encoder[id][0]+2)Encoder[id][0]=Encoder[id][1]; else; deltaEncoder[id]=Encoder[id][0]+Encoder[id][1]+unCountEncoder[id]; //odom里程计 deltaEncoderMM=deltaEncoder[id]/encoder2mm;//码盘脉冲计数值转换为距离mm；encoder2mm = 轮子转一圈的码盘计数值/轮子周长 = 1560 / 204 = 7.6470588235294117647058823529412 unCountEncoder[id]=deltaEncoder[id]-deltaEncoderMM*encoder2mm; //计算由于精度原因未统计的值 _EncoderUM[id]=_EncoderUM[id]+_motorDir[id]*deltaEncoderMM*1000; //计算总里程 //caculate spd(unit: mm/s) if(deltaTimeUS>0) { MotorActualSpd[id] = deltaEncoderMM*(9000000/deltaTimeUS); //deltaTimeUS的单位是1/9us，转化为s,需要乘以9000000 } if(id==0)printf("左码盘:%2u个;v=%3dmm/s,dt:%2u毫秒;",deltaEncoder[id],(int)MotorActualSpd[id],deltaTimeUS/9000); if(id==1)printf("右码盘:%2u个;v=%3dmm/s,dt:%2u毫秒.\n",deltaEncoder[id],(int)MotorActualSpd[id],deltaTimeUS/9000); //清零 Encoder[id][0]=0; Encoder[id][1]=0; }
(3)主函数
//电机码盘实现里程计实验 int main(void) { NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); //PriorityGroupConfig NVIC中断分组设置 组2(2位抢占优先级，2位响应优先级) initDebugSerial(500000);//初始化调试串口USART1，波特率500000 initSysTick();//初始化滴答定时器和TIM4定时器 showVersion();//显示版本 initMoter(); //初始化电机 initEncoder(); //参数：左电机速度，左电机方向，右电机速度，右电机方向 //速度值范围[4000,30000],值过小电机可能会不转 //方向：参照小车朝向，0往后转，1向前转 moterControl(5000,1,5000,1); while(1) { updateEncoder(0); updateEncoder(1); delay_ms(10); } }
(4)实验结果
