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

1万 · 2016-12-25 12:53

/  return (SPEED_CONTROL_BIAS + targetspeed - speed)*100 + intag;
//  //这里假设kd与速度控制的关系是线性的，kd越大速度越小，则目标速度越大kd越小，
//  //当前速度越大kd越大。
//}

1万 · 2016-12-25 12:54

#include "stm32f10x.h"
#include "encoder_operation.h"
#include "my_stm32_bitband_define.h"
#include "machine_control.h"
#include "math.h"
#include "atan_list_1.h"
#include "machine_poslist.h"

#define DELTA_BASE 10//机器移动步进距离
#define SPEED_BASE 30//机器基速度。有时间再将速度可调

1万 · 2016-12-25 12:54

typedef struct
{ int x;
  int y;
  int z;
  int nowx;
  int nowy;
  int nowz;
  int tx;
  int ty;
  int tz;
  int pos[3][5];
  int angle[4];
  int cmdpointer;//指向哪个值，x还是y还是z
  int numpointer;//指向某值的哪个位，第一还是第二还是第三

  int done_steps;//已完成的动作数目
  int autorun;//允许自己跑
}machinestatus;

1万 · 2016-12-25 12:55

static machinestatus machine;

//  int a,b,c, speed_a, speed_b, speed_c;//分别是三个电机的编码器值和速度
//  int k1,k2,k3, u1,u2,u3, v1,v2,v3;//解算中间值，详见那姐们的论文
//  float t1,t2,t3;
//  int angle1, angle2, angle3;
//  int mid,mid2,mid4,mid5;//解算优化中间值
//  long long mid3;

1万 · 2016-12-25 12:56

//  int a,b,c, speed_a, speed_b, speed_c;//分别是三个电机的编码器值和速度
//  int k1,k2,k3, u1,u2,u3, v1,v2,v3;//解算中间值，详见那姐们的论文
//  float t1,t2,t3;
//  int angle1, angle2, angle3;
//  int mid,mid2,mid4,mid5;//解算优化中间值
//  long long mid3;

//放在USART1里的函数，用于接收和更改命令========================
//
void UASRT_cmd_process(void)
{
  char cmdval;
  cmdval = USART_ReceiveData(USART1) & 0xFF;//接收数据，整理除去前两位
  if (cmdval == 'x')
{machine.cmdpointer = 0; machine.numpointer = 0; USART_SendData(USART1 , 'x');}//选x坐标
  else if(cmdval == 'y')
{machine.cmdpointer = 1; machine.numpointer = 0; USART_SendData(USART1 , 'y');}//选y坐标
  else if(cmdval == 'z')
{machine.cmdpointer = 2; machine.numpointer = 0; USART_SendData(USART1 , 'z');}//选z坐标
  else if(cmdval == '-' || cmdval =='+')
{machine.pos[machine.cmdpointer][4]  = 44 - cmdval; USART_SendData(USART1 , cmdval);}
  else if(cmdval == 'd')machine.z -= 10;//抬刀一点
  else if(cmdval == 'u')machine.z += 10;//落刀一点
  else if(cmdval == 'l')machine.x +=100;//x+
  else if(cmdval == 'j')machine.x -=100;//x-
  else if(cmdval == 'i')machine.y +=100;//y+
  else if(cmdval == 'k')machine.y -=100;//y-
  else if(cmdval == 'a')machine.autorun = 1;
  else if(cmdval == 'u')machine.autorun = 0;
  else if(cmdval>='0' && cmdval<='9')//是数字或符号
{
//    if(cmdval < 48)machine.pos[machine.cmdpointer][3]  = 44 - cmdval;
   machine.pos[machine.cmdpointer][machine.numpointer] = cmdval - '0';
   machine.numpointer++;
   USART_SendData(USART1 , cmdval);
   if(machine.numpointer>=4 && machine.cmdpointer == 0)
      {
      machine.numpointer = 0;
      machine.x = machine.pos[0][0]*10000 + machine.pos[0][1]*1000 + machine.pos[0][2]*100 + machine.pos[0][3]*10;
      machine.x *= machine.pos[0][4];
      //USART_SendData(USART1 , 'X');
      //注意，保存的是以10um为单位
      }

1万 · 2016-12-25 12:56

  if(machine.numpointer>=4 && machine.cmdpointer == 1)
      {
      machine.numpointer = 0;
      machine.y = machine.pos[1][0]*10000 + machine.pos[1][1]*1000 + machine.pos[1][2]*100 + machine.pos[1][3]*10;
      machine.y *= machine.pos[1][4];
      //USART_SendData(USART1 , 'Y');
      }
   if(machine.numpointer>=4 && machine.cmdpointer == 2)
      {
      machine.numpointer = 0;
      machine.z = machine.pos[2][0]*10000 + machine.pos[2][1]*1000 + machine.pos[2][2]*100 + machine.pos[2][3]*10;
      machine.z *= machine.pos[2][4];
      //USART_SendData(USART1 , 'Z');
      }
}
  else if(cmdval == 'o')machine_status_init();//o指的是回原点

  USART_ClearITPendingBit(USART1,USART_IT_RXNE);
}

1万 · 2016-12-25 12:57

//电机状态初始化==================================================
//
void machine_status_init(void)
{
  char j,k;
  //初始化机器当前和目标坐标以及步进坐标
  machine.x = 0;
  machine.y = 0;
  machine.z = -21000;
  machine.nowx = 0;
  machine.nowy = 0;
  machine.nowz = -21000;
  machine.tx = 0;
  machine.ty = 0;
  machine.tz = -21000;
  //初始化机器命令坐标数组
  for(j=0;j<3;j++)

1万 · 2016-12-25 12:57

  {
   for(k=0;k<3;k++)machine.pos[j][k] = 0;
}
  //初始化电机的编码器值
  for(j=0;j<4;j++)machine.angle[j] = 0;
  //初始化坐标符号
  machine.pos[0][3] = 1;
  machine.pos[1][3] = 1;
  machine.pos[2][3] = 1;
  //初始化命令指针
  machine.cmdpointer = 0;
  machine.numpointer = 0;
  machine.done_steps = 0;
}

1万 · 2016-12-25 12:59

//取得当前坐标====================================================
//
int get_now_pos(char num)
{
  //存在内存中的数据是10um为单位，给别人看的则是100um
  if(num == 'x')return machine.nowx;
  if(num == 'y')return machine.nowy;
  if(num == 'z')return machine.nowz;

  return 0;
}

1万 · 2016-12-25 12:59

//取得目标坐标====================================================
//
int get_target_pos(char num)
{
  if(num == 'x')return machine.x;
  if(num == 'y')return machine.y;
  if(num == 'z')return machine.z;

  return 0;
}

1万 · 2016-12-25 13:00

//直接给坐标，不经过串口命令，慎用================================
//注意，输入的单位是mm
void give_pos_to_machine(int x, int y, int z)
{
  machine.x = x*100;
  machine.y = y*100;
  machine.z = z*100;
}

1万 · 2016-12-25 13:01

////计算出正解，即电机要转动到的编码器值并传递给电机驱动===========================
//注意，输入的单位是10um
void solve_motor_f(int x, int y, int z)
{
#define La 11700//234 * 50
#define Lb 5150 //103 * 50
#define La2 136890000
#define Lb2 26522500

  int a,b,c, speed_a, speed_b, speed_c;//分别是三个电机的编码器值和速度
  int k1,k2,k3, u1,u2,u3, v1,v2,v3;//解算中间值，详见那姐们的论文
  float t1,t2,t3;
  int angle1, angle2, angle3;
  long long mid,mid2,mid4,mid5;//解算优化中间值
  long long mid3;
  int delta_a, delta_b, delta_c,delta_max;

  x /= 2;
  y /= 2;
  z /= 2; //从这里开始，运算单位都是20um,之前是10um  ///3

  mid = x*x + y*y + z*z;//mid记录的是三个坐标平方和，因为后面用的频繁 ///9
  mid2 = (La2 - Lb2 - mid); ///10
  //以下算a编码器值--------------------------------------------------------
  k1 = mid2 / Lb + 2*z;  ///19
  v1 = mid2 / Lb - 2*z;  ///20
  u1 = (3464*x + 2000*y)/1000;//这里先把sqrt(3)给放大了，最后再缩小 ///18
  mid5 = k1*v1/10;  ///27
  mid5 *=4; //19
  mid3 = u1*u1/10 - mid5;  ///29
  mid4 = sqrt(mid3)*6325;  ///373
  t1 = (-u1*2000 - mid4)/(2*k1);//至此，t1被放大了2000倍，精度0.025%  //46
  angle1 = atan_list_1[(int)(t1*2.5)];  ///2568,**
  a = -angle1 * 13201 ; ///209，**
  a = a /5000;
  //算b编码器值------------------------------------------------------------
  k2 = k1;//mid2 / Lb + 2*z;
  v2 = v1;//mid2 / Lb - 2*z;
  u2 = (-3464*x + 2000*y)/1000;
//  mid5 = k2*v2/10;  //在这里a的mid3没有变化
//  mid5 *=4;
  mid3 = u2*u2/10 - mid5;
  mid4 = sqrt(mid3)*6325;
  t2 = (-u2*2000 - mid4)/(2*k2);
  angle2 = atan_list_1[(int)(t2*2.5)];
  b = -angle2 * 13201 ;
  b = b /5000;
  //以下算c编码器值--------------------------------------------------------
  k3 = k1 / 2;//mid2 / Lb + 2*z;
  v3 = v1 / 2;//mid2 / Lb - 2*z;
  u3 =  -2*y;
  mid5 = mid5 / 4;//k1*v1/10;
  //mid5 *=4; //19
  mid3 = u3*u3/10 - mid5;
  mid4 = sqrt(mid3)*6325;
  t3 = (-u3*2000 - mid4)/(2*k3);
  angle3 = atan_list_1[(int)(t3*2.5)];
  c = -angle3 * 13201 ;
  c = c /5000;
  //解算完鸟!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
  //然后算编码器速度---------------------------------------------------------
  //算出各个电机编码器值的差值，绝对值
  delta_a = get_real_encoder(1) - a;
  if(delta_a<0)delta_a = -delta_a;

  delta_b = get_real_encoder(2) - b;
  if(delta_b<0)delta_b = -delta_b;

  delta_c = get_real_encoder(3) - c;
  if(delta_c<0)delta_c = -delta_c;

  //找出最大差值
  delta_max = delta_a;
  if(delta_max < delta_b)delta_max = delta_b;
  if(delta_max < delta_c)delta_max = delta_c;

  //算出速度
  speed_a = (delta_a * SPEED_BASE)/ delta_max ;
  speed_b = (delta_b * SPEED_BASE)/ delta_max ;
  speed_c = (delta_c * SPEED_BASE)/ delta_max ;
  //算完速度鸟----------------------------------------------------
  //电机保护
  if(a>84000)a = 84000;  if(a<-47000)a=-47000;
  if(b>94000)b = 94000;  if(b<-40000)b=-40000;
  if(c>90000)c = 90000;  if(c<-50000)c=-50000;
  if(speed_a == 0 )speed_a = 2;
  if(speed_b == 0 )speed_b = 2;
  if(speed_c == 0 )speed_c = 2;
  //赋值一戳，退出
  set_motor_distance(1, a, speed_a);
  set_motor_distance(2, b, speed_b);
  set_motor_distance(3, c, speed_c);
}

1万 · 2016-12-25 13:01

//机械控制函数，紧跟着电机控制函数放在systick中断里===============
//
void machine_control_IRQ(void)
{
  static int cycle = 0;
  int delta_x, delta_y, delta_z, delta_max, delta_abs;
  int if_justnow_done;
//
  cycle++;
  if(cycle%4 != 0)return;

  if(get_if_reach(1) == 1 && get_if_reach(2) == 1 && get_if_reach(3) ==1)
  {//若三个电机纷纷表示已经完成移动了
machine.nowx = machine.tx;
machine.nowy = machine.ty;//说明到了上次要求的地方
machine.nowz = machine.tz;
delta_x = machine.x - machine.nowx;
delta_y = machine.y - machine.nowy;//找出三个当前坐标离目标坐标的差值
delta_z = machine.z - machine.nowz;

if(delta_x > 0)delta_abs = delta_x;
            else delta_abs = -delta_x;
delta_max = delta_abs;

if(delta_y > 0)delta_abs = delta_y;
            else delta_abs = -delta_y;
if(delta_max < delta_abs)
   delta_max = delta_abs;

if(delta_z > 0)delta_abs = delta_z;
            else delta_abs = -delta_z;
if(delta_max < delta_abs)
   delta_max = delta_abs;    //以上找出最大差值

if(delta_max<20)

1万 · 2016-12-25 13:02

{
   if(if_justnow_done == 0)
      {
      machine.done_steps++;
      if_justnow_done = 1;
      }
   return;//差不多就够了
}
else if_justnow_done = 0;

delta_x = (delta_x * DELTA_BASE) / delta_max ;
delta_y = (delta_y * DELTA_BASE) / delta_max ;
delta_z = (delta_z * DELTA_BASE) / delta_max ;
machine.tx = machine.nowx + delta_x;//求出新x
machine.ty = machine.nowy + delta_y;//求出新y
machine.tz = machine.nowz + delta_z;//求出新z

solve_motor_f(machine.tx, machine.ty, machine.tz);//解出正解并给电机驱动

  }
  return;
}

1万 · 2016-12-25 13:02

//取得已完成动作数目==============================================
int get_done_steps(void)
{
  return(machine.done_steps);
}

//读出轨迹数组并执行=============================================
void read_machine_poslist(void)
{
  static int pointer = 0;
  pointer++;
  give_pos_to_machine(machine_postlist[pointer%4][0],
                  machine_postlist[pointer%4][1],
                  machine_postlist[pointer%4][2]);
}
//是否允许机器自己按轨迹跑，用于装逼============================
int if_machine_autorun(void)
{
  return machine.autorun;
}