RoboGame参赛总结-3
驱动板简单介绍
我们选用的是最常用的以pca9685为基础的PWM驱动板，以IIC作为通信协议，如果使用Arduino的话，控制相当简单，但STM32因为缺乏相关库，所以驱动的编写有些复杂。
先了解一下原理：
机械臂的运动主要依靠舵机，也就是一种伺服电机，由PWM信号决定运行的角度，并且没有信号就罢工了，这么多的PWM波，依靠STM32本身来维持是很糟糕的，16路舵机驱动板很好的解决了这个问题。我们只需要通过IIC给出角度信号，产生和维持PWM信号的工作就交给驱动板了。
这里需要基于IIC的驱动函数，这个在网上很容易找到：
void IIC_Init(void); //初始化IIC的IO口
void IIC_Start(void); //发送IIC开始信号
void IIC_Stop(void); //发送IIC停止信号
void IIC_Send_Byte(u8 txd); //IIC发送一个字节
u8 IIC_Read_Byte(unsigned char ack);//IIC读取一个字节
u8 IIC_Wait_Ack(void); //IIC等待ACK信号
void IIC_Ack(void); //IIC发送ACK信号
void IIC_NAck(void); //IIC不发送ACK信号
驱动板函数编写
关键的问题在于如何方便快捷的调用舵机运动，使得main函数变得简洁便于更改维护，先介绍一些基本的对pca9685的操作：
我们需要一些宏定义：
#define PCA9685_SUBADR1 0x2
#define PCA9685_SUBADR2 0x3
#define PCA9685_SUBADR3 0x4
#define pca_adrr 0x80
#define pca_mode1 0x0
#define pca_pre 0xFE
#define LED0_ON_L 0x6
#define LED0_ON_H 0x7
#define LED0_OFF_L 0x8
#define LED0_OFF_H 0x9
#define ALLLED_ON_L 0xFA
#define ALLLED_ON_H 0xFB
#define ALLLED_OFF_L 0xFC
#define ALLLED_OFF_H 0xFD
其实这些控制逻辑跟使用LCD屏幕是高度类似的，就是发送一些定义好的指令就可以了：
void pca_write(u8 adrr,u8 data)
{
IIC_Start();
IIC_Send_Byte(pca_adrr);
IIC_Wait_Ack();
IIC_Send_Byte(adrr);
IIC_Wait_Ack();
IIC_Send_Byte(data);
IIC_Wait_Ack();
IIC_Stop();
}
u8 pca_read(u8 adrr)
{
u8 data;
IIC_Start();
IIC_Send_Byte(pca_adrr);
IIC_Wait_Ack();
IIC_Send_Byte(adrr);
IIC_Wait_Ack();
IIC_Start();
IIC_Send_Byte(pca_adrr|0x01);
IIC_Wait_Ack();
data=IIC_Read_Byte(0);
IIC_Stop();
return data;
}
void pca_setfreq(float freq)
{
u8 prescale,oldmode,newmode;
double prescaleval;
freq *= 0.92;
prescaleval = 25000000;
prescaleval /= 4096;
prescaleval /= freq;
prescaleval -= 1;
prescale =floor(prescaleval + 0.5f);
oldmode = pca_read(pca_mode1);
newmode = (oldmode&0x7F) | 0x10;
pca_write(pca_mode1, newmode);
pca_write(pca_pre, prescale);
pca_write(pca_mode1, oldmode);
delay_ms(2);
pca_write(pca_mode1, oldmode | 0xa1);
}
void pca_setpwm(u8 num, u32 on, u32 off)
{
pca_write(LED0_ON_L+4*num,on);
pca_write(LED0_ON_H+4*num,on>>8);
pca_write(LED0_OFF_L+4*num,off);
pca_write(LED0_OFF_H+4*num,off>>8);
}
在实际使用的时候，需要初始化IIC通道：
delay_init();
IIC_Init();
pca_write(pca_mode1, 0x00);
pca_setfreq(50);
但现在，我们能做的也仅仅是通过main函数给驱动板发送一个角度信息，然后舵机做相关动作：
pca_setpwm(ptr->engine_index, 0, ptr->NowAngle);
这是不符合实际需求的，这时我想到了运用中断，在中断中完成从某个角度运动到另一个角度的过程，保证平稳的同时，方便调速，还不会在main函数中占用大量时间，可以让main函数去调配其他任务，首先初始化一个定时器：
void TIM5_PWM_Init(u16 arr, u16 psc){
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
NVIC_InitTypeDef NVIC_InitStructure;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5, ENABLE); //使能 TIM5 时钟
TIM_TimeBaseStructure.TIM_Period = arr; //设置在下一个更新事件装入活动的自动重装载寄存器周期的值
TIM_TimeBaseStructure.TIM_Prescaler =psc; //设置用来作为TIMx时钟频率除数的预分频值
TIM_TimeBaseStructure.TIM_ClockDivision = 0; //设置时钟分割:TDTS = Tck_tim
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; //TIM向上计数模式
TIM_TimeBaseInit(TIM5, &TIM_TimeBaseStructure); //根据TIM_TimeBaseInitStruct中指定的参数初始化TIMx的时间基数单位
TIM_ITConfig(TIM5, TIM_IT_Update, ENABLE);
TIM_Cmd(TIM5, ENABLE);
NVIC_InitStructure.NVIC_IRQChannel = TIM5_IRQn; //NVIC配置
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
}
使用更新溢出中断就足够了，然后我们需要定义一个结构体来储存舵机的各种信息：
typedef struct ControlNode{
u8 engine_index;
int step;
u16 NowAngle;
u16 FinalAngle;
struct ControlNode* next;
}ControlNode;
一个结构体对应一个舵机，包含了当前角度，转动步长，舵机编号，和目标角度。需要运行的舵机会新建一个对应拥有相应index的结构体，插入链表中：
extern ControlNode* ControlHead;
这是一个全局的指针，作为头指针被调用，用于检索所有需要运动的舵机，到达目标值之后，这个结构体会被清除：
void Sterengine_Scan(void){
ControlNode* ptr, *pre;
pre = ControlHead;
ptr = ControlHead->next;
while(ptr!=NULL){
if(ptr->NowAngle==ptr->FinalAngle){
StatusNode[ptr->engine_index].status = 0; //任务值空
pre->next = ptr->next;
free(ptr);
pre = pre;
ptr = pre->next;
continue;
} //清除结构体
ptr->NowAngle = ptr->NowAngle+ptr->step;
StatusNode[ptr->engine_index].NowAngle = ptr->NowAngle;
pca_setpwm(ptr->engine_index, 0, ptr->NowAngle);
pre = ptr;
ptr = ptr->next;
}
}
将上面这个函数放入定义好的中断服务中：
void TIM5_IRQHandler(void){
if(TIM_GetITStatus(TIM5, TIM_IT_Update) != RESET){
GetInf();
Sterengine_Scan(); //舵机任务执行
//Irsensor_Control();
//Control_FindSideWay();
TIM_ClearITPendingBit(TIM5, TIM_IT_Update);
}
}
最后，我们需要一个函数用于装载需要运动的结构体，为了方便管理，将index设置为宏定义，这个index同在驱动板上的插线有关，具体可以查相关技术手册：
#define BehindShoulder 0
#define BehindArm 1
#define BehindPaw 2
#define FrontShoulder 3
#define FrontArm 4
#define FrontPaw 5
#define RightPaw 6
#define LeftPaw 7
我们的机器人共使用8个舵机，将角度信息作为数组存储起来，应为实际上待命的角度一般只有两个：
static u16 OpenAngleBuf[9]={480, 280, 395, 435, 250, 320, 400, 400, 400};
static u16 CloseAngleBuf[9]={80, 105, 260, 200, 110, 180, 250, 250, 250};
最关键的来了，在插入函数中，通过指令决定相关动作，并设置成为宏定义：
#define Open -1
#define Close 1
#define toR -1
#define toM 0
#define toL 1
#define Up 1
#define Down -1
#define Open1 1
#define Close1 -1
其实归根结底不过是调用上面那个数组的两种顺序罢了。
void Sterengine_Control(u8 index, int commond){
u16 MidAngle;
ControlNode* ptr;
/*
0:Behindshoulder
1:BehindArm
2:BehindPaw
3:FrondShoulder
4:FrontArm
5:FrontPaw
6:RightPaw
7:LeftPaw
*/
Sterengine_Ready(index);
ptr = (ControlNode*)malloc(sizeof(ControlNode));
ptr->next = ControlHead->next;
ControlHead->next = ptr;
ptr->engine_index = index;
StatusNode[index].status = 1;
ptr->NowAngle = StatusNode[index].NowAngle;
//新建任务
if(index == 0||index==3){ //检索到云台
MidAngle = (OpenAngleBuf[index]+CloseAngleBuf[index])/2;
switch(commond){
case 0: ptr->FinalAngle = MidAngle; break;
case 1: ptr->FinalAngle = OpenAngleBuf[index]; break;
case -1: ptr->FinalAngle = CloseAngleBuf[index]; break;
}
}
else{
if(commond == 1){
ptr->FinalAngle = OpenAngleBuf[index];
}
else{
ptr->FinalAngle = CloseAngleBuf[index];
}
}
//设置步长正负值
if(ptr->FinalAngle-StatusNode[index].NowAngle>0){
ptr->step = 1;
}
else{
ptr->step = -1;
}
}
也许注意到了在语句执行之前还有一个函数，这个函数的目的是如果这个舵机正在执行，也就是结构体还在链表中，那么需要等待完成再将新的任务结构体插入链表中，这样就避免了未知的冲突：
void Sterengine_Ready(u8 index){
while(StatusNode[index].status!=0);
}
这里利用了一个新的结构体：
typedef struct MissionStatus{
u8 status;
u16 NowAngle;
}MissionStatus;
status为0表示空闲，1表示繁忙，当前角度是在中断中被写入的，其实可以通过IIC从pca9685上读入，但不是非常必要，但我们这样做的话，必须设置一个初始值，也就是一开始所有舵机角度需要到达我们预设的位置，相应的角度信息也必须写入内存，否则在执行的时候会带来混乱。
举个例子：
一开始认为的NowAngle是180，这是我们自己定义的，但舵机却未必是这样，当运行的时候，舵机可能收到一个和当前的物理角度相距甚远的指令，这种情况是要尽量避免的，所以有这样一个初始化函数：
void Grab_Init(void){
//所有舵机信息结构体赋初值，舵机就位
u8 index;
u16 temp;
if(ObjectSearchNode->mirror_direction == -1){
pca_setpwm(BehindShoulder, 0, OpenAngleBuf[BehindShoulder]);
StatusNode[BehindShoulder].NowAngle = OpenAngleBuf[BehindShoulder];
}
else if(ObjectSearchNode->mirror_direction == 1){
pca_setpwm(BehindShoulder, 0, CloseAngleBuf[BehindShoulder]);
StatusNode[BehindShoulder].NowAngle = CloseAngleBuf[BehindShoulder];
}
delay_ms(100);
temp = (OpenAngleBuf[FrontShoulder] + CloseAngleBuf[FrontShoulder])/2;
pca_setpwm(FrontShoulder, 0, temp);
StatusNode[FrontShoulder].NowAngle = temp;
delay_ms(100);
temp = OpenAngleBuf[FrontArm];
pca_setpwm(FrontArm, 0, temp);
StatusNode[FrontArm].NowAngle = temp;
delay_ms(100);
temp = OpenAngleBuf[FrontPaw];
pca_setpwm(FrontPaw, 0, temp);
StatusNode[FrontPaw].NowAngle = temp;
delay_ms(100);
temp = CloseAngleBuf[BehindArm];
pca_setpwm(BehindArm, 0, temp);
StatusNode[BehindArm].NowAngle = temp;
delay_ms(100);
temp = CloseAngleBuf[BehindPaw];
pca_setpwm(BehindPaw, 0, temp);
StatusNode[BehindPaw].NowAngle = temp;
delay_ms(100);
temp = CloseAngleBuf[RightPaw];
pca_setpwm(RightPaw, 0, temp);
StatusNode[RightPaw].NowAngle = temp;
delay_ms(100);
temp = CloseAngleBuf[LeftPaw];
pca_setpwm(LeftPaw, 0, temp);
StatusNode[LeftPaw].NowAngle = temp;
delay_ms(100);
for(index=0; index<8; index++){
StatusNode[index].status = 0;
}
}
这样，舵机就可以非常方便的发送指令使之完成相关动作了：
void FrontPlace(void){
if(ObjectSearchNode->mirror_direction == 1){
Sterengine_Control(FrontShoulder, toL);
}
else if(ObjectSearchNode->mirror_direction == -1){
Sterengine_Control(FrontShoulder, toR);
}
delay_ms(300);
Sterengine_Control(FrontArm, Down);
Sterengine_Control(FrontPaw, Open);
}
这是一个实际使用例子。
861
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
