Arduino的控制（一）：Arduino步进电机六轴机械手（油管搬）

于 2022-03-13 10:04:45 发布
阅读量3.5k 收藏 55
点赞数 6
文章标签： 单片机 嵌入式硬件 arduino
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/qq_40969179/article/details/115579102
版权 
/*
用简单的脚本移动6自由度机械手臂
*/
#include <math.h>

#define PI 3.1415926535897932384626433832795

//轴1的驱动程序
#define PUL1_PIN 39
#define DIR1_PIN 37
//轴2的驱动程序
#define PUL2_PIN 43
#define DIR2_PIN 41
//轴3的驱动程序
#define PUL3_PIN 47
#define DIR3_PIN 45
//轴4的驱动程序
#define PUL4_PIN 46
#define DIR4_PIN 48
//轴5的驱动程序
#define PUL5_PIN A6
#define DIR5_PIN A7
//轴6的驱动程序
#define PUL6_PIN A0
#define DIR6_PIN A1

//轴3,2和1的使能引脚
#define EN321_PIN 32
#define EN4_PIN A8
#define EN5_PIN A2
#define EN6_PIN 38

double curPos1 = 0.0;
double curPos2 = 0.0;
double curPos3 = 0.0;
double curPos4 = 0.0;
double curPos5 = 0.0;
double curPos6 = 0.0;

boolean PULstat1 = 0;
boolean PULstat2 = 0;
boolean PULstat3 = 0;
boolean PULstat4 = 0;
boolean PULstat5 = 0;
boolean PULstat6 = 0;

//机器人几何模型
const double dl1 = 360.0/200.0/32.0/4.8;
const double dl2 = 360.0/200.0/32.0/4.0;
const double dl3 = 360.0/200.0/32.0/5.0;
const double dl4 = 360.0/200.0/32.0/2.8;
const double dl5 = 360.0/200.0/32.0/2.1;
const double dl6 = 360.0/200.0/32.0/1.0;
const double r1 = 47.0;
const double r2 = 110.0;
const double r3 = 26.0; 
const double d1 = 133.0;
const double d3 = 0.0;
const double d4 = 117.50;
const double d6 = 28.0;

void setup()
{
   
  pinMode(PUL1_PIN, OUTPUT);
  pinMode(DIR1_PIN, OUTPUT);
  pinMode(PUL2_PIN, OUTPUT);
  pinMode(DIR2_PIN, OUTPUT);
  pinMode(PUL3_PIN, OUTPUT);
  pinMode(DIR3_PIN, OUTPUT);
  pinMode(PUL4_PIN, OUTPUT);
  pinMode(DIR4_PIN, OUTPUT);
  pinMode(PUL5_PIN, OUTPUT);
  pinMode(DIR5_PIN, OUTPUT);
  pinMode(PUL6_PIN, OUTPUT);
  pinMode(DIR6_PIN, OUTPUT);

  pinMode(EN321_PIN, OUTPUT);
  pinMode(EN4_PIN, OUTPUT);
  pinMode(EN5_PIN, OUTPUT);
  pinMode(EN6_PIN, OUTPUT);
  
  digitalWrite(PUL1_PIN, LOW); // 齿轮速比= 96/20 = 4.8
  digitalWrite(DIR1_PIN, LOW); //LOW为负向
  
  digitalWrite(PUL2_PIN, LOW); // 齿数比为4
  digitalWrite(DIR2_PIN, LOW); //LOW为负
  
  digitalWrite(PUL3_PIN, LOW); //齿速比为5
  digitalWrite(DIR3_PIN, LOW); //LOW = 反方向
  
  digitalWrite(PUL4_PIN, LOW); // 齿数比为2.8
  digitalWrite(DIR4_PIN, LOW); //LOW = positive direction
  
  digitalWrite(PUL5_PIN, LOW); // gear ratio = 42/20 = 2.1
  digitalWrite(DIR5_PIN, LOW); //LOW = positive direction
  
  digitalWrite(PUL6_PIN, LOW); // gear ratio = 1
  digitalWrite(DIR6_PIN, LOW); //LOW = positive direction

  // 所有关节关闭
  digitalWrite(EN321_PIN, HIGH);
  digitalWrite(EN4_PIN, HIGH);
  digitalWrite(EN5_PIN, HIGH);
  digitalWrite(EN6_PIN, HIGH); 
  //Serial.begin(9600);
}

void loop()
{
   
  // 打开所有关节
  delay(10000);
  digitalWrite(EN321_PIN, LOW);
  digitalWrite(EN4_PIN, LOW);
  digitalWrite(EN5_PIN, LOW);
  digitalWrite(EN6_PIN, LOW);
  delay(1000);
  // 回到坐标位置(所有关节等于0)
  // joint #2
  digitalWrite(DIR2_PIN, HIGH);
  int delValue=4000;
  int incValue = 7;
  int accRate=530;
  int totSteps=2791*2;
  for (int i=0; i < totSteps; i++)
  {
   
    if (totSteps > (2*accRate + 1))//#1
    {
   
      if (i < accRate)
      {
   
      //加速 
      delValue = delValue - incValue;//4000-7 = 3993
      } 
      else if (i > (totSteps - accRate))
     {
   
        //减速
        delValue = delValue + incValue;
      }
    } 
     else//#1
     {
   
      //无适当的加速度/减速空间
      if (i < ((totSteps - (totSteps % 2))/2))
      {
   
        //加速
        delValue = delValue - incValue;
      } 
      else if (i > ((totSteps + (totSteps % 2))/2))
      {
   
        //减速
        delValue = delValue + incValue;
      }
    }
    digitalWrite(PUL2_PIN, HIGH);
    delayMicroseconds(delValue);//delValue = 3997
    digitalWrite(PUL2_PIN, LOW);
    delayMicroseconds(delValue);
  }
  // joint #3
  digitalWrite(DIR3_PIN, HIGH);
  delValue=4000;
  incValue=7;
  accRate=530;
  totSteps=6569;
  for (int i=0; i < totSteps; i++)
  {
   
   if (totSteps > (2*accRate + 1)){
   
      if (i < accRate){
   
        //acceleration
        delValue = delValue - incValue;
      } else if (i > (totSteps - accRate)){
   
        //decceleration
        delValue = delValue + incValue;
      }
    } else {
   
      //no space for proper acceleration/decceleration
      if (i < ((totSteps - (totSteps % 2))/2)){
   
        //acceleration
        delValue = delValue - incValue;
      } else if (i > ((totSteps + (totSteps % 2))/2)){
   
        //decceleration
        delValue = delValue + incValue;
      }
    }
    digitalWrite(PUL3_PIN, HIGH);
    delayMicroseconds(delValue);
    digitalWrite(PUL3_PIN, LOW);
    delayMicroseconds(delValue);
  }
  // joint #5
  digitalWrite(DIR5_PIN, HIGH);
  delValue=4000;
  incValue=7;
  accRate=530;
  totSteps=90/dl5;
  for (int i=0; i < totSteps; i++)
  {
   
   if (totSteps > (2*accRate + 1)){
   
      if (i < accRate){
   
        //acceleration
        delValue = delValue - incValue;
      } else if (i > (totSteps - accRate)){
   
        //decceleration
        delValue = delValue + incValue;
      }
    } else {
   
      //no space for proper acceleration/decceleration
      if (i < ((totSteps - (totSteps % 2))/2)){
   
        //acceleration
        delValue = delValue - incValue;
      } else if (i > ((totSteps + (totSteps % 2))/2)){
   
        //decceleration
        delValue = delValue + incValue;
      }
    }
    digitalWrite(PUL5_PIN, HIGH);
    delayMicroseconds(delValue);
    digitalWrite(PUL5_PIN, LOW);
    delayMicroseconds(delValue);
  }
  
  //--------------------------------------------------------GoGoGo-------------------
  curPos1=0.0;
  curPos2=0.0;
  curPos3=0.0;
  curPos4=0.0;
  curPos5=90.0;
  curPos6=0.0;
  float Xhome[6]={
   164.5, 0.0, 241.0, 90.0, 180.0, -90.0}; //{x, y, z, ZYZ 欧拉角}
  
  float X1[6]={
   164.5, 0.0, 141.0, 90.0, 180.0, -90.0};
  float X11[6]={
   164.5+14.7, 35.4, 141.0, 90.0, 180.0, -90.0};
  float X12[6]={
   164.5+50.0, 50.0, 141.0, 90.0, 180.0, -90.0};
  float X13[6]={
   164.5+85.3, 35.4, 141.0, 90.0, 180.0, -90.0};
  float X14[6]={
   164.5+100.0, 0.0, 141.0, 90.0, 180.0, -90.0};
  float X15[6]={
   164.5+85.3, -35.4, 141.0, 90.0, 180.0, -90.0};
  float X16[6]={
   164.5+50.0, -50.0, 141.0, 90.0, 180.0, -90.0};
  float X17[6]={
   164.5+14.7, -35.4, 141.0, 90.0, 180.0, -90.0};

  float X18[6]={
   164.5+50.0, 0.0, 141.0, 90.0, 180.0, -90.0};
  
  float X2[6]={
   264.5, 0.0, 141.0, 0.0, 90.0, 0.0};
  float X3[6]={
   164.5, 100.0, 141.0, 90.0, 90.0, 0.0};
  float X4[6]={
   164.5, -100.0, 141.0, 90.0, -90.0, 0.0};
  
  float Jhome[6], J1[6], J11[6], J12[6], J13[6], J14[6], J15[6], J16[6], J17[6], J18[6], J2[6], J3[6], J4[6];
  InverseK(Xhome, Jhome);
  InverseK(X1, J1);
  InverseK(X11, J11);
  InverseK(X12, J12);
  InverseK(X13, J13);
  InverseK(X14, J14);
  InverseK(X15, J15);
  InverseK(X16, J16);
  InverseK(X17, J17);
  InverseK(X18, J18);
  InverseK(X2, J2);
  InverseK(X3, J3);
  InverseK(X4, J4);

  goStrightLine(Jhome, J1, 0.25e-4, 0.75e-10, 0.0, 0.0);

  float velG=0.25e-4;
  goStrightLine(J1, J11, 0.25e-4, 0.75e-10
最低0.47元/天 解锁文章
Onino_plus
关注
评论 5
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值