题目背景:
涉及一款加持力保持不变的机械手,有参数要求,这里只展示原理。
解决方案:
采用离传感器和PID算法,单片机为STM32F4系列,力传感器采用应变式,电机采用无刷电机加减速器。控制效果好。机械上采用滚珠丝杠传动,非标零件3D打印。
传感器做了卡尔曼滤波,不过参数调不好,运算速度还慢。
图片效果:
实物图片丢失,只剩模型了,模型中只有一个机械手,另一个对称,没有画出。
代码:
需要引入arm的数学库,想加速运算,还可以开启浮点运算单元。
主函数
初始化一些外设,按键、电机、压力传感器。
#include "stm32f4xx.h"
#include "usart.h"
#include "delay.h"
#include "key.h"
#include "motor.h"
#include "sensor.h"
#include "timer.h"
#include "arm_math.h"
u16 EXPECT=2000;
extern float32_t xkh;
int main(void)
{
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);//设置系统中断优先级分组2
uart_init(115200);
delay_init(168);
printf("kaishi\r\n");
motorinit();
Init_HX711pin();
delay_ms(500);
Get_Maopi();
manualoperation_init(); //手动操作相关初始化
automode_init();
// KEY_Init(); //忘了啥程序了 好像没用
// motorstart(); //这是回零程序
// limitswitch_init();
TIM3_Int_Init(1000-1,8400-1); //定时器时钟84M,分频系数8400,所以84M/8400=10Khz的计数频率,计1000次为100ms
printf("kai123shi\r\n");
while(1)
{
// Get_Weight();
// motorpid();
printf("F=%d,PWM=%d,E=%d\r\n",zout,pwmout,iError);
delay_ms(10);
}
}
按键的初始化
设置了四个按键,分别控制:加紧、松开、紧急停止、自动加紧。
key.c
#include "key.h"
#include "delay.h"
#include "motor.h"
#include "timer.h"
#include "sensor.h"
#include "usart.h"
//按键初始化函数
void KEY_Init(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOE, ENABLE);//使能GPIOA,GPIOE时钟
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2|GPIO_Pin_3|GPIO_Pin_4; //KEY0 KEY1 KEY2对应引脚
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;//普通输入模式
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;//100M
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;//上拉
GPIO_Init(GPIOE, &GPIO_InitStructure);//初始化GPIOE2,3,4
}
void manualoperation_init(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
EXTI_InitTypeDef EXTI_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE);//使能SYSCFG时钟
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);//使能GPIOA,GPIOE时钟
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE);//使能GPIOA,GPIOE时钟
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3|GPIO_Pin_2;//对应引脚PA0~2
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;//普通输入模式
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;//100M
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_DOWN ;//下拉 (不一定 要看买回来的按键模块)
GPIO_Init(GPIOA, &GPIO_InitStructure); //初始化GPIOA0
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1;//对应引脚PA0~2
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;//普通输入模式
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;//100M
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_DOWN ;//下拉 (不一定 要看买回来的按键模块)
GPIO_Init(GPIOC, &GPIO_InitStructure); //初始化GPIOA0
SYSCFG_EXTILineConfig(EXTI_PortSourceGPIOA, EXTI_PinSource2);//PA3 连接到中断线3
SYSCFG_EXTILineConfig(EXTI_PortSourceGPIOC, EXTI_PinSource1);//PA1 连接到中断线4
SYSCFG_EXTILineConfig(EXTI_PortSourceGPIOA, EXTI_PinSource3);//PA0 连接到中断线0
/* 配置EXTI_Line0 */
EXTI_InitStructure.EXTI_Line = EXTI_Line3|EXTI_Line1|EXTI_Line2 ;//LINE0~2
EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;//中断事件
EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Rising; //上升沿触发
EXTI_InitStructure.EXTI_LineCmd = ENABLE;//使能LINE0
EXTI_Init(&EXTI_InitStructure);//配置
NVIC_InitStructure.NVIC_IRQChannel = EXTI3_IRQn;//外部中断3 急停
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x00;//抢占优先级0
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x00;//子优先级0
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;//使能外部中断通道
NVIC_Init(&NVIC_InitStructure);//配置
NVIC_InitStructure.NVIC_IRQChannel = EXTI1_IRQn;//外部中断1
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x01;//抢占优先级1
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x00;//子优先级2
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;//使能外部中断通道
NVIC_Init(&NVIC_InitStructure);//配置
NVIC_InitStructure.NVIC_IRQChannel = EXTI2_IRQn;//外部中断2
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x01;//抢占优先级2
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x00;//子优先级0
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;//使能外部中断通道
NVIC_Init(&NVIC_InitStructure);//配置
}
//外部中断3服务程序,,停车
void EXTI3_IRQHandler(void)
{
delay_ms(10); //消抖
if(STOPKEY==1)
{
printf("ting\r\n");
TIM_Cmd(TIM3,DISABLE);
TIM_SetCompare1(TIM14,1); //停车
}
EXTI_ClearITPendingBit(EXTI_Line3); //清除LINE3上的中断标志位
}
//中断1 手动前进程序,
void EXTI1_IRQHandler(void)
{
delay_ms(10); //消抖
if(FOEWARDKEY==1)
{
// if(KEYFORWARD==1)
// {
printf("qian\r\n");
TIM_Cmd(TIM3,DISABLE);
MOTORDIRECTION=FORWARD;
TIM_SetCompare1(TIM14,arr); //
// }
}
EXTI_ClearITPendingBit(EXTI_Line1); //清的中断标志位
}
//中断2 手动后退程序
void EXTI2_IRQHandler(void)
{
delay_ms(10); //消抖
if(BACKWARDKEY==1)
{
// if(KEYBACK==1)
// {
printf("hou\r\n");
TIM_Cmd(TIM3,DISABLE);
MOTORDIRECTION=BACKWORD;
TIM_SetCompare1(TIM14,arr);
// }
}
EXTI_ClearITPendingBit(EXTI_Line2); //清诚的中断标志位
}
void limitswitch_init(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
EXTI_InitTypeDef EXTI_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE);//使能SYSCFG时钟
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOE, ENABLE);//使能,GPIOE时钟
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5|GPIO_Pin_6;//对应引脚Pe3~4
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;//普通输入模式
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;//100M
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP ;//上拉 (不一定 要看买回来的按键模块)
GPIO_Init(GPIOE, &GPIO_InitStructure); //初始化GPIOe0
//
// GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4;//对应引脚Pe3~4
// GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;//普通输入模式
// GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;//100M
// GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP ;//下拉 (不一定 要看买回来的按键模块)
// GPIO_Init(GPIOC, &GPIO_InitStructure); //初始化GPIOe0
SYSCFG_EXTILineConfig(EXTI_PortSourceGPIOE, EXTI_PinSource6);//PE4 连接到中断线4
SYSCFG_EXTILineConfig(EXTI_PortSourceGPIOE, EXTI_PinSource5);//PE5 连接到中断5
EXTI_InitStructure.EXTI_Line = EXTI_Line6|EXTI_Line5;//LINE
EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;//中断事件
EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Falling; //上升沿触发
EXTI_InitStructure.EXTI_LineCmd = DISABLE;//使能LINE
EXTI_Init(&EXTI_InitStructure);//配置
// NVIC_InitStructure.NVIC_IRQChannel = EXTI4_IRQn;//外部中断
// NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x00;//抢占优先级0
// NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x00;//子优先级0
// NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;//使能外部中断通道
// NVIC_Init(&NVIC_InitStructure);//配置
//
NVIC_InitStructure.NVIC_IRQChannel =EXTI9_5_IRQn;//外部中断
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x01;//抢占优先级0
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x00;//子优先级0
NVIC_InitStructure.NVIC_IRQChannelCmd = DISABLE;//使能外部中断通道
NVIC_Init(&NVIC_InitStructure);//配置
}
// 已经回到零位停车程序
void EXTI4_IRQHandler(void)
{
delay_ms(10); //消抖
printf("hLL\r\n");
if(KEYFORWARD==1)
{
TIM_Cmd(TIM3,DISABLE);
TIM_SetCompare1(TIM14,1); //停车
}
EXTI_ClearITPendingBit(EXTI_Line4); //清诚的中断标志位
}
//走到最前方了停车程序,
void EXTI9_5_IRQHandler(void)
{
delay_ms(10); //消抖
if(KEYBACK==0||KEYFORWARD==0)
{
TIM_Cmd(TIM3,DISABLE);
TIM_SetCompare1(TIM14,1); //停车
}
EXTI_ClearITPendingBit(EXTI_Line5); //清诚的中断标志位
EXTI_ClearITPendingBit(EXTI_Line6); //清诚的中断标志位
}
void automode_init(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
NVIC_InitTypeDef NVIC_InitStructure;
EXTI_InitTypeDef EXTI_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE);//使能SYSCFG时钟
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOE, ENABLE);//使能GPIOA,GPIOE时钟
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0;//对应引脚Pe3~4
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;//普通输入模式
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;//100M
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_DOWN ;//下拉 (不一定 要看买回来的按键模块)
GPIO_Init(GPIOE, &GPIO_InitStructure); //初始化GPIOA0
SYSCFG_EXTILineConfig(EXTI_PortSourceGPIOE, EXTI_PinSource0);//PE4 连接到中断线4
EXTI_InitStructure.EXTI_Line = EXTI_Line0;//LINE
EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;//中断事件
EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Rising; //上升沿触发
EXTI_InitStructure.EXTI_LineCmd = ENABLE;//使能LINE
EXTI_Init(&EXTI_InitStructure);//配置
NVIC_InitStructure.NVIC_IRQChannel = EXTI0_IRQn;//外部中断
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x02;//抢占优先级0
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x02;//子优先级0
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;//使能外部中断通道
NVIC_Init(&NVIC_InitStructure);//配置
}
extern u8 timerstate;
//自动程序
void EXTI0_IRQHandler(void)
{
delay_ms(10); //消抖
if(AUTOSTARTKEY==1)
{
EXTI_ClearITPendingBit(EXTI_Line0); //清诚的中断标志位
// timerstate=0;
// printf("11");
TIM_Cmd(TIM3,ENABLE);
}
}
void Reciprocatingmotion(void)
{
u16 i;
if(KEYFORWARD==0)
{
printf("11");
for(i=arr;i>1;i--)
{
TIM_SetCompare1(TIM14,i);
delay_ms(3);
}
MOTORDIRECTION=BACKWORD;
delay_ms(1000);
for(i=1;i<arr;i++)
{
TIM_SetCompare1(TIM14,i);
delay_ms(3);
} //
delay_ms(1000);
}
if(KEYBACK==0)
{
printf("22");
for(i=arr;i>1;i--)
{
TIM_SetCompare1(TIM14,i);
delay_ms(3);
}
MOTORDIRECTION=FORWARD;
delay_ms(1000);
for(i=1;i<arr;i++)
{
TIM_SetCompare1(TIM14,i);
delay_ms(3);
}
delay_ms(1000);
}
}
key.h
#ifndef __KEY_H
#define __KEY_H
#include "sys.h"
#include "sensor.h"
#define KEYFORWARD PEin(5) //PE4 回零接近开关
#define KEYBACK PEin(6) //PE3
#define KEY2 PEin(2) //P32
#define AUTOSTARTKEY PEin(0) //启动自动程序按键,是PE0,
#define STOPKEY PAin(3) //pa3作为急停按钮
#define FOEWARDKEY PCin(1) //pa1作为前进按钮
#define BACKWARDKEY PAin(2) //PA2作为后退按钮
void KEY_Init(void); //IO初始化
u8 KEY_Scan(u8); //按键扫描函数
void manualoperation_init(void);
void limitswitch_init(void);
void automode_init(void);
extern void Get_Weight(void);
#endif
电机
电机采用PWM控制,PWM对应着电机电流
motor.h
#ifndef __MOTOR_H
#define __MOTOR_H
#include "sys.h"
#define BACKNOTOVER 1 //格根据限位开关决定01
#define MOTORDIRECTION PFout(10) // DS1
#define FORWARD 0
#define BACKWORD 1
定时器时钟是84mhz
#define psc 20 //分频系数7-1
#define arr 399 //计数初值1200-1
#define KP 5
#define KI 2
#define KD 0
void motorinit(void);
void motorstart(void);
void motorpid(void);
#endif
motor.c
#include "delay.h"
#include "motor.h"
#include "sys.h"
#include "key.h"
#include "usart.h"
#include "sensor.h"
#include "arm_math.h"
u16 pwmval=1;//pwm比较值最大是arr,有效是119-1199
void motorinit(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_OCInitTypeDef TIM_OCInitStructure;
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOF, ENABLE);
//GPIOF9,MOTORDIRECTION
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9 | GPIO_Pin_10;//
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;//
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;//推挽
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;//100MHz
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;//
GPIO_Init(GPIOF, &GPIO_InitStructure);GPIOF10,MOTORDIRECTION
///MOTORSPEED pwm得设置//
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM14,ENABLE); //
GPIO_PinAFConfig(GPIOF,GPIO_PinSource9,GPIO_AF_TIM14); //端口复用
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9; //
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; //服用功能
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz; //
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; //推挽复用输出
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; //上拉
GPIO_Init(GPIOF,&GPIO_InitStructure); //???PF9
TIM_TimeBaseStructure.TIM_Prescaler=psc; //
TIM_TimeBaseStructure.TIM_CounterMode=TIM_CounterMode_Up; //向上计数模式
TIM_TimeBaseStructure.TIM_Period=arr; //
TIM_TimeBaseStructure.TIM_ClockDivision=TIM_CKD_DIV1;
TIM_TimeBaseInit(TIM14,&TIM_TimeBaseStructure);//
//
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; //
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; //
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High; //输出极性是:急性第
TIM_OC1Init(TIM14, &TIM_OCInitStructure); /、
TIM_OC1PreloadConfig(TIM14, TIM_OCPreload_Enable); //
TIM_ARRPreloadConfig(TIM14,ENABLE);//
TIM_Cmd(TIM14, ENABLE); //
TIM_SetCompare1(TIM14,pwmval);// 修改占空比比较值
}
void motorstart(void)
{
//回零程序
while(KEYBACK==BACKNOTOVER) //回零程序
{
MOTORDIRECTION = FORWARD;
TIM_SetCompare1(TIM14,300);
}
TIM_SetCompare1(TIM14,1); //停车
}
struct PID
{
s32 setpoint;//设定目标
s32 sum_error;//误差累计
s32 proportion;//比例常数
s32 integral;//积分常数
s32 derivative;//微分常数
s32 last_error;//e[-1]
s32 prev_error;//e[-2]
}PID1={900,0,KP,KI,KD,0,0};
extern float32_t xkh;
int pwm;
int pwmout;
s32 iError;
void motorpid(void)
{
s32 now;
/*
pwm的范围适50-500数值,要注意各个量的转化
*/
now=(s32)(zout*0.01); //传感器50以内的返回值都是杂波
iError=PID1.setpoint-now; //当前误差
PID1.sum_error+=iError;
if(PID1.sum_error>150)PID1.sum_error=150;
if(PID1.sum_error<-150)PID1.sum_error=-150; //积分项限制
pwm =(int) (PID1.proportion * iError + PID1.integral * PID1.sum_error + PID1.derivative * (PID1.last_error-iError));
PID1.prev_error=PID1.last_error; //存储误差,便于下次计算
PID1.last_error=iError;
pwm =pwm*0.06;
pwmout=pwm;
if(pwm>0)
{
MOTORDIRECTION = FORWARD;
pwm+=40;
}
if(pwm<0)
{
MOTORDIRECTION=BACKWORD;
pwm=(-pwm)+40;
}
if(pwm>250) pwm=250;
TIM_SetCompare1(TIM14,pwm);
}
力传感器
直接输出的是电压信号,一般都外接模块转成数字信号,其读数并非什么通信协议,逻辑参见淘宝。
本人尝试引入arm数学库,开启浮点运算单元,加速运算。
sensor.h
#ifndef __HX711_H
#define __HX711_H
#include "sys.h"
#define HX711_SCK PBout(0)// PB0
#define HX711_DOUT PBin(1)// PB1
extern void Init_HX711pin(void);
extern u32 HX711_Read(void);
extern s32 my_HX711_Read(u8);
void Get_Weight(void);
void Get_Maopi(void);
extern s32 HX711_Buffer;
extern s32 Weight_Maopi[4];
extern s32 Weight_Shiwu;
extern u8 Flag_Error;
#endif
sensor.c
#ifndef __HX711_H
#define __HX711_H
#include "sys.h"
#define HX711_SCK PBout(0)// PB0
#define HX711_DOUT PBin(1)// PB1
extern void Init_HX711pin(void);
extern u32 HX711_Read(void);
extern s32 my_HX711_Read(u8);
void Get_Weight(void);
void Get_Maopi(void);
extern s32 HX711_Buffer;
extern s32 Weight_Maopi[4];
extern s32 Weight_Shiwu;
extern u8 Flag_Error;
#endif
定时器
控制PID周期,略。
说明手稿:
全部资料下载:
https://download.youkuaiyun.com/download/renzemingcsdn/16387516