Joystick Controller for MeArm Robot - Recording Coordinates

Uses 2 analog joysticks to control 4 servos without of using additional board. Have 3 modes: manual, recording of coordinates and autorun.

Nov 25, 2018

•

43067 views

•

35 respects

Components and supplies

5 mm LED: Red

Analog joystick (Generic)

Micro servo SG90

Resistor 10k ohm

Resistor 680 ohm

Arduino UNO

Apps and platforms

Arduino IDE

Project description

Code

Arduiono Joystick controller for MeArm ® robot with recording of coordinates. (version 1.3.1)

arduino

Use Arduino IDE for programming Arduiono UNO base board via USB port.

1/* meArm analog joysticks version 1.3.1 - UtilStudio.com Dec 2018
2   Uses two analogue joysticks and four servos.
3
4   In version 1.3 was improved  recording of coordinates.
5   Some bugs was removed.
6
7   First joystick  moves gripper forwards, backwards, left and right,
8   button start/stop recording  positions.
9
10   Second joystick moves gripper up, down, and closes and opens,
11   button start/stop playing recorded positions.
12   Press button for 2 seconds  to autoplay.
13
14   Pins:
15   Arduino    Stick1    Stick2    Base   Shoulder  Elbow    Gripper   Record/
16      GND       GND       GND    Brown     Brown   Brown     Brown    Auto play
17       5V       VCC       VCC      Red       Red     Red       Red    LED
18       A0       HOR
19       A1       VER
20       PD2      BUTT
21       A2                 HOR
22       A3                 VER
23       PD3                BUTT
24       11                       Yellow
25       10                                 Yellow
26        9                                         Yellow
27        6                                                   Yellow
28       PD4                                                            X
29*/
30#include  <Servo.h>
31
32bool repeatePlaying = false; /* Repeatedly is running recorded  cycle */
33int delayBetweenCycles = 2000; /* Delay between cycles */
34
35int  basePin = 11;       /* Base servo */
36int shoulderPin = 10;   /* Shoulder servo  */
37int elbowPin = 9;       /* Elbow servo */
38int gripperPin = 6;     /* Gripper  servo */
39
40int xdirPin = 0;        /* Base - joystick1*/
41int ydirPin = 1;        /* Shoulder - joystick1 */
42int zdirPin = 3;        /* Elbow - joystick2  */
43int gdirPin = 2;        /* Gripper - joystick2 */
44
45//int pinRecord =  A4;     /* Button record - backward compatibility */
46//int pinPlay = A5;       /*  Button play  - backward compatibility */
47int pinRecord = PD2;     /* Button record  - recommended (A4 is deprecated, will by used for additional joystick) */
48int  pinPlay = PD3;       /* Button play  - recommended (A5 is deprecated, will by used  for additional joystick) */
49int pinLedRecord = PD4;  /* LED - indicates recording  (light) or auto play mode (blink one) */
50
51bool useInternalPullUpResistors  = false;
52
53const int buffSize = 512; /* Size of recording buffer */
54
55int  startBase = 90;
56int startShoulder = 90;
57int startElbow = 90;
58int startGripper  = 0;
59
60int posBase = 90;
61int posShoulder = 90;
62int posElbow = 90;
63int  posGripper = 0;
64
65int lastBase = 90;
66int lastShoulder = 90;
67int lastElbow  = 90;
68int lastGripper = 90;
69
70int minBase = 0;
71int maxBase = 150;
72int  minShoulder = 0;
73int maxShoulder = 150;
74int minElbow = 0;
75int maxElbow  = 150;
76int minGripper = 0;
77int maxGripper = 150;
78
79const int countServo  = 4;
80int buff[buffSize];
81int buffAdd[countServo];
82int recPos = 0;
83int  playPos = 0;
84
85int buttonRecord = HIGH;
86int buttonPlay = HIGH;
87
88int  buttonRecordLast = LOW;
89int buttonPlayLast = LOW;
90
91bool record = false;
92bool  play = false;
93bool debug = false;
94
95String command = "Manual";
96int  printPos = 0;
97
98int buttonPlayDelay = 20;
99int buttonPlayCount = 0;
100
101bool  ledLight = false;
102
103Servo servoBase;
104Servo servoShoulder;
105Servo servoElbow;
106Servo  servoGripper;
107
108void setup() {
109  Serial.begin(9600);
110
111  if (useInternalPullUpResistors)  {
112    pinMode(pinRecord, INPUT_PULLUP);
113    pinMode(pinPlay, INPUT_PULLUP);
114  }
115  else
116  {
117    pinMode(pinRecord, INPUT);
118    pinMode(pinPlay, INPUT);
119  }
120
121  pinMode(xdirPin, INPUT);
122  pinMode(ydirPin, INPUT);
123  pinMode(zdirPin,  INPUT);
124  pinMode(gdirPin, INPUT);
125
126  pinMode(pinLedRecord, OUTPUT);
127
128  servoBase.attach(basePin);
129  servoShoulder.attach(shoulderPin);
130  servoElbow.attach(elbowPin);
131  servoGripper.attach(gripperPin);
132
133  StartPosition();
134
135  digitalWrite(pinLedRecord,  HIGH);
136  delay(1000);
137  digitalWrite(pinLedRecord, LOW);
138}
139
140void  loop() {
141
142  buttonRecord = digitalRead(pinRecord);
143  buttonPlay = digitalRead(pinPlay);
144
145  //  Serial.print(buttonRecord);
146  //  Serial.print("\	");
147  //  Serial.println(buttonPlay);
148  //  for testing purposes
149
150  if (buttonPlay == LOW)
151  {
152    buttonPlayCount++;
153
154    if (buttonPlayCount >= buttonPlayDelay)
155    {
156      repeatePlaying = true;
157    }
158  }
159  else buttonPlayCount = 0;
160
161  if (buttonPlay != buttonPlayLast)
162  {
163    if (record)
164    {
165      record = false;
166    }
167
168    if  (buttonPlay == LOW)
169    {
170      play = !play;
171      repeatePlaying = false;
172
173      if (play)
174      {
175        StartPosition();
176      }
177    }
178  }
179
180  if (buttonRecord != buttonRecordLast)
181  {
182    if (buttonRecord == LOW)
183    {
184      record = !record;
185
186      if (record)
187      {
188        play  = false;
189        repeatePlaying = false;
190        recPos = 0;
191      }
192      else
193      {
194        if (debug) PrintBuffer();
195      }
196    }
197  }
198
199  buttonPlayLast = buttonPlay;
200  buttonRecordLast = buttonRecord;
201
202  float dx = map(analogRead(xdirPin), 0, 1023, -5.0, 5.0);
203  float dy = map(analogRead(ydirPin),  0, 1023, 5.0, -5.0);
204  float dz = map(analogRead(zdirPin), 0, 1023, 5.0, -5.0);
205  float dg = map(analogRead(gdirPin), 0, 1023, 5.0, -5.0);
206
207  if (abs(dx)  < 1.5) dx = 0;
208  if (abs(dy) < 1.5) dy = 0;
209  if (abs(dz) < 1.5) dz = 0;
210  if (abs(dg) < 1.5) dg = 0;
211
212  posBase += dx;
213  posShoulder += dy;
214  posElbow += dz;
215  posGripper += dg;
216
217  if (play)
218  {
219    if (playPos  >= recPos) {
220      playPos = 0;
221
222      if (repeatePlaying)
223      {
224        delay(delayBetweenCycles);
225        StartPosition();
226      }
227      else
228      {
229        play = false;
230      }
231    }
232
233    bool endOfData =  false;
234
235    while (!endOfData)
236    {
237      if (playPos >= buffSize -  1) break;
238      if (playPos >= recPos) break;
239
240      int data = buff[playPos];
241      int angle = data & 0xFFF;
242      int servoNumber = data & 0x3000;
243      endOfData  = data & 0x4000;
244
245      switch (servoNumber)
246      {
247        case 0x0000:
248          posBase = angle;
249          break;
250
251        case 0x1000:
252          posShoulder  = angle;
253          break;
254
255        case 0x2000:
256          posElbow =  angle;
257          break;
258
259        case 0x3000:
260          posGripper =  angle;
261          dg = posGripper - lastGripper;
262          break;
263      }
264
265      playPos++;
266    }
267  }
268
269  if (posBase > maxBase) posBase = maxBase;
270  if (posShoulder > maxShoulder) posShoulder = maxShoulder;
271  if (posElbow >  maxElbow) posElbow = maxElbow;
272  if (posGripper > maxGripper) posGripper = maxGripper;
273
274  if (posBase < minBase) posBase = minBase;
275  if (posShoulder < minShoulder)  posShoulder = minShoulder;
276  if (posElbow < minElbow) posElbow = minElbow;
277  if (posGripper < minGripper) posGripper = minGripper;
278
279  servoBase.write(posBase);
280  servoShoulder.write(posShoulder);
281  servoElbow.write(posElbow);
282
283  bool  waitGripper = false;
284  if (dg < 0) {
285    posGripper = minGripper;
286    waitGripper  = true;
287  }
288  else if (dg > 0) {
289    posGripper = maxGripper;
290    waitGripper  = true;
291  }
292
293  servoGripper.write(posGripper);
294  if (play && waitGripper)
295  {
296    delay(1000);
297  }
298
299  if ((lastBase != posBase) | (lastShoulder  != posShoulder) | (lastElbow != posElbow) | (lastGripper != posGripper))
300  {
301    if (record)
302    {
303      if (recPos < buffSize - countServo)
304      {
305        int buffPos = 0;
306
307        if (lastBase != posBase)
308        {
309          buffAdd[buffPos] = posBase;
310          buffPos++;
311        }
312
313        if (lastShoulder != posShoulder)
314        {
315          buffAdd[buffPos]  = posShoulder | 0x1000;
316          buffPos++;
317        }
318
319        if (lastElbow  != posElbow)
320        {
321          buffAdd[buffPos] = posElbow | 0x2000;
322          buffPos++;
323        }
324
325        if (lastGripper != posGripper)
326        {
327          buffAdd[buffPos] = posGripper | 0x3000;
328          buffPos++;
329        }
330
331        buffAdd[buffPos - 1] = buffAdd[buffPos - 1] | 0x4000;
332
333        for (int i = 0; i < buffPos; i++)
334        {
335          buff[recPos  + i] = buffAdd[i];
336        }
337
338        recPos += buffPos;
339      }
340    }
341
342    command = "Manual";
343    printPos = 0;
344
345    if (play)
346    {
347      command = "Play";
348      printPos = playPos;
349    }
350    else  if (record)
351    {
352      command = "Record";
353      printPos = recPos;
354    }
355
356    Serial.print(command);
357    Serial.print("\	");
358    Serial.print(printPos);
359    Serial.print("\	");
360    Serial.print(posBase);
361    Serial.print("\	");
362    Serial.print(posShoulder);
363    Serial.print("\	");
364    Serial.print(posElbow);
365    Serial.print("\	");
366    Serial.print(posGripper);
367    Serial.print("\	");
368    Serial.print(record);
369    Serial.print("\	");
370    Serial.print(play);
371    Serial.println();
372  }
373
374  lastBase = posBase;
375  lastShoulder = posShoulder;
376  lastElbow = posElbow;
377  lastGripper = posGripper;
378
379  if ( repeatePlaying)
380  {
381    ledLight = !ledLight;
382  }
383  else
384  {
385    if (ledLight)
386    {
387      ledLight = false;
388    }
389
390    if (record)
391    {
392      ledLight  = true;
393    }
394  };
395
396  digitalWrite(pinLedRecord, ledLight);
397  delay(50);
398}
399
400void  PrintBuffer()
401{
402  for (int i = 0; i < recPos; i++)
403  {
404    int data  = buff[i];
405    int angle = data & 0xFFF;
406    int servoNumber = data & 0x3000;
407    bool endOfData = data & 0x4000;
408
409    Serial.print("Servo=");
410    Serial.print(servoNumber);
411    Serial.print("\	Angle=");
412    Serial.print(angle);
413    Serial.print("\	End=");
414    Serial.print(endOfData);
415    Serial.print("\	Data=");
416    Serial.print(data,  BIN);
417    Serial.println();
418  }
419}
420
421void StartPosition()
422{
423  int angleBase = servoBase.read();
424  int angleShoulder = servoShoulder.read();
425  int angleElbow = servoElbow.read();
426  int angleGripper = servoGripper.read();
427
428  Serial.print(angleBase);
429  Serial.print("\	");
430  Serial.print(angleShoulder);
431  Serial.print("\	");
432  Serial.print(angleElbow);
433  Serial.print("\	");
434  Serial.print(angleGripper);
435  Serial.println("\	");
436
437  posBase = startBase;
438  posShoulder = startShoulder;
439  posElbow = startElbow;
440  posGripper = startGripper;
441
442  servoBase.write(posBase);
443  servoShoulder.write(posShoulder);
444  servoElbow.write(posElbow);
445  servoGripper.write(posGripper);
446}

/* meArm analog joysticks version 1.3.1 - UtilStudio.com Dec 2018
   Uses two analogue joysticks and four servos.

   In version 1.3 was improved  recording of coordinates.
   Some bugs was removed.

   First joystick  moves gripper forwards, backwards, left and right,
   button start/stop recording  positions.

   Second joystick moves gripper up, down, and closes and opens,
   button start/stop playing recorded positions.
   Press button for 2 seconds  to autoplay.

   Pins:
   Arduino    Stick1    Stick2    Base   Shoulder  Elbow    Gripper   Record/
      GND       GND       GND    Brown     Brown   Brown     Brown    Auto play
       5V       VCC       VCC      Red       Red     Red       Red    LED
       A0       HOR
       A1       VER
       PD2      BUTT
       A2                 HOR
       A3                 VER
       PD3                BUTT
       11                       Yellow
       10                                 Yellow
        9                                         Yellow
        6                                                   Yellow
       PD4                                                            X
*/
#include  <Servo.h>

bool repeatePlaying = false; /* Repeatedly is running recorded  cycle */
int delayBetweenCycles = 2000; /* Delay between cycles */

int  basePin = 11;       /* Base servo */
int shoulderPin = 10;   /* Shoulder servo  */
int elbowPin = 9;       /* Elbow servo */
int gripperPin = 6;     /* Gripper  servo */

int xdirPin = 0;        /* Base - joystick1*/
int ydirPin = 1;        /* Shoulder - joystick1 */
int zdirPin = 3;        /* Elbow - joystick2  */
int gdirPin = 2;        /* Gripper - joystick2 */

//int pinRecord =  A4;     /* Button record - backward compatibility */
//int pinPlay = A5;       /*  Button play  - backward compatibility */
int pinRecord = PD2;     /* Button record  - recommended (A4 is deprecated, will by used for additional joystick) */
int  pinPlay = PD3;       /* Button play  - recommended (A5 is deprecated, will by used  for additional joystick) */
int pinLedRecord = PD4;  /* LED - indicates recording  (light) or auto play mode (blink one) */

bool useInternalPullUpResistors  = false;

const int buffSize = 512; /* Size of recording buffer */

int  startBase = 90;
int startShoulder = 90;
int startElbow = 90;
int startGripper  = 0;

int posBase = 90;
int posShoulder = 90;
int posElbow = 90;
int  posGripper = 0;

int lastBase = 90;
int lastShoulder = 90;
int lastElbow  = 90;
int lastGripper = 90;

int minBase = 0;
int maxBase = 150;
int  minShoulder = 0;
int maxShoulder = 150;
int minElbow = 0;
int maxElbow  = 150;
int minGripper = 0;
int maxGripper = 150;

const int countServo  = 4;
int buff[buffSize];
int buffAdd[countServo];
int recPos = 0;
int  playPos = 0;

int buttonRecord = HIGH;
int buttonPlay = HIGH;

int  buttonRecordLast = LOW;
int buttonPlayLast = LOW;

bool record = false;
bool  play = false;
bool debug = false;

String command = "Manual";
int  printPos = 0;

int buttonPlayDelay = 20;
int buttonPlayCount = 0;

bool  ledLight = false;

Servo servoBase;
Servo servoShoulder;
Servo servoElbow;
Servo  servoGripper;

void setup() {
  Serial.begin(9600);

  if (useInternalPullUpResistors)  {
    pinMode(pinRecord, INPUT_PULLUP);
    pinMode(pinPlay, INPUT_PULLUP);
  }
  else
  {
    pinMode(pinRecord, INPUT);
    pinMode(pinPlay, INPUT);
  }

  pinMode(xdirPin, INPUT);
  pinMode(ydirPin, INPUT);
  pinMode(zdirPin,  INPUT);
  pinMode(gdirPin, INPUT);

  pinMode(pinLedRecord, OUTPUT);

  servoBase.attach(basePin);
  servoShoulder.attach(shoulderPin);
  servoElbow.attach(elbowPin);
  servoGripper.attach(gripperPin);

  StartPosition();

  digitalWrite(pinLedRecord,  HIGH);
  delay(1000);
  digitalWrite(pinLedRecord, LOW);
}

void  loop() {

  buttonRecord = digitalRead(pinRecord);
  buttonPlay = digitalRead(pinPlay);

  //  Serial.print(buttonRecord);
  //  Serial.print("\	");
  //  Serial.println(buttonPlay);
  //  for testing purposes

  if (buttonPlay == LOW)
  {
    buttonPlayCount++;

    if (buttonPlayCount >= buttonPlayDelay)
    {
      repeatePlaying = true;
    }
  }
  else buttonPlayCount = 0;

  if (buttonPlay != buttonPlayLast)
  {
    if (record)
    {
      record = false;
    }

    if  (buttonPlay == LOW)
    {
      play = !play;
      repeatePlaying = false;

      if (play)
      {
        StartPosition();
      }
    }
  }

  if (buttonRecord != buttonRecordLast)
  {
    if (buttonRecord == LOW)
    {
      record = !record;

      if (record)
      {
        play  = false;
        repeatePlaying = false;
        recPos = 0;
      }
      else
      {
        if (debug) PrintBuffer();
      }
    }
  }

  buttonPlayLast = buttonPlay;
  buttonRecordLast = buttonRecord;

  float dx = map(analogRead(xdirPin), 0, 1023, -5.0, 5.0);
  float dy = map(analogRead(ydirPin),  0, 1023, 5.0, -5.0);
  float dz = map(analogRead(zdirPin), 0, 1023, 5.0, -5.0);
  float dg = map(analogRead(gdirPin), 0, 1023, 5.0, -5.0);

  if (abs(dx)  < 1.5) dx = 0;
  if (abs(dy) < 1.5) dy = 0;
  if (abs(dz) < 1.5) dz = 0;
  if (abs(dg) < 1.5) dg = 0;

  posBase += dx;
  posShoulder += dy;
  posElbow += dz;
  posGripper += dg;

  if (play)
  {
    if (playPos  >= recPos) {
      playPos = 0;

      if (repeatePlaying)
      {
        delay(delayBetweenCycles);
        StartPosition();
      }
      else
      {
        play = false;
      }
    }

    bool endOfData =  false;

    while (!endOfData)
    {
      if (playPos >= buffSize -  1) break;
      if (playPos >= recPos) break;

      int data = buff[playPos];
      int angle = data & 0xFFF;
      int servoNumber = data & 0x3000;
      endOfData  = data & 0x4000;

      switch (servoNumber)
      {
        case 0x0000:
          posBase = angle;
          break;

        case 0x1000:
          posShoulder  = angle;
          break;

        case 0x2000:
          posElbow =  angle;
          break;

        case 0x3000:
          posGripper =  angle;
          dg = posGripper - lastGripper;
          break;
      }

      playPos++;
    }
  }

  if (posBase > maxBase) posBase = maxBase;
  if (posShoulder > maxShoulder) posShoulder = maxShoulder;
  if (posElbow >  maxElbow) posElbow = maxElbow;
  if (posGripper > maxGripper) posGripper = maxGripper;

  if (posBase < minBase) posBase = minBase;
  if (posShoulder < minShoulder)  posShoulder = minShoulder;
  if (posElbow < minElbow) posElbow = minElbow;
  if (posGripper < minGripper) posGripper = minGripper;

  servoBase.write(posBase);
  servoShoulder.write(posShoulder);
  servoElbow.write(posElbow);

  bool  waitGripper = false;
  if (dg < 0) {
    posGripper = minGripper;
    waitGripper  = true;
  }
  else if (dg > 0) {
    posGripper = maxGripper;
    waitGripper  = true;
  }

  servoGripper.write(posGripper);
  if (play && waitGripper)
  {
    delay(1000);
  }

  if ((lastBase != posBase) | (lastShoulder  != posShoulder) | (lastElbow != posElbow) | (lastGripper != posGripper))
  {
    if (record)
    {
      if (recPos < buffSize - countServo)
      {
        int buffPos = 0;

        if (lastBase != posBase)
        {
          buffAdd[buffPos] = posBase;
          buffPos++;
        }

        if (lastShoulder != posShoulder)
        {
          buffAdd[buffPos]  = posShoulder | 0x1000;
          buffPos++;
        }

        if (lastElbow  != posElbow)
        {
          buffAdd[buffPos] = posElbow | 0x2000;
          buffPos++;
        }

        if (lastGripper != posGripper)
        {
          buffAdd[buffPos] = posGripper | 0x3000;
          buffPos++;
        }

        buffAdd[buffPos - 1] = buffAdd[buffPos - 1] | 0x4000;

        for (int i = 0; i < buffPos; i++)
        {
          buff[recPos  + i] = buffAdd[i];
        }

        recPos += buffPos;
      }
    }

    command = "Manual";
    printPos = 0;

    if (play)
    {
      command = "Play";
      printPos = playPos;
    }
    else  if (record)
    {
      command = "Record";
      printPos = recPos;
    }

    Serial.print(command);
    Serial.print("\	");
    Serial.print(printPos);
    Serial.print("\	");
    Serial.print(posBase);
    Serial.print("\	");
    Serial.print(posShoulder);
    Serial.print("\	");
    Serial.print(posElbow);
    Serial.print("\	");
    Serial.print(posGripper);
    Serial.print("\	");
    Serial.print(record);
    Serial.print("\	");
    Serial.print(play);
    Serial.println();
  }

  lastBase = posBase;
  lastShoulder = posShoulder;
  lastElbow = posElbow;
  lastGripper = posGripper;

  if ( repeatePlaying)
  {
    ledLight = !ledLight;
  }
  else
  {
    if (ledLight)
    {
      ledLight = false;
    }

    if (record)
    {
      ledLight  = true;
    }
  };

  digitalWrite(pinLedRecord, ledLight);
  delay(50);
}

void  PrintBuffer()
{
  for (int i = 0; i < recPos; i++)
  {
    int data  = buff[i];
    int angle = data & 0xFFF;
    int servoNumber = data & 0x3000;
    bool endOfData = data & 0x4000;

    Serial.print("Servo=");
    Serial.print(servoNumber);
    Serial.print("\	Angle=");
    Serial.print(angle);
    Serial.print("\	End=");
    Serial.print(endOfData);
    Serial.print("\	Data=");
    Serial.print(data,  BIN);
    Serial.println();
  }
}

void StartPosition()
{
  int angleBase = servoBase.read();
  int angleShoulder = servoShoulder.read();
  int angleElbow = servoElbow.read();
  int angleGripper = servoGripper.read();

  Serial.print(angleBase);
  Serial.print("\	");
  Serial.print(angleShoulder);
  Serial.print("\	");
  Serial.print(angleElbow);
  Serial.print("\	");
  Serial.print(angleGripper);
  Serial.println("\	");

  posBase = startBase;
  posShoulder = startShoulder;
  posElbow = startElbow;
  posGripper = startGripper;

  servoBase.write(posBase);
  servoShoulder.write(posShoulder);
  servoElbow.write(posElbow);
  servoGripper.write(posGripper);
}

Downloadable files

MeArm robot - joystick servo recorder - test board

Testing board with only two servos connected. See schematic diagram to connect 4 servo motors.

MeArm robot - joystick servo recorder - test board

Joystick controller for MeArm ® robot - shematic diagram (version 1.3)

Joystick controller for MeArm ® robot with recording of coordinates - shematic diagram

Joystick controller for MeArm ® robot - shematic diagram (version 1.3)

Joystick controller for MeArm ® robot with recording of coordinates - shematic diagram

Joystick controller for MeArm ® robot - shematic diagram (version 1.3)

MeArm robot - joystick servo recorder - test board

Testing board with only two servos connected. See schematic diagram to connect 4 servo motors.

MeArm robot - joystick servo recorder - test board

Comments

Only logged in users can leave comments

utilstudio

0 Followers

•

0 Projects

Intro