康复手系统控制
目录
1控制流程
(1)5路PWM输出同时驱动5路直线电机
(2)实时采集5路电机位置信号,5路连杆弹簧位置信号,电压差值作为反馈构成位置闭环控制系统
(3)与上位机通过串口实时通信,获取上位机发送的控制指令,同时向上位机发送康复手位置状态
2STM32F407源码
main.c
#include "sys.h"
#include "delay.h"
#include "usart.h"
#include "led.h"
#include "pwm.h"
#include "adc.h"
int main(void)
{
u16 times=0;
u16 L=200;
u16 adc_M1,adc_M2,adc_M3,adc_M4,adc_M5; //反馈电机位置得到的电压值,(二进制,0-4096)
float t_M1=0,t_M2=0,t_M3=0,t_M4=0,t_M5=0; //反馈电机位置得到的电压值,(浮点数,0-3.3V)
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); //设置系统中断优先级分组2
delay_init(168); //初始化延时函数
uart_init(115200); //初始化串口波特率为115200
TIM3_PWM_Init(1000-1,84-1); //84M/84=1Mhz的计数频率,重装载值1000,所以PWM频率为 1M/1000=1Khz.
TIM4_PWM_Init(1000-1,84-1);
Adc1_Init();
Adc2_Init();
LED_Init();
while(1)
{
if(USART_RX_STA&0x8000)
{
switch(USART_RX_BUF[0])
{
//(1)单次被动训练动作
case '1':
{
printf("U send '1',grasp : \r\n");
TIM_SetCompare1(TIM3,L); // 0, 占空比100%,3.3V
TIM_SetCompare2(TIM3,L); // 1000,占空比 0, 0V
TIM_SetCompare3(TIM3,L);
TIM_SetCompare1(TIM4,L);
TIM_SetCompare2(TIM4,L);
break;
}
case '2':
{
printf("U send '2' open : \r\n");
TIM_SetCompare1(TIM3,1000);
TIM_SetCompare2(TIM3,1000);
TIM_SetCompare3(TIM3,1000);
TIM_SetCompare1(TIM4,1000);
TIM_SetCompare2(TIM4,1000);
break;
}
case '3':
{
printf("U send '3',realx : \r\n");
TIM_SetCompare1(TIM3,500);
TIM_SetCompare2(TIM3,500);
TIM_SetCompare3(TIM3,500);
TIM_SetCompare1(TIM4,500);
TIM_SetCompare2(TIM4,500);
break;
}
//(2)实现连续被动开闭手训练
case '7':
{
delay_ms(10);
printf("Send '7', Continuous passive training : \r\n");
TIM_SetCompare1(TIM3,L);
TIM_SetCompare2(TIM3,L);
TIM_SetCompare3(TIM3,L);
TIM_SetCompare1(TIM4,L);
TIM_SetCompare2(TIM4,L);
delay_ms(4000);
//获取电机的反馈电压值
adc_M1=Get_Adc_Average(ADC_Channel_0,20); t_M1=(float)adc_M1*(3.3/4096);
adc_M2=Get_Adc_Average(ADC_Channel_1,20); t_M2=(float)adc_M2*(3.3/4096);
adc_M3=Get_Adc_Average(ADC_Channel_2,20); t_M3=(float)adc_M3*(3.3/4096);
adc_M4=Get_Adc_Average(ADC_Channel_3,20); t_M4=(float)adc_M4*(3.3/4096);
adc_M5=Get_Adc_Average(ADC_Channel_4,20); t_M5=(float)adc_M5*(3.3/4096);
printf("%s%f%s%f%s%f%s%f%s%f%s%s","Feedback Voltage ",t_M1,"V ",t_M2,"V ",t_M3,"V ",t_M4,"V ",t_M5,"V ","\r\n\r\n");
delay_ms(1000);
TIM_SetCompare1(TIM3,1000);
TIM_SetCompare2(TIM3,1000);
TIM_SetCompare3(TIM3,1000);
TIM_SetCompare1(TIM4,1000);
TIM_SetCompare2(TIM4,1000);
delay_ms(4000);
//获取电机的反馈电压值
adc_M1=Get_Adc_Average(ADC_Channel_0,20); t_M1=(float)adc_M1*(3.3/4096);
adc_M2=Get_Adc_Average(ADC_Channel_1,20); t_M2=(float)adc_M2*(3.3/4096);
adc_M3=Get_Adc_Average(ADC_Channel_2,20); t_M3=(float)adc_M3*(3.3/4096);
adc_M4=Get_Adc_Average(ADC_Channel_3,20); t_M4=(float)adc_M4*(3.3/4096);
adc_M5=Get_Adc_Average(ADC_Channel_4,20); t_M5=(float)adc_M5*(3.3/4096);
printf("%s%f%s%f%s%f%s%f%s%f%s%s","Feedback Voltage ",t_M1,"V ",t_M2,"V ",t_M3,"V ",t_M4,"V ",t_M5,"V ","\r\n\r\n");
delay_ms(1000);
break;
}
//(3)实现主动 闭手训练
case 'a':
{
printf("Send 'a', active fist training : \r\n");
TIM_SetCompare1(TIM3,L);
TIM_SetCompare2(TIM3,L);
TIM_SetCompare3(TIM3,L);
TIM_SetCompare1(TIM4,L);
TIM_SetCompare2(TIM4,L);
delay_ms(5000);
printf("auto open\r\n");
TIM_SetCompare1(TIM3,1000);
TIM_SetCompare2(TIM3,1000);
TIM_SetCompare3(TIM3,1000);
TIM_SetCompare1(TIM4,1000);
TIM_SetCompare2(TIM4,1000);
delay_ms(5000);
break;
}
//(4)实现主动 开手训练
case 'b':
{
printf("Send 'b', active open training : \r\n");
TIM_SetCompare1(TIM3,1000);
TIM_SetCompare2(TIM3,1000);
TIM_SetCompare3(TIM3,1000);
TIM_SetCompare1(TIM4,1000);
TIM_SetCompare2(TIM4,1000);
delay_ms(5000);
printf("auto grasp\r\n");
TIM_SetCompare1(TIM3,L);
TIM_SetCompare2(TIM3,L);
TIM_SetCompare3(TIM3,L);
TIM_SetCompare1(TIM4,L);
TIM_SetCompare2(TIM4,L);
delay_ms(5000);
break;
}
}
USART_RX_STA=0;
}
else //没有发送数据
{
times++;
if(times%500==0) printf("Please send a character.\r\n");
if(times%50==0) LED0=!LED0; //闪烁LED,提示系统正在运行
delay_ms(10);
}
}
}
adc.h
#ifndef __ADC_H
#define __ADC_H
#include "sys.h"
void Adc1_Init(void); //ADC通道初始化
void Adc2_Init(void); //ADC通道初始化
u16 Get_Adc(u8 ch); //获得某个通道值
u16 Get_Adc_Average(u8 ch,u8 times); //得到某个通道给定次数采样的平均值
#endif
adc.c
#include "adc.h"
#include "delay.h"
/*
ADC12_IN0 反馈电机1 的位置信号
ADC12_IN1 反馈电机2 的位置信号
ADC12_IN2 反馈电机3 的位置信号
ADC12_IN3 反馈电机4 的位置信号
ADC12_IN4 反馈电机5 的位置信号
ADC12_IN5 反馈弹簧1 的位置信号
ADC12_IN6 反馈弹簧2 的位置信号
ADC12_IN7 反馈弹簧3 的位置信号
ADC12_IN10 反馈弹簧4 的位置信号
ADC12_IN11 反馈弹簧5 的位置信号
*/
//初始化ADC
void Adc1_Init(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
ADC_CommonInitTypeDef ADC_CommonInitStructure;
ADC_InitTypeDef ADC_InitStructure;
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE); //使能GPIOA时钟
RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1, ENABLE); //使能ADC1时钟
//先初始化ADC1通道0/1/2/3/4/5/6/7 IO口
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0|GPIO_Pin_1|GPIO_Pin_2|GPIO_Pin_3|GPIO_Pin_4|GPIO_Pin_5|GPIO_Pin_6|GPIO_Pin_7; //PA01234567 ADC通道01234567
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AN; //模拟输入
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL ; //不带上下拉
GPIO_Init(GPIOA, &GPIO_InitStructure); //初始化
RCC_APB2PeriphResetCmd(RCC_APB2Periph_ADC1,ENABLE); //ADC1复位
RCC_APB2PeriphResetCmd(RCC_APB2Periph_ADC1,DISABLE); //复位结束
ADC_CommonInitStructure.ADC_Mode = ADC_Mode_Independent; //独立模式
ADC_CommonInitStructure.ADC_TwoSamplingDelay = ADC_TwoSamplingDelay_5Cycles; //两个采样阶段之间的延迟5个时钟
ADC_CommonInitStructure.ADC_DMAAccessMode = ADC_DMAAccessMode_Disabled; //DMA失能,不使用DMA模式
ADC_CommonInitStructure.ADC_Prescaler = ADC_Prescaler_Div4; //预分频4分频。ADCCLK=PCLK2/4=84/4=21Mhz,ADC时钟最好不要超过36Mhz
ADC_CommonInit(&ADC_CommonInitStructure); //初始化
ADC_InitStructure.ADC_Resolution = ADC_Resolution_12b; //12位模式
ADC_InitStructure.ADC_ScanConvMode = DISABLE; //非扫描模式
ADC_InitStructure.ADC_ContinuousConvMode = DISABLE; //关闭连续转换
ADC_InitStructure.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_None; //禁止触发检测,使用软件触发
ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right; //右对齐
ADC_InitStructure.ADC_NbrOfConversion = 1; //1个转换在规则序列中 也就是只转换规则序列1
ADC_Init(ADC1, &ADC_InitStructure); //ADC初始化
ADC_Cmd(ADC1, ENABLE); //开启AD转换器
}
void Adc2_Init(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
ADC_CommonInitTypeDef ADC_CommonInitStructure;
ADC_InitTypeDef ADC_InitStructure;
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE); //使能GPIOC时钟
RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1, ENABLE); //使能ADC1时钟
//先初始化ADC1通道10/11 IO口
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0|GPIO_Pin_1; //PC0/1 通道10/11
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AN; //模拟输入
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL ; //不带上下拉
GPIO_Init(GPIOC, &GPIO_InitStructure); //初始化
RCC_APB2PeriphResetCmd(RCC_APB2Periph_ADC1,ENABLE); //ADC1复位
RCC_APB2PeriphResetCmd(RCC_APB2Periph_ADC1,DISABLE); //复位结束
ADC_CommonInitStructure.ADC_Mode = ADC_Mode_Independent; //独立模式
ADC_CommonInitStructure.ADC_TwoSamplingDelay = ADC_TwoSamplingDelay_5Cycles; //两个采样阶段之间的延迟5个时钟
ADC_CommonInitStructure.ADC_DMAAccessMode = ADC_DMAAccessMode_Disabled; //DMA失能,不使用DMA模式
ADC_CommonInitStructure.ADC_Prescaler = ADC_Prescaler_Div4; //预分频4分频。ADCCLK=PCLK2/4=84/4=21Mhz,ADC时钟最好不要超过36Mhz
ADC_CommonInit(&ADC_CommonInitStructure); //初始化
ADC_InitStructure.ADC_Resolution = ADC_Resolution_12b; //12位模式
ADC_InitStructure.ADC_ScanConvMode = DISABLE; //非扫描模式
ADC_InitStructure.ADC_ContinuousConvMode = DISABLE; //关闭连续转换
ADC_InitStructure.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_None; //禁止触发检测,使用软件触发
ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right; //右对齐
ADC_InitStructure.ADC_NbrOfConversion = 1; //1个转换在规则序列中 也就是只转换规则序列1
ADC_Init(ADC1, &ADC_InitStructure); //ADC初始化
ADC_Cmd(ADC1, ENABLE); //开启AD转换器
}
//获得ADC值
//ch: @ref ADC_channels
//通道值 0~16取值范围为:ADC_Channel_0~ADC_Channel_16
//返回值:转换结果
u16 Get_Adc(u8 ch)
{
//设置指定ADC的规则组通道,一个序列,采样时间
ADC_RegularChannelConfig(ADC1, ch, 1, ADC_SampleTime_480Cycles ); //ADC1,ADC通道,480个周期,提高采样时间可以提高精确度
// ADC 转换时间为 采样时间 + 12个周期
ADC_SoftwareStartConv(ADC1); //使能指定的ADC1的软件转换启动功能
while(!ADC_GetFlagStatus(ADC1, ADC_FLAG_EOC ));//等待转换结束
return ADC_GetConversionValue(ADC1); //返回最近一次ADC1规则组的转换结果
}
//获取通道ch的转换值,取times次,然后平均
//ch:通道编号
//times:获取次数
//返回值:通道ch的times次转换结果平均值
u16 Get_Adc_Average(u8 ch,u8 times)
{
u32 temp_val=0;
u8 t;
for(t=0;t<times;t++)
{
temp_val+=Get_Adc(ch);
delay_ms(5);
}
return temp_val/times;
}
pwc.h
#ifndef __PWM_H
#define __PWM_H
#include "sys.h"
void TIM3_PWM_Init(u32 arr,u32 psc);
void TIM4_PWM_Init(u32 arr,u32 psc);
#endif
pwc.c
#include "pwm.h"
#include "led.h"
#include "usart.h"
/*
TIM3_CH1 控制电机1
TIM3_CH2 控制电机2
TIM3_CH3 控制电机3
TIM4_CH1 控制电机4
TIM4_CH2 控制电机5
*/
//TIM3 PWM部分初始化
//PWM输出初始化
//arr:自动重装值
//psc:时钟预分频数
void TIM3_PWM_Init(u32 arr,u32 psc)
{
//此部分需手动修改IO口设置
GPIO_InitTypeDef GPIO_InitStructure;
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_OCInitTypeDef TIM_OCInitStructure;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3,ENABLE); //TIM3时钟使能
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE); //使能PORTC时钟
GPIO_PinAFConfig(GPIOC,GPIO_PinSource6,GPIO_AF_TIM3);
GPIO_PinAFConfig(GPIOC,GPIO_PinSource7,GPIO_AF_TIM3);
GPIO_PinAFConfig(GPIOC,GPIO_PinSource8,GPIO_AF_TIM3); //GPIOC6/7/8复用为定时器3
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7|GPIO_Pin_8; //GPIOC6/7/8
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; //复用功能
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz; //速度100MHz
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; //推挽复用输出
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; //上拉
GPIO_Init(GPIOC,&GPIO_InitStructure); //初始化PC6/7/8
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(TIM3,&TIM_TimeBaseStructure); //初始化定时器3
//初始化TIM3 Channel PWM模式
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; //选择定时器模式:TIM脉冲宽度调制模式2
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; //比较输出使能
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_Low; //输出极性:TIM输出比较极性低
TIM_OC1Init(TIM3, &TIM_OCInitStructure); //根据T指定的参数初始化外设TIM1 4OC1
TIM_OC1PreloadConfig(TIM3, TIM_OCPreload_Enable); //使能TIM3在CCR1上的预装载寄存器
TIM_OC2Init(TIM3, &TIM_OCInitStructure); //根据T指定的参数初始化外设TIM1 4OC1
TIM_OC2PreloadConfig(TIM3, TIM_OCPreload_Enable); //使能TIM3在CCR2上的预装载寄存器
TIM_OC3Init(TIM3, &TIM_OCInitStructure); //根据T指定的参数初始化外设TIM1 4OC1
TIM_OC3PreloadConfig(TIM3, TIM_OCPreload_Enable); //使能TIM3在CCR3上的预装载寄存器
TIM_ARRPreloadConfig(TIM3,ENABLE); //ARPE使能
TIM_Cmd(TIM3, ENABLE); //使能TIM3
}
//TIM4 PWM部分初始化
//PWM输出初始化
//arr:自动重装值
//psc:时钟预分频数
void TIM4_PWM_Init(u32 arr,u32 psc)
{
//此部分需手动修改IO口设置
GPIO_InitTypeDef GPIO_InitStructure;
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_OCInitTypeDef TIM_OCInitStructure;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4,ENABLE); //TIM4时钟使能
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE); //使能PORTB时钟
GPIO_PinAFConfig(GPIOB,GPIO_PinSource6,GPIO_AF_TIM4);
GPIO_PinAFConfig(GPIOB,GPIO_PinSource7,GPIO_AF_TIM4); //GPIOB6/7复用为定时器4
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6|GPIO_Pin_7; //GPIOB6/7
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; //复用功能
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz; //速度100MHz
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; //推挽复用输出
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; //上拉
GPIO_Init(GPIOB,&GPIO_InitStructure); //初始化PB6/7
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(TIM4,&TIM_TimeBaseStructure); //初始化定时器4
//初始化TIM4 Channel PWM模式
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; //选择定时器模式:TIM脉冲宽度调制模式2
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; //比较输出使能
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_Low; //输出极性:TIM输出比较极性低
TIM_OC1Init(TIM4, &TIM_OCInitStructure); //根据T指定的参数初始化外设TIM1 4OC1
TIM_OC1PreloadConfig(TIM4, TIM_OCPreload_Enable); //使能TIM4在CCR1上的预装载寄存器
TIM_OC2Init(TIM4, &TIM_OCInitStructure); //根据T指定的参数初始化外设TIM1 4OC1
TIM_OC2PreloadConfig(TIM4, TIM_OCPreload_Enable); //使能TIM4在CCR2上的预装载寄存器
TIM_ARRPreloadConfig(TIM4,ENABLE); //ARPE使能
TIM_Cmd(TIM4, ENABLE); //使能TIM4
}