Gesture Controlled Robotic Arm

Robotic arm mimics human arm movement with MEMS and e-compass based and transmitted thru Bluetooth.

Aug 30, 2017

•

6751 views

•

6 respects

remote control

robots

Components and supplies

ST Bluenrg-132

Arduino Nano R3

Tools and machines

ST Sensor Tile

Apps and platforms

Arduino IDE

Project description

Code

s_control.ino

c_cpp

Takes command in serial port to control servo to move to specified angle position e.g. fxa45 moves A servo to 45 degree fxp1 all servo to 45 degree fxp2 a pick and place demostration

1//#define DEBUG_MOVE_STEP
2class FRobot {
3  public:
4    FRobot()   {
5      
6      delay_time = 20;
7    }
8    void home() {}
9    
10     int read(int i) {
11      return servo[i].read();
12    }
13    void set_delay(int   t) {
14      delay_time = t;
15    }
16    int move_step(int which, int target)   {
17      int pos;
18      pos = servo[which].read();
19      #ifdef DEBUG_MOVE_STEP
20       Serial.print(which, DEC); Serial.print(" Target"); Serial.print(target,DEC);
21       Serial.print("cur pos"); Serial.println(pos,DEC);
22      #endif
23      if   (pos < target)
24        servo[which].write(pos + 1);
25      else if (pos > target)
26         servo[which].write(pos - 1);
27      else if (pos == target)
28        return   1;
29      return 0;
30    }
31    void motion(unsigned char m[][4], int steps)
32     {
33      int i, j, ret = 0;;
34      for (i = 0; i < steps; i++) {
35        Serial.print("motion:   ");
36        Serial.println(i, DEC);
37        do {
38          ret = 0;
39           for (j = 0; j < 4; j++)  ret += move_step(j, m[i][j]);
40          delay(delay_time);
41         } while (ret < 4);
42      }
43      Serial.println("motion end");
44     }
45    void Execute(int val[])
46    {
47      int j, ret;
48      do {
49         ret = 0;
50        for (j = 0; j < 4; j++)  ret += move_step(j, val[j]);
51         delay(delay_time);
52      } while (ret < 4);
53      Serial.print("move   to "); Serial.print(val[0]); Serial.print(val[1]); Serial.print(val[2]); Serial.println(val[3]);
54     }
55
56  public:
57    Servo *servo;
58    int delay_time;
59};
60
61class   FRobot_Arm : public FRobot {
62  public:
63    FRobot_Arm(): FRobot() {
64      servo   = s;
65      
66    }
67    void attach(int p1, int p2, int p3, int p4){
68       servo[0].attach(p1);
69      servo[1].attach(p2);
70      servo[2].attach(p3);
71       servo[3].attach(p4);
72    }
73    void home(int a, int b, int c, int d)   {
74      servo[0].write(a);
75      servo[1].write(b);
76      servo[2].write(c);
77       servo[3].write(d);
78    }
79  private:
80    Servo s[4];
81
82};
83
84

//#define DEBUG_MOVE_STEP
class FRobot {
  public:
    FRobot()   {
      
      delay_time = 20;
    }
    void home() {}
    
     int read(int i) {
      return servo[i].read();
    }
    void set_delay(int   t) {
      delay_time = t;
    }
    int move_step(int which, int target)   {
      int pos;
      pos = servo[which].read();
      #ifdef DEBUG_MOVE_STEP
       Serial.print(which, DEC); Serial.print(" Target"); Serial.print(target,DEC);
       Serial.print("cur pos"); Serial.println(pos,DEC);
      #endif
      if   (pos < target)
        servo[which].write(pos + 1);
      else if (pos > target)
         servo[which].write(pos - 1);
      else if (pos == target)
        return   1;
      return 0;
    }
    void motion(unsigned char m[][4], int steps)
     {
      int i, j, ret = 0;;
      for (i = 0; i < steps; i++) {
        Serial.print("motion:   ");
        Serial.println(i, DEC);
        do {
          ret = 0;
           for (j = 0; j < 4; j++)  ret += move_step(j, m[i][j]);
          delay(delay_time);
         } while (ret < 4);
      }
      Serial.println("motion end");
     }
    void Execute(int val[])
    {
      int j, ret;
      do {
         ret = 0;
        for (j = 0; j < 4; j++)  ret += move_step(j, val[j]);
         delay(delay_time);
      } while (ret < 4);
      Serial.print("move   to "); Serial.print(val[0]); Serial.print(val[1]); Serial.print(val[2]); Serial.println(val[3]);
     }

  public:
    Servo *servo;
    int delay_time;
};

class   FRobot_Arm : public FRobot {
  public:
    FRobot_Arm(): FRobot() {
      servo   = s;
      
    }
    void attach(int p1, int p2, int p3, int p4){
       servo[0].attach(p1);
      servo[1].attach(p2);
      servo[2].attach(p3);
       servo[3].attach(p4);
    }
    void home(int a, int b, int c, int d)   {
      servo[0].write(a);
      servo[1].write(b);
      servo[2].write(c);
       servo[3].write(d);
    }
  private:
    Servo s[4];

};

s_control.ino

c_cpp

Takes command in serial port to control servo to move to specified angle position e.g. fxa45 moves A servo to 45 degree fxp1 all servo to 45 degree fxp2 a pick and place demostration

1//#define DEBUG_MOVE_STEP
2class FRobot {
3  public:
4    FRobot()  {
5      
6      delay_time = 20;
7    }
8    void home() {}
9    
10    int read(int i) {
11      return servo[i].read();
12    }
13    void set_delay(int  t) {
14      delay_time = t;
15    }
16    int move_step(int which, int target)  {
17      int pos;
18      pos = servo[which].read();
19      #ifdef DEBUG_MOVE_STEP
20      Serial.print(which, DEC); Serial.print(" Target"); Serial.print(target,DEC);
21      Serial.print("cur pos"); Serial.println(pos,DEC);
22      #endif
23      if  (pos < target)
24        servo[which].write(pos + 1);
25      else if (pos > target)
26        servo[which].write(pos - 1);
27      else if (pos == target)
28        return  1;
29      return 0;
30    }
31    void motion(unsigned char m[][4], int steps)
32    {
33      int i, j, ret = 0;;
34      for (i = 0; i < steps; i++) {
35        Serial.print("motion:  ");
36        Serial.println(i, DEC);
37        do {
38          ret = 0;
39          for (j = 0; j < 4; j++)  ret += move_step(j, m[i][j]);
40          delay(delay_time);
41        } while (ret < 4);
42      }
43      Serial.println("motion end");
44    }
45    void Execute(int val[])
46    {
47      int j, ret;
48      do {
49        ret = 0;
50        for (j = 0; j < 4; j++)  ret += move_step(j, val[j]);
51        delay(delay_time);
52      } while (ret < 4);
53      Serial.print("move  to "); Serial.print(val[0]); Serial.print(val[1]); Serial.print(val[2]); Serial.println(val[3]);
54    }
55
56  public:
57    Servo *servo;
58    int delay_time;
59};
60
61class  FRobot_Arm : public FRobot {
62  public:
63    FRobot_Arm(): FRobot() {
64      servo  = s;
65      
66    }
67    void attach(int p1, int p2, int p3, int p4){
68      servo[0].attach(p1);
69      servo[1].attach(p2);
70      servo[2].attach(p3);
71      servo[3].attach(p4);
72    }
73    void home(int a, int b, int c, int d)  {
74      servo[0].write(a);
75      servo[1].write(b);
76      servo[2].write(c);
77      servo[3].write(d);
78    }
79  private:
80    Servo s[4];
81
82};
83
84

//#define DEBUG_MOVE_STEP
class FRobot {
  public:
    FRobot()  {
      
      delay_time = 20;
    }
    void home() {}
    
    int read(int i) {
      return servo[i].read();
    }
    void set_delay(int  t) {
      delay_time = t;
    }
    int move_step(int which, int target)  {
      int pos;
      pos = servo[which].read();
      #ifdef DEBUG_MOVE_STEP
      Serial.print(which, DEC); Serial.print(" Target"); Serial.print(target,DEC);
      Serial.print("cur pos"); Serial.println(pos,DEC);
      #endif
      if  (pos < target)
        servo[which].write(pos + 1);
      else if (pos > target)
        servo[which].write(pos - 1);
      else if (pos == target)
        return  1;
      return 0;
    }
    void motion(unsigned char m[][4], int steps)
    {
      int i, j, ret = 0;;
      for (i = 0; i < steps; i++) {
        Serial.print("motion:  ");
        Serial.println(i, DEC);
        do {
          ret = 0;
          for (j = 0; j < 4; j++)  ret += move_step(j, m[i][j]);
          delay(delay_time);
        } while (ret < 4);
      }
      Serial.println("motion end");
    }
    void Execute(int val[])
    {
      int j, ret;
      do {
        ret = 0;
        for (j = 0; j < 4; j++)  ret += move_step(j, val[j]);
        delay(delay_time);
      } while (ret < 4);
      Serial.print("move  to "); Serial.print(val[0]); Serial.print(val[1]); Serial.print(val[2]); Serial.println(val[3]);
    }

  public:
    Servo *servo;
    int delay_time;
};

class  FRobot_Arm : public FRobot {
  public:
    FRobot_Arm(): FRobot() {
      servo  = s;
      
    }
    void attach(int p1, int p2, int p3, int p4){
      servo[0].attach(p1);
      servo[1].attach(p2);
      servo[2].attach(p3);
      servo[3].attach(p4);
    }
    void home(int a, int b, int c, int d)  {
      servo[0].write(a);
      servo[1].write(b);
      servo[2].write(c);
      servo[3].write(d);
    }
  private:
    Servo s[4];

};

Arduino nano robot arm servo control

c_cpp

It takes commands from serial port and execute servo movement to the angle specified

1#include <Servo.h>
2#include "FRobot.h"
3
4//UART send 1~9==>20~180  degree
5
6int val, val2, temp;
7void Execute(char c, int v1);
8void Execute(int  v[]);
9void motion(unsigned char m[][4], int steps);
10
11unsigned char home_pos[1][4]  = { 45, 45, 45, 45 };
12unsigned char hello_dance[6][4] = { {45, 0, 0, 80}, {30,  0, 0, 80},
13  {90, 10, 10, 10}, {30, 10, 10, 80},
14  {30, 45, 90, 80}, {45,  45, 45, 45}
15};
16unsigned char pickup_dance[][4] = { {110, 95, 140, 80}, {110,  95, 140, 80}, {110, 95, 140, 30},
17  {110, 70, 90, 30}, {180, 70, 90, 30},
18  {180, 95, 90, 30}, {180, 95, 150, 30},
19  {180, 100, 150, 80}, {180, 70, 90,  80}, {45, 45, 45, 45}
20};
21FRobot_Arm arm;
22void setup()
23{
24  int i;
25  pinMode(13, OUTPUT);
26  Serial.begin(9600);//设置波特率为9600
27  Serial.println("servo=o_seral_simple  ready" ) ;
28  arm.attach(10, 9, 8, 7);
29  arm.home(45, 45, 45, 45);
30  Serial.println("home");
31}
32int  input_cnt = 0;
33char command;
34const int Servo_Limit[4][2] = { { 0, 180}, {45,  150}, {0, 180}, {10, 80}};
35void loop()
36{
37  int val[4], i;
38  char c;
39  
40  if (!Serial.available())
41    return;
42 // Serial.print("input cnt  "); Serial.println(input_cnt, DEC);
43  if (input_cnt > 2) {
44    input_cnt  = 0;
45    val[0] = Serial.parseInt();
46    constrain(val[0], Servo_Limit[0][0],  Servo_Limit[0][1]);
47    if (command == 'x') {
48      for (i = 1; i < 4; i++)  {
49        val[i] = Serial.parseInt();
50        constrain(val[i], Servo_Limit[i][0],  Servo_Limit[i][1]);
51      }
52      arm.Execute(val);
53    } else
54      Execute(command,  val[0]);
55    return;
56  }
57  c = Serial.read();
58  input_cnt ++;
59  switch  (input_cnt) {
60    case 1:
61      if (c != 'f')  input_cnt = 0;
62      break;
63    case 2:
64      if (c != 'x') input_cnt = 0;
65      break;
66    case 3:
67      command = c;
68  //    Serial.print("command = ");
69   //   Serial.println(command,  DEC);
70      break;
71  }
72}
73void Execute(char c, int v1)
74{
75  char  buf[64];
76  switch (c) {
77    case 'S': case 's':
78      sprintf(buf, "A:  %d B: %d C: %d D: %d, ",
79              arm.read(0), arm.read(1), arm.read(2),  arm.read(3));
80      Serial.println(buf);
81      break;
82    case 'b': case  'c': case 'd': case'a':
83      sprintf(buf, "move %c to %d", c, v1);
84      Serial.println(buf);
85      while (!arm.move_step(c - 97, v1));            break; //a ASCII = 97
86
87    case 'T':
88    case 't':
89      arm.set_delay(v1);
90      break;
91    case  'p':
92      if (v1 == 1) {
93        arm.motion(home_pos, 1);
94      } else  if (v1 == 2) {
95        arm.motion(pickup_dance, 10);
96      } else if (v1 ==  3) arm.motion(hello_dance, 6);
97      break;
98    case 'L':
99      if (v1  > 0)
100        digitalWrite(13, HIGH);
101      else
102        digitalWrite(13,  LOW);
103      break;
104  }
105
106 // Serial.print(c, DEC);
107 // Serial.print("  v1="); Serial.println(v1, DEC);
108}
109
110
111//#define DEBUG_MOVE_STEP
112class  FRobot {
113  public:
114    FRobot() {
115      
116      delay_time = 20;
117    }
118    void home() {}
119    
120    int read(int i) {
121      return servo[i].read();
122    }
123    void set_delay(int t) {
124      delay_time = t;
125    }
126    int  move_step(int which, int target) {
127      int pos;
128      pos = servo[which].read();
129      #ifdef DEBUG_MOVE_STEP
130      Serial.print(which, DEC); Serial.print("  Target"); Serial.print(target,DEC);
131      Serial.print("cur pos"); Serial.println(pos,DEC);
132      #endif
133      if (pos < target)
134        servo[which].write(pos + 1);
135      else if (pos > target)
136        servo[which].write(pos - 1);
137      else  if (pos == target)
138        return 1;
139      return 0;
140    }
141    void  motion(unsigned char m[][4], int steps)
142    {
143      int i, j, ret = 0;;
144      for (i = 0; i < steps; i++) {
145        Serial.print("motion: ");
146        Serial.println(i,  DEC);
147        do {
148          ret = 0;
149          for (j = 0; j < 4; j++)  ret += move_step(j, m[i][j]);
150          delay(delay_time);
151        } while  (ret < 4);
152      }
153      Serial.println("motion end");
154    }
155    void  Execute(int val[])
156    {
157      int j, ret;
158      do {
159        ret =  0;
160        for (j = 0; j < 4; j++)  ret += move_step(j, val[j]);
161        delay(delay_time);
162      } while (ret < 4);
163      Serial.print("move to "); Serial.print(val[0]);  Serial.print(val[1]); Serial.print(val[2]); Serial.println(val[3]);
164    }
165
166  public:
167    Servo *servo;
168    int delay_time;
169};
170
171class FRobot_Arm  : public FRobot {
172  public:
173    FRobot_Arm(): FRobot() {
174      servo =  s;
175      
176    }
177    void attach(int p1, int p2, int p3, int p4){
178      servo[0].attach(p1);
179      servo[1].attach(p2);
180      servo[2].attach(p3);
181      servo[3].attach(p4);
182    }
183    void home(int a, int b, int c, int d) {
184      servo[0].write(a);
185      servo[1].write(b);
186      servo[2].write(c);
187      servo[3].write(d);
188    }
189  private:
190    Servo s[4];
191
192};
193

#include <Servo.h>
#include "FRobot.h"

//UART send 1~9==>20~180  degree

int val, val2, temp;
void Execute(char c, int v1);
void Execute(int  v[]);
void motion(unsigned char m[][4], int steps);

unsigned char home_pos[1][4]  = { 45, 45, 45, 45 };
unsigned char hello_dance[6][4] = { {45, 0, 0, 80}, {30,  0, 0, 80},
  {90, 10, 10, 10}, {30, 10, 10, 80},
  {30, 45, 90, 80}, {45,  45, 45, 45}
};
unsigned char pickup_dance[][4] = { {110, 95, 140, 80}, {110,  95, 140, 80}, {110, 95, 140, 30},
  {110, 70, 90, 30}, {180, 70, 90, 30},
  {180, 95, 90, 30}, {180, 95, 150, 30},
  {180, 100, 150, 80}, {180, 70, 90,  80}, {45, 45, 45, 45}
};
FRobot_Arm arm;
void setup()
{
  int i;
  pinMode(13, OUTPUT);
  Serial.begin(9600);//设置波特率为9600
  Serial.println("servo=o_seral_simple  ready" ) ;
  arm.attach(10, 9, 8, 7);
  arm.home(45, 45, 45, 45);
  Serial.println("home");
}
int  input_cnt = 0;
char command;
const int Servo_Limit[4][2] = { { 0, 180}, {45,  150}, {0, 180}, {10, 80}};
void loop()
{
  int val[4], i;
  char c;
  
  if (!Serial.available())
    return;
 // Serial.print("input cnt  "); Serial.println(input_cnt, DEC);
  if (input_cnt > 2) {
    input_cnt  = 0;
    val[0] = Serial.parseInt();
    constrain(val[0], Servo_Limit[0][0],  Servo_Limit[0][1]);
    if (command == 'x') {
      for (i = 1; i < 4; i++)  {
        val[i] = Serial.parseInt();
        constrain(val[i], Servo_Limit[i][0],  Servo_Limit[i][1]);
      }
      arm.Execute(val);
    } else
      Execute(command,  val[0]);
    return;
  }
  c = Serial.read();
  input_cnt ++;
  switch  (input_cnt) {
    case 1:
      if (c != 'f')  input_cnt = 0;
      break;
    case 2:
      if (c != 'x') input_cnt = 0;
      break;
    case 3:
      command = c;
  //    Serial.print("command = ");
   //   Serial.println(command,  DEC);
      break;
  }
}
void Execute(char c, int v1)
{
  char  buf[64];
  switch (c) {
    case 'S': case 's':
      sprintf(buf, "A:  %d B: %d C: %d D: %d, ",
              arm.read(0), arm.read(1), arm.read(2),  arm.read(3));
      Serial.println(buf);
      break;
    case 'b': case  'c': case 'd': case'a':
      sprintf(buf, "move %c to %d", c, v1);
      Serial.println(buf);
      while (!arm.move_step(c - 97, v1));            break; //a ASCII = 97

    case 'T':
    case 't':
      arm.set_delay(v1);
      break;
    case  'p':
      if (v1 == 1) {
        arm.motion(home_pos, 1);
      } else  if (v1 == 2) {
        arm.motion(pickup_dance, 10);
      } else if (v1 ==  3) arm.motion(hello_dance, 6);
      break;
    case 'L':
      if (v1  > 0)
        digitalWrite(13, HIGH);
      else
        digitalWrite(13,  LOW);
      break;
  }

 // Serial.print(c, DEC);
 // Serial.print("  v1="); Serial.println(v1, DEC);
}


//#define DEBUG_MOVE_STEP
class  FRobot {
  public:
    FRobot() {
      
      delay_time = 20;
    }
    void home() {}
    
    int read(int i) {
      return servo[i].read();
    }
    void set_delay(int t) {
      delay_time = t;
    }
    int  move_step(int which, int target) {
      int pos;
      pos = servo[which].read();
      #ifdef DEBUG_MOVE_STEP
      Serial.print(which, DEC); Serial.print("  Target"); Serial.print(target,DEC);
      Serial.print("cur pos"); Serial.println(pos,DEC);
      #endif
      if (pos < target)
        servo[which].write(pos + 1);
      else if (pos > target)
        servo[which].write(pos - 1);
      else  if (pos == target)
        return 1;
      return 0;
    }
    void  motion(unsigned char m[][4], int steps)
    {
      int i, j, ret = 0;;
      for (i = 0; i < steps; i++) {
        Serial.print("motion: ");
        Serial.println(i,  DEC);
        do {
          ret = 0;
          for (j = 0; j < 4; j++)  ret += move_step(j, m[i][j]);
          delay(delay_time);
        } while  (ret < 4);
      }
      Serial.println("motion end");
    }
    void  Execute(int val[])
    {
      int j, ret;
      do {
        ret =  0;
        for (j = 0; j < 4; j++)  ret += move_step(j, val[j]);
        delay(delay_time);
      } while (ret < 4);
      Serial.print("move to "); Serial.print(val[0]);  Serial.print(val[1]); Serial.print(val[2]); Serial.println(val[3]);
    }

  public:
    Servo *servo;
    int delay_time;
};

class FRobot_Arm  : public FRobot {
  public:
    FRobot_Arm(): FRobot() {
      servo =  s;
      
    }
    void attach(int p1, int p2, int p3, int p4){
      servo[0].attach(p1);
      servo[1].attach(p2);
      servo[2].attach(p3);
      servo[3].attach(p4);
    }
    void home(int a, int b, int c, int d) {
      servo[0].write(a);
      servo[1].write(b);
      servo[2].write(c);
      servo[3].write(d);
    }
  private:
    Servo s[4];

};

Comments

Only logged in users can leave comments

Teddy99

0 Followers

•

0 Projects

Work attribution

Intro