机械手舵机动作组编号及程序简单化

[咸鱼yyq]

舵机的动作组编号也被转换成了角度，而且程序复杂了。。大家好好呢把我改改哦。。谢了。。。

程序代码

#include <Servo.h>
//定义变量
Servo servo[6];
int se[6]= {
  80, 110, 80, 90, 100, 130
};//舵机初始化数组
int i=0, t=1000;//t为延时

String data="";
int mark=0, v[6]= {
  80, 110, 80, 90, 100, 130
}
, over=0;
boolean tag[6]= {
  1, 1, 1, 1, 1, 1
};//初始化标志位数组,不相等

//舵机初始化
void datadeal()
{
  int j = 0;
  while (Serial.available () > 0)
  {
    delay(20);
    data+=char(Serial.read());
    mark=1;
  }
  if (mark==1)
  {
    Serial.println(data);
    Serial.println(data.length());
    int A[6]= {
      0
    };
    for (int i = 0; i < data.length() ; i++)
    {

      if (data[i] == ',')
      {
        j++;
      }
      else
      { 
        A[j] = A[j] * 10 + (data - '0');
        v[j] =A[j];
        // v[j] =data*10+(data - '0');
      }
    }
    data=String("");
    for (int i = 0; i < 6; i++)
    {
      Serial.print("Servo");
      Serial.print(i);
      Serial.print(" = ");
      Serial.println(v);
    }
    mark = 0;
    over=1;
  }
}


//单个舵机控制函数
void fun(int a, int b, int i)
{
  if (a!=b)
  {
    if (a<b)
    {
      while (a<b)
      {
        a+=5;
        servo.write(a);
        se=a;
        Serial.println(a);
        delay(t);
      }
    }
    else if (a>b)
    {
      while (a>b) {
        a-=5;
        servo.write(a);
        se=a;
        Serial.println(a);
        delay(t);
      }
    }
  }

  else 
    return;
}
//舵机动作函数
void Action(int a, int b, int i)
{ 
  if (a<b)
  {
    a+=5;
    servo.write(a);
    se=a;
    Serial.println(a);
    delay(t);
  }
  else if (a>b)
  { 
    a-=5;
    servo.write(a);
    se=a;
    Serial.println(a);
    delay(t);
  }
}

void setup()
{
  servo[0].attach(22, 380, 1200);
  servo[1].attach(23, 380, 1200);
  servo[2].attach(24, 380, 1200);
  servo[3].attach(25, 380, 1200);
  servo[4].attach(26, 380, 1200);
  servo[5].attach(27, 380, 1200);
  for (i=0;i<6;i++)
    servo.write(se);
  delay(2000);
  Serial.begin(9600);//设置波特率
  Serial.println("Ready");
}
void loop()
{
  datadeal();
  //单个舵机控制函数
  if (over==1)
  {

    //boolean tag[6]={1,1,1,1,1,1};//初始化标志位数组,不相等
    //判断两个数组元素是否相等
    for (int i=0;i<6;i++)
    {
      //相等则置标志位为0
      if (se[i]==v[i])//对应元素相等则tag=0
        tag[i]=0;
    }
    int sum=0;
    for (int j=0;j<6;j++)
      sum+=tag[j];
    if (sum>0)//当tag的和大于0时，有对应元素需改变，调用循环
    {
      for (int num=0;num<6;num++)
      {
        Serial.println(v[num]); 
        fun(se[num], v[num], num);//函数调用6次
        tag[num]=0;
      }
    }
    //}
    over=0;
    //舵机连续动作组控制
    while (Serial.available () > 0)
    {
      delay(20);
      char action=Serial.read();

      int a[6]= {
        80, 175, 80, 90, 100, 130
      };//下降
      int b[6]= {
        80, 110, 175, 90, 100, 130
      };//上升

      switch(action)
      {
      case 'a'://下降

        while (sum>0)//当tag的和大于0时，有对应元素需改变，调用循环
        {
          for (int num=0;num<6;num++)
          {
            Serial.println(v[num]); 
            delay (100);
            Action(se[num], a[num], num);//函数调用6次
            //tag[num]=0;
            delay(50);
          }
          for (int i=0;i<6;i++)
          {
            //相等则置标志位为0
            if (se[i]==v[i])//对应元素相等则tag=0
              tag[i]=0;
          }
          int sum=0;
          for (int j=0;j<6;j++)
            sum+=tag[j];
          if (sum==0)
            break;
        }
        break;
      case '2'://上升
        break;
        //Action();
      }
    }
  }
}

[ChocolateUniverse]

请把问题描述一下。

[ChocolateUniverse]

se开头定义成全局变量，数组。后面又当做貌似标识来用，我没太看懂你的意图。
另外舵机初始化，一般放在setup函数里。

[咸鱼yyq]

是两个数组之间的比较。。。se【6】是舵机的初始角度，v【6】是待输入的舵机角度，所以两个进行比较。。后面的下降上升的数组是机械手执行这两个动作必须转动的角度。。我弄成数组，方便比较

[咸鱼yyq]

se开头定义成全局变量，数组。后面又当做貌似标识来用，我没太看懂你的意图。
另外舵机初始化，一般放在setup函数里。

是两个数组之间的比较。。。se【6】是舵机的初始角度，v【6】是待输入的舵机角度，所以两个进行比较。。后面的下降上升的数组是机械手执行这两个动作必须转动的角度。。我弄成数组，方便比较

[ChocolateUniverse]

先搞懂数组的用法

[咸鱼yyq]

先搞懂数组的用法

帖子有问题啊。。。
我的是这样的
if(se==v)//对应元素相等则tag=0
tag=0;

[suoma]

先搞懂数组的用法