RoboPicker--auto arm mecanum robot with openmv camera

Use an openmv camera, 6-axis arm, and mecanum chassis to build your auto arm robot!

Apr 3, 2022

•

6082 views

•

7 respects

cars

remote control

robots

Components and supplies

OpenMV Cam M7

Arduino UNO

Arduino Mega 2560

Project description

Code

all resources

containing all code ,CADs and diagrams for RoboPicker

no description/ Read More

Latest commit to the master branch on Invalid date

Download as a zip

arm.ino

c_cpp

The code of the Robot arm.

1#include <Servo.h>                // 声明调用Servo.h库
2#include <PID_v1.h>
3#define   SERVO_NUM  6             //舵机数量
4Servo myservo[SERVO_NUM];         //创建六个舵机类
5const   byte servo_pin[SERVO_NUM] = {10, A2, A3, A0, A1, 7}; //宏定义舵机控制引脚
6
7int data[10],   cnt, l, num;
8
9int ServoPwmDuty[8] = {1500, 1500, 1500, 1500, 1500, 1500, 1500,   1500}; //PWM脉冲宽度
10int ServoPwmDutySet[8] = {1500, 1500, 1500, 1500, 1500, 1500,   1500, 1500}; //PWM脉冲宽度
11double ServoPwmDutyInc[8];   //为了速度控制，当PWM脉宽发生变化时，每2.5ms或20ms递增的PWM脉宽
12
13double   cx = 93, cy = 61, Kpx = 0.2, Kpy = 0.4,  xnow, ynow;
14double angle[10] = {1500,   1500, 1800, 800, 1500, 1500, 500};
15
16bool ServoPwmDutyHaveChange = 0;  //脉宽有变化标志位
17bool   found = 0;
18
19int ServoTime = 5000;      //舵机从当前角度运动到指定角度的时间，也就是控制速度
20
21unsigned   long t, T;
22
23void ServoSetPluseAndTime(int id, int p, int time)
24{
25  if   (id >= 0 && id < 6 && p >= 500 && p <= 2500)
26  {
27    if (time < 20)
28      time   = 20;
29    if (time > 30000)
30      time = 30000;
31    ServoPwmDutySet[id]   = p;
32    ServoTime = time;
33    ServoPwmDutyHaveChange = 1;
34  }
35}
36
37void   ServoPwmDutyCompare(void)//脉宽变化比较及速度控制
38{
39  int i;
40
41  static int ServoPwmDutyIncTimes;   //需要递增的次数
42  static bool ServoRunning = 0; //舵机正在以指定速度运动到指定的脉宽对应的位置
43  if (ServoPwmDutyHaveChange)   //停止运动并且脉宽发生变化时才进行计算      ServoRunning == 0 &&
44  {
45    ServoPwmDutyHaveChange   = 0;
46    ServoPwmDutyIncTimes = ServoTime / 20; //当每20ms调用一次ServoPwmDutyCompare()函数时用此句
47     for (i = 0; i < 6; i++)
48    {
49      //if(ServoPwmDuty[i] != ServoPwmDutySet[i])
50       {
51        if (ServoPwmDutySet[i] > ServoPwmDuty[i])
52        {
53          ServoPwmDutyInc[i]   = ServoPwmDutySet[i] - ServoPwmDuty[i];
54          ServoPwmDutyInc[i] = -ServoPwmDutyInc[i];
55         }
56        else
57        {
58          ServoPwmDutyInc[i] = ServoPwmDuty[i]   - ServoPwmDutySet[i];
59        }
60        ServoPwmDutyInc[i] /= ServoPwmDutyIncTimes;//每次递增的脉宽
61       }
62    }
63    ServoRunning = 1;  //舵机开始动作
64  }
65  if (ServoRunning)
66   {
67    ServoPwmDutyIncTimes--;
68    for (i = 0; i < 6; i++)
69    {
70      if   (ServoPwmDutyIncTimes == 0)
71      { //最后一次递增就直接将设定值赋给当前值
72        ServoPwmDuty[i]   = ServoPwmDutySet[i];
73        ServoRunning = 0; //到达设定位置，舵机停止运动
74      }
75       else
76      {
77        ServoPwmDuty[i] = ServoPwmDutySet[i] + (signed   short int)(ServoPwmDutyInc[i] * ServoPwmDutyIncTimes);
78      }
79      myservo[i].writeMicroseconds(ServoPwmDuty[i]);
80     }
81  }
82}
83void GetData()
84{
85  found = 0;
86  if (Serial.available())
87   {
88    found = 1;
89    char input = Serial.read();
90    while (input !=   '[' && input != -1)   input = Serial.read();
91    if (input != -1)
92    {
93       String s = Serial.readStringUntil(']'), numstr = "";
94      l = s.length(),   cnt = 0;
95      for (int i = 0; i < l; i++)
96      {
97        if (isDigit(s[i]))
98           numstr += s[i];
99        if (s[i] == ',' || i == l - 1)
100        {
101           data[++cnt] = numstr.toInt();
102          numstr = "";
103        }
104       }
105    }
106  }
107  xnow = data[1];
108  ynow = data[2];
109}
110void PrintData()
111{
112   for (int i = 1; i <= cnt; i++)
113  {
114    Serial.println(data[i]);
115  }
116   Serial.println("==========");
117}
118
119void compute_x(int ref, int real)
120{
121   double error = 0;
122  error = ref - real;
123  angle[1] += Kpx * error;
124  angle[1]   = min(2500, angle[1]);
125  angle[1] = max(500, angle[1]);
126}
127
128void compute_y(int   ref, int real)
129{
130  double error = 0;
131  error = ref - real;
132  angle[2]   += Kpy * error;
133  angle[2] = min(2500, angle[2]);
134  angle[2] = max(500, angle[2]);
135   if (angle[2] > 1500) angle[4] = angle[2] - 1000;
136  //else angle[3] = angle[2];
137}
138
139void   aim()
140{
141  if (abs(xnow - cx) > 2) compute_x(cx, xnow);
142  if (abs(ynow -   cy) > 2) compute_y(cy, ynow);
143  if (data[3] >= 7 && data[3] <= 9)  angle[6] =   1000;
144  else  angle[6] = 500;
145}
146void def()
147{
148  angle[1] = angle[5]   = angle[7] = 1500;
149  angle[2] = 1800;
150  angle[3] = 1500;
151  angle[4] = 800;
152}
153
154void   setup() {
155  //put your setup code here, to run once:
156  Serial.begin(9600);
157   pinMode(3, INPUT);
158  for (byte i = 0; i < SERVO_NUM; i++ ) {
159    myservo[i].attach(servo_pin[i]);     // 将10引脚与声明的舵机对象连接起来
160  }
161  def();
162  t = millis() + 20;
163  T = millis()   + 300;
164}
165
166void loop() {
167  //put your main code here, to run repeatedly:
168   GetData();
169  if (millis() > t)
170  {
171    if (digitalRead(3)) angle[6] =   500;
172    if (ServoPwmDuty[5] == 1000)
173    {
174      def();
175    }
176    if   (ServoPwmDuty[5] == 500)
177    {
178      if (!found) num++;
179      else num   = 0;
180      if (num < 10) aim();
181      ///Serial.println(1);
182    }
183    for   (int i = 0; i < 6; i++)  ServoSetPluseAndTime(i, angle[i + 1], 60);
184    ServoPwmDutyCompare();
185     t = millis() + 20;
186  }
187  //PrintData();
188  //Serial.println(ServoPwmDuty[5]);
189   //Serial.println(gripped);
190}
191

#include <Servo.h>                // 声明调用Servo.h库
#include <PID_v1.h>
#define   SERVO_NUM  6             //舵机数量
Servo myservo[SERVO_NUM];         //创建六个舵机类
const   byte servo_pin[SERVO_NUM] = {10, A2, A3, A0, A1, 7}; //宏定义舵机控制引脚

int data[10],   cnt, l, num;

int ServoPwmDuty[8] = {1500, 1500, 1500, 1500, 1500, 1500, 1500,   1500}; //PWM脉冲宽度
int ServoPwmDutySet[8] = {1500, 1500, 1500, 1500, 1500, 1500,   1500, 1500}; //PWM脉冲宽度
double ServoPwmDutyInc[8];   //为了速度控制，当PWM脉宽发生变化时，每2.5ms或20ms递增的PWM脉宽

double   cx = 93, cy = 61, Kpx = 0.2, Kpy = 0.4,  xnow, ynow;
double angle[10] = {1500,   1500, 1800, 800, 1500, 1500, 500};

bool ServoPwmDutyHaveChange = 0;  //脉宽有变化标志位
bool   found = 0;

int ServoTime = 5000;      //舵机从当前角度运动到指定角度的时间，也就是控制速度

unsigned   long t, T;

void ServoSetPluseAndTime(int id, int p, int time)
{
  if   (id >= 0 && id < 6 && p >= 500 && p <= 2500)
  {
    if (time < 20)
      time   = 20;
    if (time > 30000)
      time = 30000;
    ServoPwmDutySet[id]   = p;
    ServoTime = time;
    ServoPwmDutyHaveChange = 1;
  }
}

void   ServoPwmDutyCompare(void)//脉宽变化比较及速度控制
{
  int i;

  static int ServoPwmDutyIncTimes;   //需要递增的次数
  static bool ServoRunning = 0; //舵机正在以指定速度运动到指定的脉宽对应的位置
  if (ServoPwmDutyHaveChange)   //停止运动并且脉宽发生变化时才进行计算      ServoRunning == 0 &&
  {
    ServoPwmDutyHaveChange   = 0;
    ServoPwmDutyIncTimes = ServoTime / 20; //当每20ms调用一次ServoPwmDutyCompare()函数时用此句
     for (i = 0; i < 6; i++)
    {
      //if(ServoPwmDuty[i] != ServoPwmDutySet[i])
       {
        if (ServoPwmDutySet[i] > ServoPwmDuty[i])
        {
          ServoPwmDutyInc[i]   = ServoPwmDutySet[i] - ServoPwmDuty[i];
          ServoPwmDutyInc[i] = -ServoPwmDutyInc[i];
         }
        else
        {
          ServoPwmDutyInc[i] = ServoPwmDuty[i]   - ServoPwmDutySet[i];
        }
        ServoPwmDutyInc[i] /= ServoPwmDutyIncTimes;//每次递增的脉宽
       }
    }
    ServoRunning = 1;  //舵机开始动作
  }
  if (ServoRunning)
   {
    ServoPwmDutyIncTimes--;
    for (i = 0; i < 6; i++)
    {
      if   (ServoPwmDutyIncTimes == 0)
      { //最后一次递增就直接将设定值赋给当前值
        ServoPwmDuty[i]   = ServoPwmDutySet[i];
        ServoRunning = 0; //到达设定位置，舵机停止运动
      }
       else
      {
        ServoPwmDuty[i] = ServoPwmDutySet[i] + (signed   short int)(ServoPwmDutyInc[i] * ServoPwmDutyIncTimes);
      }
      myservo[i].writeMicroseconds(ServoPwmDuty[i]);
     }
  }
}
void GetData()
{
  found = 0;
  if (Serial.available())
   {
    found = 1;
    char input = Serial.read();
    while (input !=   '[' && input != -1)   input = Serial.read();
    if (input != -1)
    {
       String s = Serial.readStringUntil(']'), numstr = "";
      l = s.length(),   cnt = 0;
      for (int i = 0; i < l; i++)
      {
        if (isDigit(s[i]))
           numstr += s[i];
        if (s[i] == ',' || i == l - 1)
        {
           data[++cnt] = numstr.toInt();
          numstr = "";
        }
       }
    }
  }
  xnow = data[1];
  ynow = data[2];
}
void PrintData()
{
   for (int i = 1; i <= cnt; i++)
  {
    Serial.println(data[i]);
  }
   Serial.println("==========");
}

void compute_x(int ref, int real)
{
   double error = 0;
  error = ref - real;
  angle[1] += Kpx * error;
  angle[1]   = min(2500, angle[1]);
  angle[1] = max(500, angle[1]);
}

void compute_y(int   ref, int real)
{
  double error = 0;
  error = ref - real;
  angle[2]   += Kpy * error;
  angle[2] = min(2500, angle[2]);
  angle[2] = max(500, angle[2]);
   if (angle[2] > 1500) angle[4] = angle[2] - 1000;
  //else angle[3] = angle[2];
}

void   aim()
{
  if (abs(xnow - cx) > 2) compute_x(cx, xnow);
  if (abs(ynow -   cy) > 2) compute_y(cy, ynow);
  if (data[3] >= 7 && data[3] <= 9)  angle[6] =   1000;
  else  angle[6] = 500;
}
void def()
{
  angle[1] = angle[5]   = angle[7] = 1500;
  angle[2] = 1800;
  angle[3] = 1500;
  angle[4] = 800;
}

void   setup() {
  //put your setup code here, to run once:
  Serial.begin(9600);
   pinMode(3, INPUT);
  for (byte i = 0; i < SERVO_NUM; i++ ) {
    myservo[i].attach(servo_pin[i]);     // 将10引脚与声明的舵机对象连接起来
  }
  def();
  t = millis() + 20;
  T = millis()   + 300;
}

void loop() {
  //put your main code here, to run repeatedly:
   GetData();
  if (millis() > t)
  {
    if (digitalRead(3)) angle[6] =   500;
    if (ServoPwmDuty[5] == 1000)
    {
      def();
    }
    if   (ServoPwmDuty[5] == 500)
    {
      if (!found) num++;
      else num   = 0;
      if (num < 10) aim();
      ///Serial.println(1);
    }
    for   (int i = 0; i < 6; i++)  ServoSetPluseAndTime(i, angle[i + 1], 60);
    ServoPwmDutyCompare();
     t = millis() + 20;
  }
  //PrintData();
  //Serial.println(ServoPwmDuty[5]);
   //Serial.println(gripped);
}

main.ino

c_cpp

1#include "include.h"
2
3void setup()
4{
5  pinMode(53,OUTPUT);
6  InitSbus();
7  InitMotor();
8  Serial.begin(9600);
9  t = millis() + 30;
10}
11
12void  loop()
13{
14  TaskRun(); 
15}
16

#include "include.h"

void setup()
{
  pinMode(53,OUTPUT);
  InitSbus();
  InitMotor();
  Serial.begin(9600);
  t = millis() + 30;
}

void  loop()
{
  TaskRun(); 
}

main.ino

c_cpp

1#include "include.h"
2
3void setup()
4{
5  pinMode(53,OUTPUT);
6
7  InitSbus();
8  InitMotor();
9  Serial.begin(9600);
10  t = millis() + 30;
11}
12
13void
14  loop()
15{
16  TaskRun(); 
17}
18

#include "include.h"

void setup()
{
  pinMode(53,OUTPUT);

  InitSbus();
  InitMotor();
  Serial.begin(9600);
  t = millis() + 30;
}

void
  loop()
{
  TaskRun(); 
}

all resources

containing all code ,CADs and diagrams for RoboPicker

no description/ Read More

Latest commit to the master branch on Invalid date

Download as a zip

arm.ino

c_cpp

The code of the Robot arm.

1#include <Servo.h>                // 声明调用Servo.h库
2#include <PID_v1.h>
3#define  SERVO_NUM  6             //舵机数量
4Servo myservo[SERVO_NUM];         //创建六个舵机类
5const  byte servo_pin[SERVO_NUM] = {10, A2, A3, A0, A1, 7}; //宏定义舵机控制引脚
6
7int data[10],  cnt, l, num;
8
9int ServoPwmDuty[8] = {1500, 1500, 1500, 1500, 1500, 1500, 1500,  1500}; //PWM脉冲宽度
10int ServoPwmDutySet[8] = {1500, 1500, 1500, 1500, 1500, 1500,  1500, 1500}; //PWM脉冲宽度
11double ServoPwmDutyInc[8];   //为了速度控制，当PWM脉宽发生变化时，每2.5ms或20ms递增的PWM脉宽
12
13double  cx = 93, cy = 61, Kpx = 0.2, Kpy = 0.4,  xnow, ynow;
14double angle[10] = {1500,  1500, 1800, 800, 1500, 1500, 500};
15
16bool ServoPwmDutyHaveChange = 0;  //脉宽有变化标志位
17bool  found = 0;
18
19int ServoTime = 5000;      //舵机从当前角度运动到指定角度的时间，也就是控制速度
20
21unsigned  long t, T;
22
23void ServoSetPluseAndTime(int id, int p, int time)
24{
25  if  (id >= 0 && id < 6 && p >= 500 && p <= 2500)
26  {
27    if (time < 20)
28      time  = 20;
29    if (time > 30000)
30      time = 30000;
31    ServoPwmDutySet[id]  = p;
32    ServoTime = time;
33    ServoPwmDutyHaveChange = 1;
34  }
35}
36
37void  ServoPwmDutyCompare(void)//脉宽变化比较及速度控制
38{
39  int i;
40
41  static int ServoPwmDutyIncTimes;  //需要递增的次数
42  static bool ServoRunning = 0; //舵机正在以指定速度运动到指定的脉宽对应的位置
43  if (ServoPwmDutyHaveChange)  //停止运动并且脉宽发生变化时才进行计算      ServoRunning == 0 &&
44  {
45    ServoPwmDutyHaveChange  = 0;
46    ServoPwmDutyIncTimes = ServoTime / 20; //当每20ms调用一次ServoPwmDutyCompare()函数时用此句
47    for (i = 0; i < 6; i++)
48    {
49      //if(ServoPwmDuty[i] != ServoPwmDutySet[i])
50      {
51        if (ServoPwmDutySet[i] > ServoPwmDuty[i])
52        {
53          ServoPwmDutyInc[i]  = ServoPwmDutySet[i] - ServoPwmDuty[i];
54          ServoPwmDutyInc[i] = -ServoPwmDutyInc[i];
55        }
56        else
57        {
58          ServoPwmDutyInc[i] = ServoPwmDuty[i]  - ServoPwmDutySet[i];
59        }
60        ServoPwmDutyInc[i] /= ServoPwmDutyIncTimes;//每次递增的脉宽
61      }
62    }
63    ServoRunning = 1;  //舵机开始动作
64  }
65  if (ServoRunning)
66  {
67    ServoPwmDutyIncTimes--;
68    for (i = 0; i < 6; i++)
69    {
70      if  (ServoPwmDutyIncTimes == 0)
71      { //最后一次递增就直接将设定值赋给当前值
72        ServoPwmDuty[i]  = ServoPwmDutySet[i];
73        ServoRunning = 0; //到达设定位置，舵机停止运动
74      }
75      else
76      {
77        ServoPwmDuty[i] = ServoPwmDutySet[i] + (signed  short int)(ServoPwmDutyInc[i] * ServoPwmDutyIncTimes);
78      }
79      myservo[i].writeMicroseconds(ServoPwmDuty[i]);
80    }
81  }
82}
83void GetData()
84{
85  found = 0;
86  if (Serial.available())
87  {
88    found = 1;
89    char input = Serial.read();
90    while (input !=  '[' && input != -1)   input = Serial.read();
91    if (input != -1)
92    {
93      String s = Serial.readStringUntil(']'), numstr = "";
94      l = s.length(),  cnt = 0;
95      for (int i = 0; i < l; i++)
96      {
97        if (isDigit(s[i]))
98          numstr += s[i];
99        if (s[i] == ',' || i == l - 1)
100        {
101          data[++cnt] = numstr.toInt();
102          numstr = "";
103        }
104      }
105    }
106  }
107  xnow = data[1];
108  ynow = data[2];
109}
110void PrintData()
111{
112  for (int i = 1; i <= cnt; i++)
113  {
114    Serial.println(data[i]);
115  }
116  Serial.println("==========");
117}
118
119void compute_x(int ref, int real)
120{
121  double error = 0;
122  error = ref - real;
123  angle[1] += Kpx * error;
124  angle[1]  = min(2500, angle[1]);
125  angle[1] = max(500, angle[1]);
126}
127
128void compute_y(int  ref, int real)
129{
130  double error = 0;
131  error = ref - real;
132  angle[2]  += Kpy * error;
133  angle[2] = min(2500, angle[2]);
134  angle[2] = max(500, angle[2]);
135  if (angle[2] > 1500) angle[4] = angle[2] - 1000;
136  //else angle[3] = angle[2];
137}
138
139void  aim()
140{
141  if (abs(xnow - cx) > 2) compute_x(cx, xnow);
142  if (abs(ynow -  cy) > 2) compute_y(cy, ynow);
143  if (data[3] >= 7 && data[3] <= 9)  angle[6] =  1000;
144  else  angle[6] = 500;
145}
146void def()
147{
148  angle[1] = angle[5]  = angle[7] = 1500;
149  angle[2] = 1800;
150  angle[3] = 1500;
151  angle[4] = 800;
152}
153
154void  setup() {
155  //put your setup code here, to run once:
156  Serial.begin(9600);
157  pinMode(3, INPUT);
158  for (byte i = 0; i < SERVO_NUM; i++ ) {
159    myservo[i].attach(servo_pin[i]);    // 将10引脚与声明的舵机对象连接起来
160  }
161  def();
162  t = millis() + 20;
163  T = millis()  + 300;
164}
165
166void loop() {
167  //put your main code here, to run repeatedly:
168  GetData();
169  if (millis() > t)
170  {
171    if (digitalRead(3)) angle[6] =  500;
172    if (ServoPwmDuty[5] == 1000)
173    {
174      def();
175    }
176    if  (ServoPwmDuty[5] == 500)
177    {
178      if (!found) num++;
179      else num  = 0;
180      if (num < 10) aim();
181      ///Serial.println(1);
182    }
183    for  (int i = 0; i < 6; i++)  ServoSetPluseAndTime(i, angle[i + 1], 60);
184    ServoPwmDutyCompare();
185    t = millis() + 20;
186  }
187  //PrintData();
188  //Serial.println(ServoPwmDuty[5]);
189  //Serial.println(gripped);
190}
191

#include <Servo.h>                // 声明调用Servo.h库
#include <PID_v1.h>
#define  SERVO_NUM  6             //舵机数量
Servo myservo[SERVO_NUM];         //创建六个舵机类
const  byte servo_pin[SERVO_NUM] = {10, A2, A3, A0, A1, 7}; //宏定义舵机控制引脚

int data[10],  cnt, l, num;

int ServoPwmDuty[8] = {1500, 1500, 1500, 1500, 1500, 1500, 1500,  1500}; //PWM脉冲宽度
int ServoPwmDutySet[8] = {1500, 1500, 1500, 1500, 1500, 1500,  1500, 1500}; //PWM脉冲宽度
double ServoPwmDutyInc[8];   //为了速度控制，当PWM脉宽发生变化时，每2.5ms或20ms递增的PWM脉宽

double  cx = 93, cy = 61, Kpx = 0.2, Kpy = 0.4,  xnow, ynow;
double angle[10] = {1500,  1500, 1800, 800, 1500, 1500, 500};

bool ServoPwmDutyHaveChange = 0;  //脉宽有变化标志位
bool  found = 0;

int ServoTime = 5000;      //舵机从当前角度运动到指定角度的时间，也就是控制速度

unsigned  long t, T;

void ServoSetPluseAndTime(int id, int p, int time)
{
  if  (id >= 0 && id < 6 && p >= 500 && p <= 2500)
  {
    if (time < 20)
      time  = 20;
    if (time > 30000)
      time = 30000;
    ServoPwmDutySet[id]  = p;
    ServoTime = time;
    ServoPwmDutyHaveChange = 1;
  }
}

void  ServoPwmDutyCompare(void)//脉宽变化比较及速度控制
{
  int i;

  static int ServoPwmDutyIncTimes;  //需要递增的次数
  static bool ServoRunning = 0; //舵机正在以指定速度运动到指定的脉宽对应的位置
  if (ServoPwmDutyHaveChange)  //停止运动并且脉宽发生变化时才进行计算      ServoRunning == 0 &&
  {
    ServoPwmDutyHaveChange  = 0;
    ServoPwmDutyIncTimes = ServoTime / 20; //当每20ms调用一次ServoPwmDutyCompare()函数时用此句
    for (i = 0; i < 6; i++)
    {
      //if(ServoPwmDuty[i] != ServoPwmDutySet[i])
      {
        if (ServoPwmDutySet[i] > ServoPwmDuty[i])
        {
          ServoPwmDutyInc[i]  = ServoPwmDutySet[i] - ServoPwmDuty[i];
          ServoPwmDutyInc[i] = -ServoPwmDutyInc[i];
        }
        else
        {
          ServoPwmDutyInc[i] = ServoPwmDuty[i]  - ServoPwmDutySet[i];
        }
        ServoPwmDutyInc[i] /= ServoPwmDutyIncTimes;//每次递增的脉宽
      }
    }
    ServoRunning = 1;  //舵机开始动作
  }
  if (ServoRunning)
  {
    ServoPwmDutyIncTimes--;
    for (i = 0; i < 6; i++)
    {
      if  (ServoPwmDutyIncTimes == 0)
      { //最后一次递增就直接将设定值赋给当前值
        ServoPwmDuty[i]  = ServoPwmDutySet[i];
        ServoRunning = 0; //到达设定位置，舵机停止运动
      }
      else
      {
        ServoPwmDuty[i] = ServoPwmDutySet[i] + (signed  short int)(ServoPwmDutyInc[i] * ServoPwmDutyIncTimes);
      }
      myservo[i].writeMicroseconds(ServoPwmDuty[i]);
    }
  }
}
void GetData()
{
  found = 0;
  if (Serial.available())
  {
    found = 1;
    char input = Serial.read();
    while (input !=  '[' && input != -1)   input = Serial.read();
    if (input != -1)
    {
      String s = Serial.readStringUntil(']'), numstr = "";
      l = s.length(),  cnt = 0;
      for (int i = 0; i < l; i++)
      {
        if (isDigit(s[i]))
          numstr += s[i];
        if (s[i] == ',' || i == l - 1)
        {
          data[++cnt] = numstr.toInt();
          numstr = "";
        }
      }
    }
  }
  xnow = data[1];
  ynow = data[2];
}
void PrintData()
{
  for (int i = 1; i <= cnt; i++)
  {
    Serial.println(data[i]);
  }
  Serial.println("==========");
}

void compute_x(int ref, int real)
{
  double error = 0;
  error = ref - real;
  angle[1] += Kpx * error;
  angle[1]  = min(2500, angle[1]);
  angle[1] = max(500, angle[1]);
}

void compute_y(int  ref, int real)
{
  double error = 0;
  error = ref - real;
  angle[2]  += Kpy * error;
  angle[2] = min(2500, angle[2]);
  angle[2] = max(500, angle[2]);
  if (angle[2] > 1500) angle[4] = angle[2] - 1000;
  //else angle[3] = angle[2];
}

void  aim()
{
  if (abs(xnow - cx) > 2) compute_x(cx, xnow);
  if (abs(ynow -  cy) > 2) compute_y(cy, ynow);
  if (data[3] >= 7 && data[3] <= 9)  angle[6] =  1000;
  else  angle[6] = 500;
}
void def()
{
  angle[1] = angle[5]  = angle[7] = 1500;
  angle[2] = 1800;
  angle[3] = 1500;
  angle[4] = 800;
}

void  setup() {
  //put your setup code here, to run once:
  Serial.begin(9600);
  pinMode(3, INPUT);
  for (byte i = 0; i < SERVO_NUM; i++ ) {
    myservo[i].attach(servo_pin[i]);    // 将10引脚与声明的舵机对象连接起来
  }
  def();
  t = millis() + 20;
  T = millis()  + 300;
}

void loop() {
  //put your main code here, to run repeatedly:
  GetData();
  if (millis() > t)
  {
    if (digitalRead(3)) angle[6] =  500;
    if (ServoPwmDuty[5] == 1000)
    {
      def();
    }
    if  (ServoPwmDuty[5] == 500)
    {
      if (!found) num++;
      else num  = 0;
      if (num < 10) aim();
      ///Serial.println(1);
    }
    for  (int i = 0; i < 6; i++)  ServoSetPluseAndTime(i, angle[i + 1], 60);
    ServoPwmDutyCompare();
    t = millis() + 20;
  }
  //PrintData();
  //Serial.println(ServoPwmDuty[5]);
  //Serial.println(gripped);
}

Downloadable files

mega shield PCB file

The PCB file of the shield board of mega2560.

mega shield PCB file

Documentation

Computer brace

Step file of computer brace (which can be used to support a laptop on RoboPicker)

Computer brace

upper chassis

Step file of the upper chassis

upper chassis

lower chassis

Step file of the lower chassis.

lower chassis

Step file of the lower chassis.

lower chassis

Computer brace

Step file of computer brace (which can be used to support a laptop on RoboPicker)

Computer brace

upper chassis

Step file of the upper chassis

upper chassis

Comments

Only logged in users can leave comments

wzzzq

0 Followers

•

0 Projects

Intro