请问谁做过MCU控制并联臂机器人的案例？

只看该作者 · 2016-12-25 12:16

详见照片11

只看该作者 · 2016-12-25 12:19

详见照片11

只看该作者 · 2016-12-25 12:20

详见照片12

只看该作者 · 2016-12-25 12:20

详见照片13

只看该作者 · 2016-12-25 12:22

以上1-13算法有需要的，可以看看

只看该作者 · 2016-12-25 12:29

这个源码不好找啊！

只看该作者 · 2016-12-25 12:30

感觉这算法有点难

只看该作者 · 2016-12-25 12:33

给个解析会很清楚

1万 · 2016-12-25 12:39

楼上的，我也搞过一些算法，下面来学习分享一下

1万 · 2016-12-25 12:40

#include "stm32f10x.h"
#include "encoder_operation.h"
#include "my_stm32_bitband_define.h"

#define INTNUM 40
#define OFF_SPEED_LOOP_VAL 300 //离目标位置还差这么多时退出速度环的
#define SPEED_CONTROL_BIAS 0 //速度控制算出kd需要一个偏移量。具体解释看speed_control()函数。

typedef struct

1万 · 2016-12-25 12:40

int nowspeed;
  int targetspeed;
  int nowdistance;
  int targetdistance;
  int orderspeed;
  int ifdone;//是否完成定位。
}Encoder_structure;

static Encoder_structure EncoderStatus[4];
static int pre[4],now[4],encoderhigh[4]={0,0,0,0};
static int speed[4];
static const int kp_0[4] = {0, 30, 30, 30};
static const int ki_0[4] = {0, 100, 100, 100};
//static int kd_1[4] = {0, -100, -100, -100};
static const int kd_0[4] = {0, -10, -10, -10};

1万 · 2016-12-25 12:46

            电机编号设置当前值    设置目标值    设置目标速度
void motor_init(int num, int now_set, int target_set, int targetspeed_set)
{
  encoderhigh[num] = now_set/65536;

  if(num ==1)ENCVAL_1 = now_set%65536;
  if(num ==2)ENCVAL_2 = now_set%65536;
  if(num ==3)ENCVAL_3 = now_set%65536;

  EncoderStatus[num].nowdistance = now_set;
  EncoderStatus[num].targetdistance = target_set;
  EncoderStatus[num].targetspeed = targetspeed_set;
}
//别的文件的函数取得编码器的速度：==============================
int get_encoder_speed(int motornum)
{
  return(EncoderStatus[motornum].nowspeed)

1万 · 2016-12-25 12:49

/取得实际编码器数值============================================
int get_real_encoder(int motornum)
{
  return(EncoderStatus[motornum].nowdistance);
}
//别的文件设置电机相对位置，速度==============================
//
void set_motor_addon_distance(int num, int addon, int speed)
{
  EncoderStatus[num].targetdistance += addon;
  EncoderStatus[num].targetspeed = speed;
}

1万 · 2016-12-25 12:50

//别的文件设置电机绝对位置，速度==============================
//
void set_motor_distance(int num, int dis, int speed)
{
  EncoderStatus[num].targetdistance = dis;
  EncoderStatus[num].targetspeed = speed;
}
//别的文件查询是否已经运动到目标点==============================
//
int get_if_reach(int num)
{
  return EncoderStatus[num].ifdone;
}

1万 · 2016-12-25 12:50

//对电机进行PID控制=============================================
//在公式pwm = kp*(target - now) - kd*speed + 1/ki*segma(error),由于kd越大，
//pwm值越小，speed越小，则若要让speed达到指定值，kd要减小；小于指定值，kd增大
//反转时，speed是负的，要让speed达到指定值，kd还是要减小
//现在发现用kd控制速度不是个办法，于是将速度环和位置环分离了
void motor_pid(int num)
{
  int pwmval=0;
  int speed=0,now=0;//当前速度和位置
  int target=0,targetspeed=0;//目标位置和目标速度
  int err=0,err_abs=0;//与目标误差，及其绝对值
  int kp,ki,kd;//三个参数
  static int justnowerr[4];//上次的速度，上次的误差
  static int speedintag[4];//速度积分项，长期积分
  int frflag;//正反转标志

1万 · 2016-12-25 12:51

target = EncoderStatus[num].targetdistance; //先给各个变量赋值
  targetspeed = EncoderStatus[num].targetspeed;
  now = EncoderStatus[num].nowdistance;
  speed = EncoderStatus[num].nowspeed;
  kp = kp_0[num];
  ki = ki_0[num];

  /*求出位置误差*/
  err = now - target;
  if(err<0)err_abs = -err;
  else err_abs = err;

  /*把我数字全变成正的!!!!之前使负数晕死我了*/
  if(target<0) target = -target;
  if(now<0) now = -now;
  if(speed<0) speed = -speed;

  /*决定是速度闭环还是位置闭环*/
  if( err_abs < OFF_SPEED_LOOP_VAL )
  {

1万 · 2016-12-25 12:51

/*终于开始上位置PID了*/
pwmval = err_abs * kp; //算出比例项
pwmval = pwmval + kd_0[num]*speed;//加上微分项，减速
frflag = err;
EncoderStatus[num].ifdone = 1;//算是已经到达定位点了
}

1万 · 2016-12-25 12:52

else  //离终点远着呢，速度环继续,位置环歇着
  {

if(frflag<0 && justnowerr[num]>0)speedintag[num]=0;//换向的话，速度积分重算
if(frflag>0 && justnowerr[num]<0)speedintag[num]=0;
justnowerr[num] = frflag;

speedintag[num] += targetspeed - speed;
if(speedintag[num] > 4000)speedintag[num] = 4000;
if(speedintag[num] < -4000)speedintag[num] = -4000;

pwmval = (SPEED_CONTROL_BIAS + targetspeed - speed)*100 + speedintag[num];

if(speed > targetspeed+2)frflag = -err;
else frflag = err;
if(pwmval<0)pwmval = 0;
if(pwmval>3920)pwmval = 3920;
EncoderStatus[num].ifdone = 0;//未完成定位
}

1万 · 2016-12-25 12:52

/*赋给PWM值*/
//#define HALFPWM
#ifdef HALFPWM
#define
  if(num == 1)
  {
if(frflag<0){MOTOR1_R; PWMVALUE_1 = pwmval/2;}
else {MOTOR1_F; PWMVALUE_1 = pwmval/2;}
  }
  else if(num == 2)
  {
if(frflag<0){MOTOR2_R; PWMVALUE_2 = pwmval/2;}
else {MOTOR2_F; PWMVALUE_2 = pwmval/2;}
  }
  else if(num == 3)
  {
if(frflag<0){MOTOR3_R; PWMVALUE_3 = pwmval/2;}
else {MOTOR3_F; PWMVALUE_3 = pwmval/2;}
  }
#else
if(num == 1)
  {
if(frflag<0){MOTOR1_R; PWMVALUE_1 = pwmval;}
else {MOTOR1_F; PWMVALUE_1 = pwmval;}
  }
  else if(num == 2)
  {
if(frflag<0){MOTOR2_R; PWMVALUE_2 = pwmval;}
else {MOTOR2_F; PWMVALUE_2 = pwmval;}
  }
  else if(num == 3)
  {
if(frflag<0){MOTOR3_R; PWMVALUE_3 = pwmval;}
else {MOTOR3_F; PWMVALUE_3 = pwmval;}
  }
#endif

1万 · 2016-12-25 12:53

/*留下1%占空比给电荷泵*/
  if(PWMVALUE_1 < 40)PWMVALUE_1 = 40;
  if(PWMVALUE_2 < 40)PWMVALUE_2 = 40;
  if(PWMVALUE_3 < 40)PWMVALUE_3 = 40;
}

////电机速度控制，返回的是PID控制中的kd值============================
////废弃不用了
//int speed_control(int speed, int targetspeed, int err)
//{
//  static int justnowspeed,justnowerr;
//  static int intag;//积分项，长期积分
//
//  if(err<0 && justnowerr>0)intag=0;//换向的话，速度积分重算
//  if(err>0 && justnowerr<0)intag=0;
//  justnowerr = err;
//
//  intag += targetspeed - speed