├── FORCE_CLAW ├── force_claw.h └── force_claw.c ├── readme.txt ├── ADC ├── adc.h └── adc.c ├── DAC ├── dac.h └── dac.c ├── STEP_MOTOR ├── step_motor.h └── step_motor.c ├── SERVO_CTRL ├── servo_ctrl.h └── servo_ctrl.c ├── VOICE_MOTOR_DRIVE ├── voice_drive.h └── voice_drive.c ├── ENCODER ├── encoder.h └── encoder.c ├── TIMER ├── timer.h └── timer.c ├── PID_CONTROLLER ├── pid_controller.h └── pid_controller.c └── main.c /FORCE_CLAW/force_claw.h: -------------------------------------------------------------------------------- 1 | #ifndef __FORCE_CLAW_H 2 | #define __FORCE_CLAW_H 3 | 4 | void forceCtrl(); 5 | #endif 6 | -------------------------------------------------------------------------------- /readme.txt: -------------------------------------------------------------------------------- 1 | 项目简介： 2 | 此项目为力反馈机械爪，由音圈电机、步进电机组成。其中，步进电机作为机械爪，机械爪末端装有力传感器， 3 | 音圈电机本身是个伺服系统，在此作为控制器，将位置作为步进电机目标值，控制机械爪抓取，机械爪的力传感器 4 | 反馈压力值，转换为电流值叠加到音圈电机上，所以人在控制机械爪抓取时能感受到抓取力，实现力交互控制。音 5 | 圈电机位置环参数都设为零，可认为工作在速度模式，改变速度环比例值，能改变阻尼值可优化控制效果 -------------------------------------------------------------------------------- /ADC/adc.h: -------------------------------------------------------------------------------- 1 | #ifndef __ADC_H 2 | #define __ADC_H 3 | #include "sys.h" 4 | 5 | 6 | void Adc_Init(void); //ADC通道初始化 7 | u16 Get_Adc(u8 ch); //获得某个通道值 8 | 9 | #endif 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /DAC/dac.h: -------------------------------------------------------------------------------- 1 | #ifndef __DAC_H 2 | #define __DAC_H 3 | #include "sys.h" 4 | 5 | void Dac1_Init(void); //DAC通道1初始化 6 | void Dac_Set_Vol(uint8_t chan, float vol); //设置通道1输出电压 7 | #endif 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /STEP_MOTOR/step_motor.h: -------------------------------------------------------------------------------- 1 | #ifndef __STEP_MOTOR_H 2 | #define __STEP_MOTOR_H 3 | #include "sys.h" 4 | 5 | //motor ctrl端口定义 6 | 7 | #define DrY PDout(1) 8 | #define StY PDout(3) 9 | 10 | void stepMotorIO_Init(void);//初始化 11 | void stepMotorSetGoalPosition(int pos); 12 | void stepMotorCtrl(); 13 | 14 | #endif 15 | -------------------------------------------------------------------------------- /SERVO_CTRL/servo_ctrl.h: -------------------------------------------------------------------------------- 1 | #ifndef SERVO_CONTROL_H 2 | #define SERVO_CONTROL_H 3 | #include 4 | #include 5 | #include 6 | 7 | #define CURRENT_MAX 1.5 8 | 9 | void servoCtrlInit(); 10 | void servoCtrlLoop(); 11 | void servoExternSetCurrent(float cur); 12 | void servoSetGoalPos(float pos); 13 | 14 | #endif 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /VOICE_MOTOR_DRIVE/voice_drive.h: -------------------------------------------------------------------------------- 1 | #ifndef SERVO_BSP_H 2 | #define SERVO_BSP_H 3 | #include 4 | #include 5 | #include 6 | 7 | #define MOTOR_DIR_IN1 PAout(2) 8 | #define MOTOR_DIR_IN2 PAout(3) 9 | 10 | 11 | void voiceBspInit(); 12 | float getMotorPos(); 13 | void setMotorPos(float pos); 14 | float getMotorVel(); 15 | void setMotorCur(float cur); 16 | void setCtrlPeriod(int period_ms); 17 | 18 | #endif 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /ENCODER/encoder.h: -------------------------------------------------------------------------------- 1 | #ifndef _ENCODER_H 2 | #define _ENCODER_H 3 | #include "sys.h" 4 | 5 | #define ENCODER_TIMER TIM3 6 | #define RCC_APB1Periph_ENCODER_TIMER RCC_APB1Periph_TIM3 7 | #define ENCODER_TIMER_IRQHandler TIM3_IRQHandler 8 | #define ENCODER_TIMER_IRQn TIM3_IRQn 9 | #define ENCODER_RESOLUTION 8001 10 | 11 | 12 | void ENCODER_TIMER_Init(); 13 | int getEncoderCount(); 14 | int setEncoderCount(int count); 15 | 16 | #endif 17 | 18 | 19 | -------------------------------------------------------------------------------- /TIMER/timer.h: -------------------------------------------------------------------------------- 1 | #ifndef _TIMER_H 2 | #define _TIMER_H 3 | #include "sys.h" 4 | 5 | #define SYS_CLOCK_TIMER TIM2 6 | #define RCC_APB1Periph_SYS_CLOCK_TIMER RCC_APB1Periph_TIM2 7 | #define SYS_CLOCK_TIMER_IRQn TIM2_IRQn 8 | #define SYS_CLOCK_TIMER_IRQHandler TIM2_IRQHandler 9 | 10 | void registCallBack(void (*CallBack)()); 11 | u32 getSysTime(); 12 | void SYS_CLOCK_TIMER_Init(u16 arr,u16 psc); 13 | void setControlPeriodFlag(u8 val); 14 | u8 getControlPeriodFlag(); 15 | 16 | #endif 17 | 18 | 19 | -------------------------------------------------------------------------------- /FORCE_CLAW/force_claw.c: -------------------------------------------------------------------------------- 1 | #include "force_claw.h" 2 | #include "voice_drive.h" 3 | #include "servo_ctrl.h" 4 | #include "step_motor.h" 5 | #include "adc.h" 6 | #include "timer.h" 7 | 8 | 9 | int voice_motor_pos = 0; 10 | 11 | /** 12 | * @brief 13 | * @param 14 | * @return 15 | */ 16 | void forceCtrl() 17 | { 18 | voice_motor_pos = getMotorPos(); //获取控制器（音圈电机）位置 19 | stepMotorSetGoalPosition(voice_motor_pos / 2.0); //step motor 20 | 21 | float force = Get_Adc(0) / (4096.0 / 1.5); 22 | servoExternSetCurrent(force); //将电流叠加到音圈电机 23 | } 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /PID_CONTROLLER/pid_controller.h: -------------------------------------------------------------------------------- 1 | #ifndef __PID_STRUCT_H 2 | #define __PID_STRUCT_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #define PI 3.141592654 9 | #define MAX_OUT_VAL 2000.0 10 | #define MAX_INTEGRAL_VAL 5000.0 11 | 12 | typedef struct 13 | { 14 | float goal_val; 15 | float input_val; 16 | float out_val; 17 | float last_out_val; 18 | float integral_i; 19 | 20 | float Kp; 21 | float Ki; 22 | float Kd; 23 | 24 | /* interface */ 25 | float (*getFeedbackVal)(); 26 | 27 | }TargetPID; 28 | 29 | /* interface */ 30 | float PIDControl(TargetPID *target); 31 | void PID_SetGoalVal(TargetPID *target, float goal_val); 32 | void PID_SetPidPara(TargetPID *target, float kp, float ki, float kd); 33 | 34 | #endif 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /main.c: -------------------------------------------------------------------------------- 1 | #include "encoder.h" 2 | #include "timer.h" 3 | #include "math.h" 4 | #include "dac.h" 5 | #include "adc.h" 6 | #include "servo_ctrl.h" 7 | #include "voice_drive.h" 8 | #include "step_motor.h" 9 | #include "force_claw.h" 10 | 11 | int ADC_pos = 0; 12 | 13 | //kalman位置滤波器 14 | static double KalmanFilterPos(const double ResrcData,double ProcessNiose_Q,double MeasureNoise_R) 15 | { 16 | double R = MeasureNoise_R; 17 | double Q = ProcessNiose_Q; 18 | static double x_last=0; 19 | double x_mid = x_last; 20 | double x_now=0; 21 | static double p_last=0; 22 | double p_mid=0 ; 23 | double p_now=0; 24 | double kg=0; 25 | 26 | x_mid=x_last; 27 | p_mid=p_last+Q; 28 | 29 | kg=p_mid/(p_mid+R); 30 | x_now=x_mid+kg*(ResrcData-x_mid); 31 | p_now=(1-kg)*p_mid; 32 | p_last = p_now; 33 | x_last = x_now; 34 | 35 | return x_now; 36 | } 37 | 38 | int main(void) 39 | { 40 | stepMotorIO_Init(); 41 | voiceBspInit(); 42 | setCtrlPeriod(5); //5ms 43 | SYS_CLOCK_TIMER_Init(100,84-1); 44 | ENCODER_TIMER_Init(); 45 | servoCtrlInit(); 46 | Adc_Init(); 47 | 48 | while(1) 49 | { 50 | ADC_pos = Get_Adc(5); 51 | servoSetGoalPos(KalmanFilterPos(ADC_pos / 4.0, 0.01, 100.0)); 52 | forceCtrl(); 53 | servoCtrlLoop(); 54 | } 55 | } 56 | 57 | 58 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /PID_CONTROLLER/pid_controller.c: -------------------------------------------------------------------------------- 1 | #include "pid_controller.h" 2 | 3 | /** 4 | * @brief 5 | * @param 6 | * @return pid控制输出量 7 | */ 8 | float PIDControl(TargetPID *target) 9 | { 10 | float bias = 0; 11 | 12 | target->input_val = target->getFeedbackVal(); //采集输入 13 | 14 | bias = target->goal_val - target->input_val; 15 | target->integral_i += bias; 16 | target->out_val = target->Kp * bias + target->Ki * target->integral_i + target->Kd * (bias - target->last_out_val); 17 | target->last_out_val = bias; 18 | 19 | if (target->integral_i > MAX_INTEGRAL_VAL) //积分限幅 20 | { 21 | target->integral_i = MAX_INTEGRAL_VAL; 22 | } 23 | if (target->integral_i < -MAX_INTEGRAL_VAL) 24 | { 25 | target->integral_i = -MAX_INTEGRAL_VAL; 26 | } 27 | 28 | if (target->out_val > MAX_OUT_VAL) //输出限幅 29 | { 30 | target->out_val = MAX_OUT_VAL; 31 | } 32 | if (target->out_val < -MAX_OUT_VAL) 33 | { 34 | target->out_val = -MAX_OUT_VAL; 35 | } 36 | 37 | return target->out_val; 38 | } 39 | 40 | /** 41 | * @brief 42 | * @param 43 | * @return 44 | */ 45 | void PID_SetGoalVal(TargetPID *target, float goal_val) 46 | { 47 | target->goal_val = goal_val; 48 | } 49 | 50 | /** 51 | * @brief 修改pid参数 52 | * @param 53 | * @return 54 | */ 55 | void PID_SetPidPara(TargetPID *target, float kp, float ki, float kd) 56 | { 57 | target->Kp = kp; 58 | target->Ki = ki; 59 | target->Kd = kd; 60 | } 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | -------------------------------------------------------------------------------- /DAC/dac.c: -------------------------------------------------------------------------------- 1 | #include "dac.h" 2 | 3 | /** 4 | * @brief 5 | * @param 6 | * @param 7 | */ 8 | void Dac1_Init(void) 9 | { 10 | GPIO_InitTypeDef GPIO_InitStructure; 11 | DAC_InitTypeDef DAC_InitType; 12 | 13 | RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);//使能GPIOA时钟 14 | RCC_APB1PeriphClockCmd(RCC_APB1Periph_DAC, ENABLE);//使能DAC时钟 15 | 16 | GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4; 17 | GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AN;//模拟输入 18 | GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; 19 | GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;//下拉 20 | GPIO_Init(GPIOA, &GPIO_InitStructure);//初始化 21 | 22 | DAC_InitType.DAC_Trigger=DAC_Trigger_None; //不使用触发功能 TEN1=0 23 | DAC_InitType.DAC_WaveGeneration=DAC_WaveGeneration_None;//不使用波形发生 24 | DAC_InitType.DAC_LFSRUnmask_TriangleAmplitude=DAC_LFSRUnmask_Bit0;//屏蔽、幅值设置 25 | DAC_InitType.DAC_OutputBuffer=DAC_OutputBuffer_Disable ; //DAC1输出缓存关闭 BOFF1=1 26 | DAC_Init(DAC_Channel_1,&DAC_InitType); //初始化DAC通道1 27 | DAC_Cmd(DAC_Channel_1, ENABLE); //使能DAC通道1 28 | DAC_SetChannel1Data(DAC_Align_12b_R, 0); //12位右对齐数据格式设置DAC值 29 | } 30 | 31 | /** 32 | * @brief 33 | * @param 34 | * @param 35 | */ 36 | void Dac_Set_Vol(uint8_t chan, float vol) 37 | { 38 | if(chan == 1) 39 | { 40 | DAC_SetChannel1Data(DAC_Align_12b_R,vol * 4096.0 / 3.3);//12位右对齐数据格式设置DAC值 41 | } 42 | else if(chan == 2) 43 | { 44 | DAC_SetChannel2Data(DAC_Align_12b_R,vol * 4096.0 / 3.3); 45 | } 46 | } 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | -------------------------------------------------------------------------------- /STEP_MOTOR/step_motor.c: -------------------------------------------------------------------------------- 1 | #include "step_motor.h" 2 | 3 | int goal_position_ = 0, pre_position = 0, pos_bias = 0; 4 | u8 dir = 0; 5 | 6 | /** 7 | * @brief 8 | * @param 9 | * @return 10 | */ 11 | void stepMotorIO_Init(void) 12 | { 13 | GPIO_InitTypeDef GPIO_InitStructure; 14 | 15 | RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);//使能GPIO时钟 16 | 17 | //GPIOF9,F10初始化设置 18 | GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1 | GPIO_Pin_3 | GPIO_Pin_5 | GPIO_Pin_7; 19 | GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;//普通输出模式 20 | GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;//推挽输出 21 | GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;//100MHz 22 | GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;//上拉 23 | GPIO_Init(GPIOD, &GPIO_InitStructure);//初始化GPIO 24 | 25 | GPIO_SetBits(GPIOD, GPIO_Pin_1 | GPIO_Pin_3 | GPIO_Pin_5 | GPIO_Pin_7); 26 | } 27 | 28 | /** 29 | * @brief 30 | * @param 31 | * @return 32 | */ 33 | void stepMotorSetGoalPosition(int pos) 34 | { 35 | goal_position_ = pos; 36 | } 37 | 38 | /** 39 | * @brief 40 | * @param 41 | * @return 42 | */ 43 | void stepMotorCtrl() 44 | { 45 | static int count = 0; 46 | static u8 status = 1; 47 | 48 | switch(status) 49 | { 50 | case 1: 51 | if(goal_position_ != pre_position) //扫描 52 | { 53 | pos_bias = goal_position_ - pre_position; //得到偏差 54 | status = 2; 55 | } 56 | break; 57 | 58 | case 2: 59 | if(pos_bias > 0) 60 | { 61 | count ++; 62 | DrY = 1; //正方向 63 | dir = DrY; 64 | StY = !StY; 65 | if(count == (pos_bias * 2)) 66 | { 67 | pre_position += pos_bias; 68 | count = 0; 69 | status = 1; 70 | break; 71 | } 72 | } 73 | else if(pos_bias < 0) 74 | { 75 | count ++; 76 | DrY = 0; //负方向 77 | dir = DrY; 78 | StY = !StY; 79 | if(count == ((-pos_bias) * 2)) 80 | { 81 | pre_position += pos_bias; 82 | count = 0; 83 | status = 1; 84 | break; 85 | } 86 | } 87 | 88 | default: 89 | break; 90 | } 91 | } 92 | 93 | 94 | -------------------------------------------------------------------------------- /SERVO_CTRL/servo_ctrl.c: -------------------------------------------------------------------------------- 1 | #include "timer.h" 2 | #include "encoder.h" 3 | #include "pid_controller.h" 4 | #include "voice_drive.h" 5 | #include "servo_ctrl.h" 6 | 7 | TargetPID pid_pos_loop, pid_vel_loop; 8 | float Kp_pos_loop = 0.0, Ki_pos_loop = 0.0, Kd_pos_loop = 0; 9 | float Kp_vel_loop = 0.2, Ki_vel_loop = 0.0, Kd_vel_loop = 0; 10 | float goal_pos_loop = 330, goal_vel_loop = 0, goal_cur_loop = 0; 11 | float extern_cur = 0; 12 | 13 | 14 | /** 15 | * @brief 16 | * @param 17 | * @return 18 | */ 19 | void servoCtrlInit() 20 | { 21 | pid_pos_loop.getFeedbackVal = getMotorPos; 22 | pid_vel_loop.getFeedbackVal = getMotorVel; 23 | 24 | PID_SetPidPara(&pid_pos_loop, Kp_pos_loop, Ki_pos_loop, Kd_pos_loop); 25 | PID_SetPidPara(&pid_vel_loop, Kp_vel_loop, Ki_vel_loop, Kd_vel_loop); 26 | } 27 | 28 | /** 29 | * @brief 将外部电流叠加到音圈电机 30 | * @param 31 | * @return 32 | */ 33 | void servoExternSetCurrent(float cur) 34 | { 35 | extern_cur = cur; 36 | } 37 | 38 | /** 39 | * @brief 40 | * @param 41 | * @return 42 | */ 43 | void servoSetGoalPos(float pos) 44 | { 45 | goal_pos_loop = pos; 46 | } 47 | 48 | /** 49 | * @brief 50 | * @param 51 | * @return 52 | */ 53 | void servoCtrlLoop() 54 | { 55 | getEncoderCount(); 56 | PID_SetPidPara(&pid_pos_loop, Kp_pos_loop, Ki_pos_loop, Kd_pos_loop); 57 | PID_SetPidPara(&pid_vel_loop, Kp_vel_loop, Ki_vel_loop, Kd_vel_loop); 58 | PID_SetGoalVal(&pid_pos_loop, goal_pos_loop); //设置位置环目标值 59 | goal_vel_loop = PIDControl(&pid_pos_loop) * 10.0; //速度环目标 = 位置环输出 60 | PID_SetGoalVal(&pid_vel_loop, goal_vel_loop); //设置速度环目标值 61 | goal_cur_loop = PIDControl(&pid_vel_loop) / 10.0; //电流环目标 = 速度环输出 62 | 63 | goal_cur_loop = goal_cur_loop + extern_cur; 64 | 65 | if(goal_cur_loop > CURRENT_MAX) 66 | goal_cur_loop = CURRENT_MAX; 67 | if(goal_cur_loop < -CURRENT_MAX) 68 | goal_cur_loop = -CURRENT_MAX; 69 | 70 | if(goal_cur_loop > 0) 71 | { 72 | MOTOR_DIR_IN1 = 0; 73 | MOTOR_DIR_IN2 = 1; 74 | } 75 | else 76 | { 77 | MOTOR_DIR_IN1 = 1; 78 | MOTOR_DIR_IN2 = 0; 79 | } 80 | setMotorCur(fabs(goal_cur_loop)); 81 | } 82 | 83 | 84 | -------------------------------------------------------------------------------- /ADC/adc.c: -------------------------------------------------------------------------------- 1 | #include "adc.h" 2 | #include "delay.h" 3 | 4 | /** 5 | * @brief 6 | * @param 7 | * @param 8 | */ 9 | void Adc_Init(void) 10 | { 11 | GPIO_InitTypeDef GPIO_InitStructure; 12 | ADC_CommonInitTypeDef ADC_CommonInitStructure; 13 | ADC_InitTypeDef ADC_InitStructure; 14 | 15 | RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);//使能GPIOA时钟 16 | RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE);//使能GPIOA时钟 17 | RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1, ENABLE); //使能ADC1时钟 18 | 19 | //先初始化ADC1通道 IO口 20 | GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_5;//PA 通道 21 | GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AN;//模拟输入 22 | GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL ;//不带上下拉 23 | GPIO_Init(GPIOA, &GPIO_InitStructure);//初始化 24 | 25 | RCC_APB2PeriphResetCmd(RCC_APB2Periph_ADC1,ENABLE); //ADC1复位 26 | RCC_APB2PeriphResetCmd(RCC_APB2Periph_ADC1,DISABLE); //复位结束 27 | 28 | ADC_CommonInitStructure.ADC_Mode = ADC_Mode_Independent;//独立模式 29 | ADC_CommonInitStructure.ADC_TwoSamplingDelay = ADC_TwoSamplingDelay_5Cycles;//两个采样阶段之间的延迟5个时钟 30 | ADC_CommonInitStructure.ADC_DMAAccessMode = ADC_DMAAccessMode_Disabled; //DMA失能 31 | ADC_CommonInitStructure.ADC_Prescaler = ADC_Prescaler_Div4; 32 | ADC_CommonInit(&ADC_CommonInitStructure);//初始化 33 | 34 | ADC_InitStructure.ADC_Resolution = ADC_Resolution_12b;//12位模式 35 | ADC_InitStructure.ADC_ScanConvMode = DISABLE;//非扫描模式 36 | ADC_InitStructure.ADC_ContinuousConvMode = DISABLE;//关闭连续转换 37 | ADC_InitStructure.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_None; 38 | ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right;//右对齐 39 | ADC_InitStructure.ADC_NbrOfConversion = 1; 40 | ADC_Init(ADC1, &ADC_InitStructure);//ADC初始化 41 | 42 | ADC_Cmd(ADC1, ENABLE);//开启AD转换器 43 | 44 | } 45 | 46 | /** 47 | * @brief 48 | * @param 49 | * @param 50 | */ 51 | u16 Get_Adc(u8 ch) 52 | { 53 | ADC_RegularChannelConfig(ADC1, ch, 1, ADC_SampleTime_480Cycles ); 54 | ADC_SoftwareStartConv(ADC1); //使能指定的ADC1的软件转换启动功能 55 | while(!ADC_GetFlagStatus(ADC1, ADC_FLAG_EOC ));//等待转换结束 56 | return ADC_GetConversionValue(ADC1); //返回最近一次ADC1规则组的转换结果 57 | } 58 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /TIMER/timer.c: -------------------------------------------------------------------------------- 1 | #include "timer.h" 2 | #include "encoder.h" 3 | #include "math.h" 4 | #include "step_motor.h" 5 | 6 | 7 | /*系统时间*/ 8 | volatile u32 g_sys_time = 0; 9 | u8 controlPeriodFlag = 0; 10 | void (*timerCallBack)(); 11 | 12 | /** 13 | * @brief 14 | * @param 15 | * @param 16 | */ 17 | void registCallBack(void (*CallBack)()) 18 | { 19 | timerCallBack = CallBack; 20 | } 21 | 22 | /** 23 | * @brief 24 | * @param 25 | * @param 26 | */ 27 | void SYS_CLOCK_TIMER_Init(u16 arr,u16 psc) 28 | { 29 | TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure; 30 | NVIC_InitTypeDef NVIC_InitStructure; 31 | 32 | RCC_APB1PeriphClockCmd(RCC_APB1Periph_SYS_CLOCK_TIMER,ENABLE); ///使能SYS_CLOCK_TIMER时钟 33 | 34 | TIM_TimeBaseInitStructure.TIM_Period = arr; //自动重装载值 35 | TIM_TimeBaseInitStructure.TIM_Prescaler=psc; //定时器分频 36 | TIM_TimeBaseInitStructure.TIM_CounterMode=TIM_CounterMode_Up; //向上计数模式 37 | TIM_TimeBaseInitStructure.TIM_ClockDivision=TIM_CKD_DIV1; 38 | 39 | TIM_TimeBaseInit(SYS_CLOCK_TIMER,&TIM_TimeBaseInitStructure);//初始化SYS_CLOCK_TIMER 40 | 41 | TIM_ITConfig(SYS_CLOCK_TIMER,TIM_IT_Update,ENABLE); //允许定时器3更新中断 42 | TIM_Cmd(SYS_CLOCK_TIMER,ENABLE); //使能定时器3 43 | 44 | NVIC_InitStructure.NVIC_IRQChannel=SYS_CLOCK_TIMER_IRQn; //定时器3中断 45 | NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=0x01; //抢占优先级1 46 | NVIC_InitStructure.NVIC_IRQChannelSubPriority=0x03; //子优先级3 47 | NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE; 48 | NVIC_Init(&NVIC_InitStructure); 49 | 50 | } 51 | 52 | /** 53 | * @brief get全局系统运行时间 54 | * @param 55 | * @param 56 | */ 57 | u32 getSysTime() 58 | { 59 | return g_sys_time; 60 | } 61 | 62 | /** 63 | * @brief getControlPeriodFlag 64 | * @param 65 | * @param 66 | */ 67 | 68 | u8 getControlPeriodFlag() 69 | { 70 | return controlPeriodFlag; 71 | } 72 | 73 | /** 74 | * @brief setControlPeriodFlag 75 | * @param 76 | * @param 77 | */ 78 | void setControlPeriodFlag(u8 val) 79 | { 80 | controlPeriodFlag = val; 81 | } 82 | 83 | /** 84 | * @brief 定时器中断服务函数 85 | * @param 86 | * @param 87 | */ 88 | void SYS_CLOCK_TIMER_IRQHandler(void) 89 | { 90 | static int count = 0; 91 | 92 | if(TIM_GetITStatus(SYS_CLOCK_TIMER,TIM_IT_Update)==SET) //溢出中断 93 | { 94 | g_sys_time ++; 95 | timerCallBack(); 96 | stepMotorCtrl(); 97 | if(g_sys_time % 200 == 0) 98 | { 99 | setControlPeriodFlag(1); 100 | } 101 | } 102 | 103 | TIM_ClearITPendingBit(SYS_CLOCK_TIMER,TIM_IT_Update); //清除中断标志位 104 | } 105 | -------------------------------------------------------------------------------- /ENCODER/encoder.c: -------------------------------------------------------------------------------- 1 | #include "encoder.h" 2 | 3 | int circle_count = 1; 4 | int Encoder = 0; 5 | 6 | /** 7 | * @brief Encoder init 8 | * @param 9 | * @param 10 | */ 11 | void ENCODER_TIMER_Init() 12 | { 13 | GPIO_InitTypeDef GPIO_InitStructure; 14 | TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; 15 | TIM_ICInitTypeDef TIM_ICInitStructure; 16 | NVIC_InitTypeDef NVIC_InitStructure; 17 | 18 | RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE); 19 | RCC_APB1PeriphClockCmd(RCC_APB1Periph_ENCODER_TIMER, ENABLE); 20 | 21 | GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; 22 | GPIO_InitStructure.GPIO_OType = GPIO_OType_OD; 23 | GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz; 24 | GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; 25 | GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7; 26 | GPIO_Init(GPIOA, &GPIO_InitStructure); 27 | GPIO_PinAFConfig(GPIOA, GPIO_PinSource6, GPIO_AF_TIM3); 28 | GPIO_PinAFConfig(GPIOA, GPIO_PinSource7, GPIO_AF_TIM3); 29 | 30 | TIM_DeInit(ENCODER_TIMER); 31 | TIM_TimeBaseStructInit(&TIM_TimeBaseStructure); 32 | TIM_TimeBaseStructure.TIM_Prescaler = 0; 33 | TIM_TimeBaseStructure.TIM_Period = ENCODER_RESOLUTION - 1; 34 | TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; 35 | TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Down; 36 | TIM_TimeBaseInit(ENCODER_TIMER, &TIM_TimeBaseStructure); 37 | TIM_EncoderInterfaceConfig(ENCODER_TIMER, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising ,TIM_ICPolarity_Rising); 38 | 39 | TIM_ICStructInit(&TIM_ICInitStructure); 40 | TIM_ICInitStructure.TIM_Channel=TIM_Channel_1 | TIM_Channel_2; 41 | TIM_ICInitStructure.TIM_ICPolarity=TIM_ICPolarity_Rising; 42 | TIM_ICInitStructure.TIM_ICSelection =TIM_ICSelection_DirectTI; 43 | TIM_ICInitStructure.TIM_ICFilter = 0; 44 | TIM_ICInit(ENCODER_TIMER, &TIM_ICInitStructure); 45 | 46 | TIM_ITConfig(ENCODER_TIMER,TIM_IT_Update,ENABLE); 47 | 48 | NVIC_InitStructure.NVIC_IRQChannel=ENCODER_TIMER_IRQn; 49 | NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=0x01; 50 | NVIC_InitStructure.NVIC_IRQChannelSubPriority=0x01; 51 | NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE; 52 | NVIC_Init(&NVIC_InitStructure); 53 | 54 | TIM_SetCounter(ENCODER_TIMER, 0); 55 | TIM_Cmd(ENCODER_TIMER, ENABLE); 56 | } 57 | 58 | /** 59 | * @brief setEncoderCount 60 | * @param 61 | * @param 62 | */ 63 | int setEncoderCount(int count) 64 | { 65 | Encoder = count; 66 | } 67 | 68 | /** 69 | * @brief getEncoderCount 70 | * @param 71 | * @param 72 | */ 73 | int getEncoderCount() 74 | { 75 | Encoder = TIM_GetCounter(ENCODER_TIMER) + ENCODER_RESOLUTION * circle_count; 76 | return Encoder; 77 | } 78 | 79 | /** 80 | * @brief 81 | * @param 82 | * @param 83 | */ 84 | void ENCODER_TIMER_IRQHandler(void) 85 | { 86 | if(TIM_GetITStatus(ENCODER_TIMER,TIM_IT_Update)==SET) 87 | { 88 | if((ENCODER_TIMER->CR1>>4 & 0x01)==0) //DIR==0 89 | circle_count++; 90 | else if((ENCODER_TIMER->CR1>>4 & 0x01)==1)//DIR==1 91 | circle_count--; 92 | } 93 | TIM_ClearITPendingBit(ENCODER_TIMER,TIM_IT_Update); 94 | } 95 | -------------------------------------------------------------------------------- /VOICE_MOTOR_DRIVE/voice_drive.c: -------------------------------------------------------------------------------- 1 | #include "timer.h" 2 | #include "adc.h" 3 | #include "dac.h" 4 | #include "encoder.h" 5 | #include "voice_drive.h" 6 | 7 | //针对硬盘音圈电机驱动程序 8 | 9 | float motor_pos = 0, motor_vel = 0; 10 | int ctrl_period_ms = 0; 11 | float Q_p = 0.01, Q_v = 0.001; 12 | 13 | /** 14 | * @brief kalman滤波器 15 | * @param 16 | * @return 17 | */ 18 | static double KalmanFilterP(const double ResrcData,double ProcessNiose_Q,double MeasureNoise_R) 19 | { 20 | 21 | double R = MeasureNoise_R; 22 | double Q = ProcessNiose_Q; 23 | 24 | static double x_last; 25 | double x_mid = x_last; 26 | double x_now; 27 | 28 | static double p_last; 29 | double p_mid ; 30 | double p_now; 31 | 32 | double kg; 33 | 34 | x_mid=x_last; 35 | p_mid=p_last+Q; 36 | 37 | 38 | kg=p_mid/(p_mid+R); 39 | x_now=x_mid+kg*(ResrcData-x_mid); 40 | p_now=(1-kg)*p_mid; 41 | p_last = p_now; 42 | x_last = x_now; 43 | 44 | return x_now; 45 | 46 | } 47 | 48 | /** 49 | * @brief kalman滤波器 50 | * @param 51 | * @return 52 | */ 53 | static double KalmanFilterV(const double ResrcData,double ProcessNiose_Q,double MeasureNoise_R) 54 | { 55 | 56 | double R = MeasureNoise_R; 57 | double Q = ProcessNiose_Q; 58 | 59 | static double x_last; 60 | double x_mid = x_last; 61 | double x_now; 62 | 63 | static double p_last; 64 | double p_mid ; 65 | double p_now; 66 | 67 | double kg; 68 | 69 | x_mid=x_last; 70 | p_mid=p_last+Q; 71 | 72 | 73 | kg=p_mid/(p_mid+R); 74 | x_now=x_mid+kg*(ResrcData-x_mid); 75 | p_now=(1-kg)*p_mid; 76 | p_last = p_now; 77 | x_last = x_now; 78 | 79 | return x_now; 80 | 81 | } 82 | 83 | /** 84 | * @brief 85 | * @param 86 | * @return 87 | */ 88 | void voiceCtrlIO_Init(void) 89 | { 90 | GPIO_InitTypeDef GPIO_InitStructure; 91 | 92 | RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);//使能GPIO时钟 93 | 94 | //GPIOF9,F10初始化设置 95 | GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2 | GPIO_Pin_3; 96 | GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;//普通输出模式 97 | GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;//推挽输出 98 | GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;//100MHz 99 | GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;//上拉 100 | GPIO_Init(GPIOA, &GPIO_InitStructure);//初始化GPIO 101 | 102 | GPIO_SetBits(GPIOA, GPIO_Pin_2 | GPIO_Pin_3); 103 | 104 | } 105 | 106 | 107 | /** 108 | * @brief 109 | * @param 110 | * @return 111 | */ 112 | void setCtrlPeriod(int period_ms) 113 | { 114 | ctrl_period_ms = period_ms; 115 | } 116 | 117 | /** 118 | * @brief 电位器位置反馈 119 | * @param 120 | * @return 121 | */ 122 | void voiceBspCB_ADC() 123 | { 124 | static float last_pos = 0; 125 | static int count = 0; 126 | float pos = 0; 127 | 128 | count ++; 129 | pos = Get_Adc(0) - 1833; 130 | motor_pos = KalmanFilterP(pos, Q_p, 1.0); 131 | if(count == 5) 132 | { 133 | count = 0; 134 | motor_vel = KalmanFilterV((motor_pos - last_pos) / ctrl_period_ms * 100.0, Q_v, 1.0); 135 | last_pos = motor_pos; 136 | } 137 | } 138 | 139 | /** 140 | * @brief 编码器位置反馈 141 | * @param 142 | * @return 143 | */ 144 | void voiceBspCB_Encoder() 145 | { 146 | static float last_pos = 0; 147 | static int count = 0; 148 | float pos = 0; 149 | 150 | count ++; 151 | pos = getEncoderCount(); 152 | motor_pos = pos; //KalmanFilterP(pos, Q_p, 1.0); 153 | if(count == 2) 154 | { 155 | count = 0; 156 | motor_vel = KalmanFilterV((motor_pos - last_pos) / ctrl_period_ms * 100.0, Q_v, 1.0); 157 | last_pos = motor_pos; 158 | } 159 | 160 | 161 | } 162 | 163 | /** 164 | * @brief 165 | * @param 166 | * @return 167 | */ 168 | void voiceBspInit() 169 | { 170 | registCallBack(voiceBspCB_Encoder); 171 | voiceCtrlIO_Init(); 172 | Dac1_Init(); 173 | Adc_Init(); 174 | } 175 | 176 | /** 177 | * @brief 178 | * @param 179 | * @return 180 | */ 181 | float getMotorPos() 182 | { 183 | return motor_pos; 184 | } 185 | 186 | /** 187 | * @brief 188 | * @param 189 | * @return 190 | */ 191 | float getMotorVel() 192 | { 193 | return motor_vel; 194 | } 195 | 196 | /** 197 | * @brief 198 | * @param 199 | * @return 200 | */ 201 | void setMotorCur(float cur) 202 | { 203 | Dac_Set_Vol(1, cur); 204 | } 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | --------------------------------------------------------------------------------