MeArm 1.6.1 Robot Joystick Board Recording Movements (IR)

MeArm robot with using board MeArm v1.6.1 with recording of movemens and with infrared remote controller possibility.

Jan 7, 2019

•

13231 views

•

15 respects

Components and supplies

Micro servo SG90

MeArm joystick board (version 1.6.1)

Arduino UNO

Infrared module KY-022

Apps and platforms

Arduino IDE

Project description

Code

MeArm ® 1.6.1 robot joystick board recording movements with possible infrared remote controller

arduino

Use the Arduino IDE and the USB cable to program your Arduino UNO and enjoy. ;-)

1/* meArm analog joysticks version 1.6.1.4 - UtilStudio.com Jan 2019
2   Uses two analogue joysticks and four servos.
3 
4 Version 1.6.1.x is targeted   to MeArm Joystick board version 1.6.1.
5 
6 First joystick moves gripper forwards,   backwards, left and right,
7 button start/stop recording positions.
8 
9 Second   joystick moves gripper up, down, and closes and opens,
10 button start/stop playing   recorded positions.
11 Press button for 2 seconds to autoplay.
12 
13 Please   send a short message as a feedback, if you uses this code. 
14 Thank you. ;-)
15   
16 
17 Pins:
18 Arduino  Stick1 Stick2 Base   Shoulder Elbow  Gripper Record/     Infrared
19 GND      GND    GND    Brown  Brown    Brown  Brown   Auto play   sensor
20 5V       VCC    VCC    Red    Red      Red    Red     LED
21 A0       HOR
22   A1       VER
23 PD2      BUTT
24 A2              HOR
25 A3              VER
26   PD4             BUTT
27 11                     Yellow
28 10                            Yellow
29   9                                      Yellow
30 5                                             Yellow
31   PD3                                                   X
32 12                                                               X
33   */
34
35//#define infraredSensor true; // Uncomment this line, if you want to   use Infrared controller. 
36
37/* 
38 And replace codes of infrared controller   with your own codes in function CheckIrController.
39 Set debug to true and use   a Serial monitor of your Arduino IDE to show received codes of your remote controller.
40   */
41
42#include <Servo.h>
43
44#if defined infraredSensor
45#include <IRremote.h>   
46#endif                      
47
48bool debug = false;
49
50bool repeatePlaying   = false;   /* Repeatedly is running recorded cycle */
51int delayBetweenCycles   = 2000; /* Delay between cycles */
52
53const int basePin = 11;       /* Base   servo */
54const int shoulderPin = 10;   /* Shoulder servo */
55const int elbowPin   = 9;       /* Elbow servo */
56const int gripperPin = 5;     /* Gripper servo -   changed */
57
58int xdirPin = 0;        /* Base - joystick1*/
59int ydirPin =   1;        /* Shoulder - joystick1 */
60int zdirPin = 2;        /* Elbow - joystick2   */
61int gdirPin = 3;        /* Gripper - joystick2 */
62
63int pinRecord = PD2;      /* Button record */
64int pinPlay = PD4;       /* Button play - changed */
65int   pinLedRecord = PD3;  /* LED - indicates recording (light) or auto play mode (blink   ones) - changed */
66
67int RECV_PIN = 12;      /* Infrared sensor */
68
69const   int buffSize = 412; /* Size of recording buffer */
70
71int startBase = 90;
72int   startShoulder = 42;
73int startElbow = 50;
74int startGripper = 30;
75
76int   posBase = 90;
77int posShoulder = 90;
78int posElbow = 90;
79int posGripper =   90;
80
81int lastBase = 90;
82int lastShoulder = 90;
83int lastElbow = 90;
84int   lastGripper = 90;
85
86int minBase = 0;
87int maxBase = 150;
88int minShoulder   = 25;
89int maxShoulder = 135;
90int minElbow = 10;
91int maxElbow = 118;
92int   minGripper = 10;
93int maxGripper = 61;
94
95const int countServo = 4;
96byte   currentPos[countServo] = {
97  0, 0, 0, 0};
98byte lastPos[countServo] = {
99   255, 255, 255, 255};
100byte minPos[countServo] = {
101  minBase, minShoulder,   minElbow, minGripper};
102byte maxPos[countServo] = {
103  maxBase, maxShoulder,   maxElbow, maxGripper};
104int servoPin[countServo] = {
105  basePin, shoulderPin,   elbowPin, gripperPin};
106
107
108uint16_t buff[buffSize];
109uint16_t buffAdd[countServo];
110int   recPos = 0;
111int playPos = 0;
112int blockPlayPos = 0;
113
114int buffPos = 0;
115
116struct   tPlayBuff
117{
118  byte ServoNumber;
119  byte Angle;
120};
121
122tPlayBuff playBuff[countServo];
123int   playBuffCount = 0;
124
125byte cyclesRecord = 0;
126byte cyclesPlay = 0;
127
128int   buttonRecord = HIGH;
129int buttonPlay = HIGH;
130
131int buttonRecordLast = LOW;
132int   buttonPlayLast = LOW;
133
134bool record = false;
135bool play = false;
136
137String   command = "Manual";
138int printPos = 0;
139
140int buttonPlayDelay = 20;
141int   buttonPlayCount = 0;
142
143bool ledLight = false;
144
145int servoTime = 0;
146
147float   dx = 0;
148float dy = 0;
149float dz = 0;
150float dg = 0;
151
152Servo servoBase;
153Servo   servoShoulder;
154Servo servoElbow;
155Servo servoGripper;
156
157#if defined infraredSensor
158IRrecv   irrecv(RECV_PIN);
159
160decode_results results;
161
162// Please, replace codes   of infrared controller with your own codes 
163// in function CheckIrController
164void   CheckIrController()
165{
166  if (irrecv.decode(&results))
167  {
168    switch   (results.value)
169    {
170    case 0xBE15326E:
171    case 0xDB9D3097:
172      dx   = 5;
173      break;
174    case 0x9912B99A:
175    case 0x9FA96F:
176      dx =   -5;
177      break;
178    case 0xE5139CA7:
179    case 0xAC516266:
180      dy   = 5;
181      break;
182    case 0xE4139B12:
183    case 0xAD5163FB:
184      dy   = -5;
185      break;
186    case 0x21:
187    case 0x821:
188      dz = -5;
189       break;
190    case 0x20:
191    case 0x820:
192      dz = 5;
193      break;
194     case 0x11:
195    case 0x811:
196      dg = -5;
197      break;
198    case   0x10:
199    case 0x810:
200      dg = 5;
201      break;
202    case 0x1C102884:
203     case 0x4B995E59:
204      buttonRecord = !buttonRecord;
205      break;
206     case 0xC460AC26:
207    case 0x6626F67F:
208      buttonPlay = !buttonPlay;
209       break;
210    case 0xDF50BC53:
211    case 0xF04EE0E6:
212      repeatePlaying   = !repeatePlaying;
213
214      if (repeatePlaying)
215      {
216        record   = false;
217        play = true;  
218      }
219      else
220      {
221        play   = false;
222      }
223      break;
224    }
225  }
226}
227#endif
228
229void setup()   {
230  Serial.begin(9600);
231
232  pinMode(xdirPin, INPUT);
233  pinMode(ydirPin,   INPUT);
234  pinMode(zdirPin, INPUT);
235  pinMode(gdirPin, INPUT);
236
237  pinMode(pinRecord,   INPUT_PULLUP);
238  pinMode(pinPlay, INPUT_PULLUP);
239
240  pinMode(pinLedRecord,   OUTPUT);
241
242  servoBase.attach(basePin);
243  servoShoulder.attach(shoulderPin);
244   servoElbow.attach(elbowPin);
245  servoGripper.attach(gripperPin);
246
247  StartPosition();
248
249#if   defined infraredSensor
250  irrecv.enableIRIn(); // Start the receiver
251#endif   
252
253  digitalWrite(pinLedRecord, HIGH);
254  delay(1000);
255  digitalWrite(pinLedRecord,   LOW);
256}
257
258void loop() {  
259  buttonRecord = digitalRead(pinRecord);
260   buttonPlay = digitalRead(pinPlay);
261
262  dx = 0;
263  dy = 0;
264  dz = 0;
265   dg = 0;
266
267
268#if defined infraredSensor
269  CheckIrController();
270
271   if (debug)
272  {
273    Serial.println(results.value, HEX);
274  }
275
276  irrecv.resume();   // Receive the next value
277#endif  
278
279  //  Serial.print(buttonRecord);
280   //  Serial.print("\	");
281  //  Serial.println(buttonPlay);
282  //  for testing   purposes
283
284  if (buttonPlay == LOW)
285  {
286    buttonPlayCount++;
287
288     if (buttonPlayCount >= buttonPlayDelay)
289    {
290      repeatePlaying = true;
291     }
292  }
293  else buttonPlayCount = 0;
294
295  if (buttonPlay != buttonPlayLast)
296   {
297    if (record)
298    {
299      record = false;
300    }
301
302    if   (buttonPlay == LOW)
303    {
304      play = !play;
305      repeatePlaying = false;
306
307       if (play)
308      {
309        //  StartPosition();
310      }
311    }
312     buttonPlayLast = buttonPlay;
313  }
314
315  if (buttonRecord != buttonRecordLast)
316   {
317    if (buttonRecord == LOW)
318    {
319      record = !record;
320
321       if (record)
322      {
323        play = false;
324        repeatePlaying   = false;
325        recPos = 0;
326        cyclesRecord = 0;
327      }
328      else
329       {
330        if (debug) PrintBuffer();
331      }
332    }
333    buttonRecordLast   = buttonRecord;
334  }
335
336  if (repeatePlaying)
337  {
338    play = true;
339   }
340
341  if (dx == 0) dx = map(analogRead(xdirPin), 0, 1023, 3.5, -3.5);
342   if (dy == 0) dy = map(analogRead(ydirPin), 0, 1023, 4.0, -4.0);
343  if (dz ==   0) dz = map(analogRead(zdirPin), 0, 1023, 4.0, -4.0);
344  if (dg == 0) dg = map(analogRead(gdirPin),   0, 1023, -4.0, 4.0);
345
346  if (abs(dx) < 1.5) dx = 0;
347  if (abs(dy) < 1.5)   dy = 0;
348  if (abs(dz) < 1.5) dz = 0;
349  if (abs(dg) < 1.5) dg = 0;
350
351   posBase += dx;
352  posShoulder += dy;
353  posElbow += dz;
354  posGripper +=   dg;
355
356  if ((play) | (cyclesPlay > 0))
357  {
358    if (cyclesPlay > 0)
359     {
360      cyclesPlay--;
361
362      if (cyclesPlay > 0)
363      {
364        if   (play)
365        {
366          playPos = blockPlayPos;
367        }
368        else
369         {
370          play = false;
371        }
372      }
373    }
374
375    if   (play)
376    {
377      if (playPos >= recPos)
378      {
379        playPos =   0;
380
381        if (repeatePlaying)
382        {
383          if (debug)
384          {
385             Serial.println("Auto start ");
386          }
387
388          delay(delayBetweenCycles);
389           // StartPosition();
390        }
391        else
392        {
393          play   = false;
394          repeatePlaying = false;
395        }
396      }
397
398      blockPlayPos   = playPos;
399      playBuffCount = 0;
400
401      bool endOfData = false;
402
403       while (!endOfData)
404      {
405        if (playPos >= buffSize - 1) break;
406         if (playPos >= recPos) break;
407
408        uint16_t data = buff[playPos];
409         byte angle = data & 0xFF;
410        uint16_t servoNumber = data & 0xf00;
411         endOfData = data & 0x8000;
412        uint16_t repeatCycles = data & 0x7000;
413         repeatCycles = repeatCycles >> 12;
414
415        if (play & cyclesPlay   <= 0)
416        {
417          cyclesPlay = repeatCycles;
418        }
419
420         servoNumber = servoNumber >> 8;
421
422        playBuff[playBuffCount].ServoNumber   = servoNumber;
423        playBuff[playBuffCount].Angle = angle;
424
425        playBuffCount++;
426
427         switch (servoNumber)
428        {
429        case 0:
430          posBase   = angle;
431          break;
432
433        case 1:
434          posShoulder = angle;
435           break;
436
437        case 2:
438          posElbow = angle;
439          break;
440
441         case 3:
442          posGripper = angle;
443          dg = posGripper -   lastGripper;
444          break;
445        }
446
447        //        if (debug)
448         //        {
449        //          Serial.print("Play data ");
450        //           Serial.print(data, BIN);
451        //          Serial.print(" servo   ");
452        //          Serial.print(servoNumber);
453        //          Serial.print("   angle ");
454        //          Serial.print(angle);
455        //          Serial.print("   end=");
456        //          Serial.println(endOfData);
457        //        }
458         //
459
460        playPos++;
461
462        if (playPos >= recPos)
463        {
464           play = false;
465
466          break;
467        }
468      }
469    }
470   }
471  else
472  {
473    cyclesPlay = 0;
474  }
475
476  if (posBase > maxBase)   posBase = maxBase;
477  if (posShoulder > maxShoulder) posShoulder = maxShoulder;
478   if (posElbow > maxElbow) posElbow = maxElbow;
479  if (posGripper > maxGripper)   posGripper = maxGripper;
480
481  if (posBase < minBase) posBase = minBase;
482   if (posShoulder < minShoulder) posShoulder = minShoulder;
483  if (posElbow <   minElbow) posElbow = minElbow;
484  if (posGripper < minGripper) posGripper = minGripper;
485
486   bool waitGripper = false;
487  if (dg < 0) {
488    posGripper = minGripper;
489     waitGripper = true;
490  }
491  else if (dg > 0) {
492    posGripper = maxGripper;
493     waitGripper = true;
494  }
495
496  if (play && waitGripper)
497  {
498    delay(1000);
499   }
500
501  currentPos[0] = posBase;
502  currentPos[1] = posShoulder;
503  currentPos[2]   = posElbow;
504  currentPos[3] = posGripper;
505
506  bool positionChanged = false;
507
508   if ((play) | (cyclesPlay > 0))
509  {
510    for (int i = 0; i < playBuffCount;   i++)
511    {
512      MoveServo(servoPin[playBuff[i].ServoNumber], playBuff[i].Angle,   servoTime);
513    }
514  }
515  else {
516    for (int i = 0; i < countServo; i++)
517     {
518      if (currentPos[i] > maxPos[i]) currentPos[i] = maxPos[i];
519      if   (currentPos[i] < minPos[i]) currentPos[i] = minPos[i];
520
521      if (currentPos[i]   != lastPos[i])
522      {
523        positionChanged = true;
524        MoveServo(servoPin[i],   currentPos[i], servoTime);
525      }
526    }
527  }
528
529  if (positionChanged)
530   {
531    if (record)
532    {
533      buffPos = 0;
534      cyclesRecord = 0;
535
536       for (int i = 0; i < countServo; i++)
537      {
538        if (lastPos[i]   != currentPos[i])
539        {
540          uint16_t value = (currentPos[i] | (0x100   * i)) & 0x0FFF;
541          buffAdd[buffPos] = value;
542          buffPos++;
543         }
544      }
545      buffAdd[buffPos - 1] = buffAdd[buffPos - 1] | 0x8000;
546
547       AddToBuff();
548    }
549  }
550  else
551  {
552    if (record)
553    {
554       if (recPos > 0)
555      {
556        cyclesRecord ++;
557
558        bool   added = false;
559        bool first = true;
560
561        for (int i = recPos   - 1; i >= 0; i--)
562        {
563          bool endOfData = buff[i] & 0x8000;
564
565           if (!first && endOfData) break;
566
567          if (cyclesRecord > 7)
568           {
569            cyclesRecord = 0;
570
571            AddToBuff();
572             added = true;
573          }
574
575          if (!added) {
576            uint16_t   val = cyclesRecord & 7;
577            val = val << 12;
578            val = val   & 0x7000;
579
580            buff[i] = buff[i] & 0x8FFF;
581            buff[i]   = buff[i] | val;
582
583            //            if (debug)
584            //            {
585             //              Serial.print(" i=");
586            //              Serial.print(i);
587             //              Serial.print(" Buff[i]=");
588            //              Serial.print(buff[i],   BIN);
589            //              Serial.print(" val=");
590            //               Serial.println(val, BIN);
591            //            }
592
593            first   = false;
594          }
595        }
596      }
597    }
598  }
599
600  lastBase   = posBase;
601  lastShoulder = posShoulder;
602  lastElbow = posElbow;
603  lastGripper   = posGripper;
604
605  for (int i = 0; i < countServo; i++)
606  {
607    lastPos[i]   = currentPos[i];
608  }
609
610  if ( repeatePlaying)
611  {
612    ledLight = !ledLight;
613   }
614  else
615  {
616    if (ledLight)
617    {
618      ledLight = false;
619     }
620
621    if (record)
622    {
623      ledLight = true;
624    }
625  };
626
627   digitalWrite(pinLedRecord, ledLight);
628  delay(50);
629}
630
631void AddToBuff()
632{
633   for (int i = 0; i < buffPos; i++)
634  {
635    buff[recPos + i] = buffAdd[i];
636   }
637
638  recPos += buffPos;
639
640  if (recPos >= buffSize - countServo)
641   {
642    record = false;
643  }
644}
645
646void MoveServo(uint8_t idServo, int   angle, int timeMs)
647{
648  switch (idServo)
649  {
650  case basePin:
651    servoBase.write(angle);
652     break;
653  case shoulderPin:
654    servoShoulder.write(angle);
655    break;
656   case elbowPin:
657    servoElbow.write(angle);
658    break;
659  case gripperPin:
660     servoGripper.write(angle);
661    break;
662  }
663
664  if (debug)
665  {
666     if (record) Serial.print(" Record ");
667    if (play) Serial.print(" Play   ");
668    if (repeatePlaying) Serial.print(" Auto ");
669
670    Serial.print("   Servo ");
671    Serial.print(idServo);
672    Serial.print(" Angle ");
673    Serial.println(angle);
674   }
675}
676
677void PrintBuffer()
678{
679  for (int i = 0; i < recPos; i++)
680   {
681    uint16_t data = buff[i];
682    byte angle = data & 0xFF;
683    uint16_t   servoNumber = data & 0xF00;
684    servoNumber = servoNumber >> 8;
685    bool endOfData   = data & 0x8000;
686
687    Serial.print("Servo=");
688    Serial.print(servoNumber,   HEX);
689    Serial.print("\	Angle=");
690    Serial.print(angle);
691    Serial.print("\	End=");
692     Serial.print(endOfData);
693    Serial.print("\	Data=");
694    Serial.print(data,   BIN);
695    Serial.println();
696  }
697}
698
699void StartPosition()
700{
701   int angleBase = servoBase.read();
702  int angleShoulder = servoShoulder.read();
703   int angleElbow = servoElbow.read();
704  int angleGripper = servoGripper.read();
705
706   Serial.print(angleBase);
707  Serial.print("\	");
708  Serial.print(angleShoulder);
709   Serial.print("\	");
710  Serial.print(angleElbow);
711  Serial.print("\	");
712   Serial.print(angleGripper);
713  Serial.println("\	");
714
715  posBase = startBase;
716   posShoulder = startShoulder;
717  posElbow = startElbow;
718  posGripper = startGripper;
719
720   servoBase.write(posBase);
721  servoShoulder.write(posShoulder);
722  servoElbow.write(posElbow);
723   servoGripper.write(posGripper);
724}
725

/* meArm analog joysticks version 1.6.1.4 - UtilStudio.com Jan 2019
   Uses two analogue joysticks and four servos.
 
 Version 1.6.1.x is targeted   to MeArm Joystick board version 1.6.1.
 
 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.
 
 Please   send a short message as a feedback, if you uses this code. 
 Thank you. ;-)
   
 
 Pins:
 Arduino  Stick1 Stick2 Base   Shoulder Elbow  Gripper Record/     Infrared
 GND      GND    GND    Brown  Brown    Brown  Brown   Auto play   sensor
 5V       VCC    VCC    Red    Red      Red    Red     LED
 A0       HOR
   A1       VER
 PD2      BUTT
 A2              HOR
 A3              VER
   PD4             BUTT
 11                     Yellow
 10                            Yellow
   9                                      Yellow
 5                                             Yellow
   PD3                                                   X
 12                                                               X
   */

//#define infraredSensor true; // Uncomment this line, if you want to   use Infrared controller. 

/* 
 And replace codes of infrared controller   with your own codes in function CheckIrController.
 Set debug to true and use   a Serial monitor of your Arduino IDE to show received codes of your remote controller.
   */

#include <Servo.h>

#if defined infraredSensor
#include <IRremote.h>   
#endif                      

bool debug = false;

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

const int basePin = 11;       /* Base   servo */
const int shoulderPin = 10;   /* Shoulder servo */
const int elbowPin   = 9;       /* Elbow servo */
const int gripperPin = 5;     /* Gripper servo -   changed */

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

int pinRecord = PD2;      /* Button record */
int pinPlay = PD4;       /* Button play - changed */
int   pinLedRecord = PD3;  /* LED - indicates recording (light) or auto play mode (blink   ones) - changed */

int RECV_PIN = 12;      /* Infrared sensor */

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

int startBase = 90;
int   startShoulder = 42;
int startElbow = 50;
int startGripper = 30;

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

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

int minBase = 0;
int maxBase = 150;
int minShoulder   = 25;
int maxShoulder = 135;
int minElbow = 10;
int maxElbow = 118;
int   minGripper = 10;
int maxGripper = 61;

const int countServo = 4;
byte   currentPos[countServo] = {
  0, 0, 0, 0};
byte lastPos[countServo] = {
   255, 255, 255, 255};
byte minPos[countServo] = {
  minBase, minShoulder,   minElbow, minGripper};
byte maxPos[countServo] = {
  maxBase, maxShoulder,   maxElbow, maxGripper};
int servoPin[countServo] = {
  basePin, shoulderPin,   elbowPin, gripperPin};


uint16_t buff[buffSize];
uint16_t buffAdd[countServo];
int   recPos = 0;
int playPos = 0;
int blockPlayPos = 0;

int buffPos = 0;

struct   tPlayBuff
{
  byte ServoNumber;
  byte Angle;
};

tPlayBuff playBuff[countServo];
int   playBuffCount = 0;

byte cyclesRecord = 0;
byte cyclesPlay = 0;

int   buttonRecord = HIGH;
int buttonPlay = HIGH;

int buttonRecordLast = LOW;
int   buttonPlayLast = LOW;

bool record = false;
bool play = false;

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

int buttonPlayDelay = 20;
int   buttonPlayCount = 0;

bool ledLight = false;

int servoTime = 0;

float   dx = 0;
float dy = 0;
float dz = 0;
float dg = 0;

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

#if defined infraredSensor
IRrecv   irrecv(RECV_PIN);

decode_results results;

// Please, replace codes   of infrared controller with your own codes 
// in function CheckIrController
void   CheckIrController()
{
  if (irrecv.decode(&results))
  {
    switch   (results.value)
    {
    case 0xBE15326E:
    case 0xDB9D3097:
      dx   = 5;
      break;
    case 0x9912B99A:
    case 0x9FA96F:
      dx =   -5;
      break;
    case 0xE5139CA7:
    case 0xAC516266:
      dy   = 5;
      break;
    case 0xE4139B12:
    case 0xAD5163FB:
      dy   = -5;
      break;
    case 0x21:
    case 0x821:
      dz = -5;
       break;
    case 0x20:
    case 0x820:
      dz = 5;
      break;
     case 0x11:
    case 0x811:
      dg = -5;
      break;
    case   0x10:
    case 0x810:
      dg = 5;
      break;
    case 0x1C102884:
     case 0x4B995E59:
      buttonRecord = !buttonRecord;
      break;
     case 0xC460AC26:
    case 0x6626F67F:
      buttonPlay = !buttonPlay;
       break;
    case 0xDF50BC53:
    case 0xF04EE0E6:
      repeatePlaying   = !repeatePlaying;

      if (repeatePlaying)
      {
        record   = false;
        play = true;  
      }
      else
      {
        play   = false;
      }
      break;
    }
  }
}
#endif

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

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

  pinMode(pinRecord,   INPUT_PULLUP);
  pinMode(pinPlay, INPUT_PULLUP);

  pinMode(pinLedRecord,   OUTPUT);

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

  StartPosition();

#if   defined infraredSensor
  irrecv.enableIRIn(); // Start the receiver
#endif   

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

void loop() {  
  buttonRecord = digitalRead(pinRecord);
   buttonPlay = digitalRead(pinPlay);

  dx = 0;
  dy = 0;
  dz = 0;
   dg = 0;


#if defined infraredSensor
  CheckIrController();

   if (debug)
  {
    Serial.println(results.value, HEX);
  }

  irrecv.resume();   // Receive the next value
#endif  

  //  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();
      }
    }
     buttonPlayLast = buttonPlay;
  }

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

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

  if (repeatePlaying)
  {
    play = true;
   }

  if (dx == 0) dx = map(analogRead(xdirPin), 0, 1023, 3.5, -3.5);
   if (dy == 0) dy = map(analogRead(ydirPin), 0, 1023, 4.0, -4.0);
  if (dz ==   0) dz = map(analogRead(zdirPin), 0, 1023, 4.0, -4.0);
  if (dg == 0) dg = map(analogRead(gdirPin),   0, 1023, -4.0, 4.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) | (cyclesPlay > 0))
  {
    if (cyclesPlay > 0)
     {
      cyclesPlay--;

      if (cyclesPlay > 0)
      {
        if   (play)
        {
          playPos = blockPlayPos;
        }
        else
         {
          play = false;
        }
      }
    }

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

        if (repeatePlaying)
        {
          if (debug)
          {
             Serial.println("Auto start ");
          }

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

      blockPlayPos   = playPos;
      playBuffCount = 0;

      bool endOfData = false;

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

        uint16_t data = buff[playPos];
         byte angle = data & 0xFF;
        uint16_t servoNumber = data & 0xf00;
         endOfData = data & 0x8000;
        uint16_t repeatCycles = data & 0x7000;
         repeatCycles = repeatCycles >> 12;

        if (play & cyclesPlay   <= 0)
        {
          cyclesPlay = repeatCycles;
        }

         servoNumber = servoNumber >> 8;

        playBuff[playBuffCount].ServoNumber   = servoNumber;
        playBuff[playBuffCount].Angle = angle;

        playBuffCount++;

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

        case 1:
          posShoulder = angle;
           break;

        case 2:
          posElbow = angle;
          break;

         case 3:
          posGripper = angle;
          dg = posGripper -   lastGripper;
          break;
        }

        //        if (debug)
         //        {
        //          Serial.print("Play data ");
        //           Serial.print(data, BIN);
        //          Serial.print(" servo   ");
        //          Serial.print(servoNumber);
        //          Serial.print("   angle ");
        //          Serial.print(angle);
        //          Serial.print("   end=");
        //          Serial.println(endOfData);
        //        }
         //

        playPos++;

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

          break;
        }
      }
    }
   }
  else
  {
    cyclesPlay = 0;
  }

  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;

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

  if (play && waitGripper)
  {
    delay(1000);
   }

  currentPos[0] = posBase;
  currentPos[1] = posShoulder;
  currentPos[2]   = posElbow;
  currentPos[3] = posGripper;

  bool positionChanged = false;

   if ((play) | (cyclesPlay > 0))
  {
    for (int i = 0; i < playBuffCount;   i++)
    {
      MoveServo(servoPin[playBuff[i].ServoNumber], playBuff[i].Angle,   servoTime);
    }
  }
  else {
    for (int i = 0; i < countServo; i++)
     {
      if (currentPos[i] > maxPos[i]) currentPos[i] = maxPos[i];
      if   (currentPos[i] < minPos[i]) currentPos[i] = minPos[i];

      if (currentPos[i]   != lastPos[i])
      {
        positionChanged = true;
        MoveServo(servoPin[i],   currentPos[i], servoTime);
      }
    }
  }

  if (positionChanged)
   {
    if (record)
    {
      buffPos = 0;
      cyclesRecord = 0;

       for (int i = 0; i < countServo; i++)
      {
        if (lastPos[i]   != currentPos[i])
        {
          uint16_t value = (currentPos[i] | (0x100   * i)) & 0x0FFF;
          buffAdd[buffPos] = value;
          buffPos++;
         }
      }
      buffAdd[buffPos - 1] = buffAdd[buffPos - 1] | 0x8000;

       AddToBuff();
    }
  }
  else
  {
    if (record)
    {
       if (recPos > 0)
      {
        cyclesRecord ++;

        bool   added = false;
        bool first = true;

        for (int i = recPos   - 1; i >= 0; i--)
        {
          bool endOfData = buff[i] & 0x8000;

           if (!first && endOfData) break;

          if (cyclesRecord > 7)
           {
            cyclesRecord = 0;

            AddToBuff();
             added = true;
          }

          if (!added) {
            uint16_t   val = cyclesRecord & 7;
            val = val << 12;
            val = val   & 0x7000;

            buff[i] = buff[i] & 0x8FFF;
            buff[i]   = buff[i] | val;

            //            if (debug)
            //            {
             //              Serial.print(" i=");
            //              Serial.print(i);
             //              Serial.print(" Buff[i]=");
            //              Serial.print(buff[i],   BIN);
            //              Serial.print(" val=");
            //               Serial.println(val, BIN);
            //            }

            first   = false;
          }
        }
      }
    }
  }

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

  for (int i = 0; i < countServo; i++)
  {
    lastPos[i]   = currentPos[i];
  }

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

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

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

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

  recPos += buffPos;

  if (recPos >= buffSize - countServo)
   {
    record = false;
  }
}

void MoveServo(uint8_t idServo, int   angle, int timeMs)
{
  switch (idServo)
  {
  case basePin:
    servoBase.write(angle);
     break;
  case shoulderPin:
    servoShoulder.write(angle);
    break;
   case elbowPin:
    servoElbow.write(angle);
    break;
  case gripperPin:
     servoGripper.write(angle);
    break;
  }

  if (debug)
  {
     if (record) Serial.print(" Record ");
    if (play) Serial.print(" Play   ");
    if (repeatePlaying) Serial.print(" Auto ");

    Serial.print("   Servo ");
    Serial.print(idServo);
    Serial.print(" Angle ");
    Serial.println(angle);
   }
}

void PrintBuffer()
{
  for (int i = 0; i < recPos; i++)
   {
    uint16_t data = buff[i];
    byte angle = data & 0xFF;
    uint16_t   servoNumber = data & 0xF00;
    servoNumber = servoNumber >> 8;
    bool endOfData   = data & 0x8000;

    Serial.print("Servo=");
    Serial.print(servoNumber,   HEX);
    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);
}

MeArm ® 1.6.1 robot joystick board recording movements with possible infrared remote controller

arduino

Use the Arduino IDE and the USB cable to program your Arduino UNO and enjoy. ;-)

1/* meArm analog joysticks version 1.6.1.4 - UtilStudio.com Jan 2019
2  Uses two analogue joysticks and four servos.
3 
4 Version 1.6.1.x is targeted  to MeArm Joystick board version 1.6.1.
5 
6 First joystick moves gripper forwards,  backwards, left and right,
7 button start/stop recording positions.
8 
9 Second  joystick moves gripper up, down, and closes and opens,
10 button start/stop playing  recorded positions.
11 Press button for 2 seconds to autoplay.
12 
13 Please  send a short message as a feedback, if you uses this code. 
14 Thank you. ;-)
15  
16 
17 Pins:
18 Arduino  Stick1 Stick2 Base   Shoulder Elbow  Gripper Record/    Infrared
19 GND      GND    GND    Brown  Brown    Brown  Brown   Auto play  sensor
20 5V       VCC    VCC    Red    Red      Red    Red     LED
21 A0       HOR
22  A1       VER
23 PD2      BUTT
24 A2              HOR
25 A3              VER
26  PD4             BUTT
27 11                     Yellow
28 10                            Yellow
29  9                                      Yellow
30 5                                             Yellow
31  PD3                                                   X
32 12                                                               X
33  */
34
35//#define infraredSensor true; // Uncomment this line, if you want to  use Infrared controller. 
36
37/* 
38 And replace codes of infrared controller  with your own codes in function CheckIrController.
39 Set debug to true and use  a Serial monitor of your Arduino IDE to show received codes of your remote controller.
40  */
41
42#include <Servo.h>
43
44#if defined infraredSensor
45#include <IRremote.h>  
46#endif                      
47
48bool debug = false;
49
50bool repeatePlaying  = false;   /* Repeatedly is running recorded cycle */
51int delayBetweenCycles  = 2000; /* Delay between cycles */
52
53const int basePin = 11;       /* Base  servo */
54const int shoulderPin = 10;   /* Shoulder servo */
55const int elbowPin  = 9;       /* Elbow servo */
56const int gripperPin = 5;     /* Gripper servo -  changed */
57
58int xdirPin = 0;        /* Base - joystick1*/
59int ydirPin =  1;        /* Shoulder - joystick1 */
60int zdirPin = 2;        /* Elbow - joystick2  */
61int gdirPin = 3;        /* Gripper - joystick2 */
62
63int pinRecord = PD2;     /* Button record */
64int pinPlay = PD4;       /* Button play - changed */
65int  pinLedRecord = PD3;  /* LED - indicates recording (light) or auto play mode (blink  ones) - changed */
66
67int RECV_PIN = 12;      /* Infrared sensor */
68
69const  int buffSize = 412; /* Size of recording buffer */
70
71int startBase = 90;
72int  startShoulder = 42;
73int startElbow = 50;
74int startGripper = 30;
75
76int  posBase = 90;
77int posShoulder = 90;
78int posElbow = 90;
79int posGripper =  90;
80
81int lastBase = 90;
82int lastShoulder = 90;
83int lastElbow = 90;
84int  lastGripper = 90;
85
86int minBase = 0;
87int maxBase = 150;
88int minShoulder  = 25;
89int maxShoulder = 135;
90int minElbow = 10;
91int maxElbow = 118;
92int  minGripper = 10;
93int maxGripper = 61;
94
95const int countServo = 4;
96byte  currentPos[countServo] = {
97  0, 0, 0, 0};
98byte lastPos[countServo] = {
99  255, 255, 255, 255};
100byte minPos[countServo] = {
101  minBase, minShoulder,  minElbow, minGripper};
102byte maxPos[countServo] = {
103  maxBase, maxShoulder,  maxElbow, maxGripper};
104int servoPin[countServo] = {
105  basePin, shoulderPin,  elbowPin, gripperPin};
106
107
108uint16_t buff[buffSize];
109uint16_t buffAdd[countServo];
110int  recPos = 0;
111int playPos = 0;
112int blockPlayPos = 0;
113
114int buffPos = 0;
115
116struct  tPlayBuff
117{
118  byte ServoNumber;
119  byte Angle;
120};
121
122tPlayBuff playBuff[countServo];
123int  playBuffCount = 0;
124
125byte cyclesRecord = 0;
126byte cyclesPlay = 0;
127
128int  buttonRecord = HIGH;
129int buttonPlay = HIGH;
130
131int buttonRecordLast = LOW;
132int  buttonPlayLast = LOW;
133
134bool record = false;
135bool play = false;
136
137String  command = "Manual";
138int printPos = 0;
139
140int buttonPlayDelay = 20;
141int  buttonPlayCount = 0;
142
143bool ledLight = false;
144
145int servoTime = 0;
146
147float  dx = 0;
148float dy = 0;
149float dz = 0;
150float dg = 0;
151
152Servo servoBase;
153Servo  servoShoulder;
154Servo servoElbow;
155Servo servoGripper;
156
157#if defined infraredSensor
158IRrecv  irrecv(RECV_PIN);
159
160decode_results results;
161
162// Please, replace codes  of infrared controller with your own codes 
163// in function CheckIrController
164void  CheckIrController()
165{
166  if (irrecv.decode(&results))
167  {
168    switch  (results.value)
169    {
170    case 0xBE15326E:
171    case 0xDB9D3097:
172      dx  = 5;
173      break;
174    case 0x9912B99A:
175    case 0x9FA96F:
176      dx =  -5;
177      break;
178    case 0xE5139CA7:
179    case 0xAC516266:
180      dy  = 5;
181      break;
182    case 0xE4139B12:
183    case 0xAD5163FB:
184      dy  = -5;
185      break;
186    case 0x21:
187    case 0x821:
188      dz = -5;
189      break;
190    case 0x20:
191    case 0x820:
192      dz = 5;
193      break;
194    case 0x11:
195    case 0x811:
196      dg = -5;
197      break;
198    case  0x10:
199    case 0x810:
200      dg = 5;
201      break;
202    case 0x1C102884:
203    case 0x4B995E59:
204      buttonRecord = !buttonRecord;
205      break;
206    case 0xC460AC26:
207    case 0x6626F67F:
208      buttonPlay = !buttonPlay;
209      break;
210    case 0xDF50BC53:
211    case 0xF04EE0E6:
212      repeatePlaying  = !repeatePlaying;
213
214      if (repeatePlaying)
215      {
216        record  = false;
217        play = true;  
218      }
219      else
220      {
221        play  = false;
222      }
223      break;
224    }
225  }
226}
227#endif
228
229void setup()  {
230  Serial.begin(9600);
231
232  pinMode(xdirPin, INPUT);
233  pinMode(ydirPin,  INPUT);
234  pinMode(zdirPin, INPUT);
235  pinMode(gdirPin, INPUT);
236
237  pinMode(pinRecord,  INPUT_PULLUP);
238  pinMode(pinPlay, INPUT_PULLUP);
239
240  pinMode(pinLedRecord,  OUTPUT);
241
242  servoBase.attach(basePin);
243  servoShoulder.attach(shoulderPin);
244  servoElbow.attach(elbowPin);
245  servoGripper.attach(gripperPin);
246
247  StartPosition();
248
249#if  defined infraredSensor
250  irrecv.enableIRIn(); // Start the receiver
251#endif  
252
253  digitalWrite(pinLedRecord, HIGH);
254  delay(1000);
255  digitalWrite(pinLedRecord,  LOW);
256}
257
258void loop() {  
259  buttonRecord = digitalRead(pinRecord);
260  buttonPlay = digitalRead(pinPlay);
261
262  dx = 0;
263  dy = 0;
264  dz = 0;
265  dg = 0;
266
267
268#if defined infraredSensor
269  CheckIrController();
270
271  if (debug)
272  {
273    Serial.println(results.value, HEX);
274  }
275
276  irrecv.resume();  // Receive the next value
277#endif  
278
279  //  Serial.print(buttonRecord);
280  //  Serial.print("\	");
281  //  Serial.println(buttonPlay);
282  //  for testing  purposes
283
284  if (buttonPlay == LOW)
285  {
286    buttonPlayCount++;
287
288    if (buttonPlayCount >= buttonPlayDelay)
289    {
290      repeatePlaying = true;
291    }
292  }
293  else buttonPlayCount = 0;
294
295  if (buttonPlay != buttonPlayLast)
296  {
297    if (record)
298    {
299      record = false;
300    }
301
302    if  (buttonPlay == LOW)
303    {
304      play = !play;
305      repeatePlaying = false;
306
307      if (play)
308      {
309        //  StartPosition();
310      }
311    }
312    buttonPlayLast = buttonPlay;
313  }
314
315  if (buttonRecord != buttonRecordLast)
316  {
317    if (buttonRecord == LOW)
318    {
319      record = !record;
320
321      if (record)
322      {
323        play = false;
324        repeatePlaying  = false;
325        recPos = 0;
326        cyclesRecord = 0;
327      }
328      else
329      {
330        if (debug) PrintBuffer();
331      }
332    }
333    buttonRecordLast  = buttonRecord;
334  }
335
336  if (repeatePlaying)
337  {
338    play = true;
339  }
340
341  if (dx == 0) dx = map(analogRead(xdirPin), 0, 1023, 3.5, -3.5);
342  if (dy == 0) dy = map(analogRead(ydirPin), 0, 1023, 4.0, -4.0);
343  if (dz ==  0) dz = map(analogRead(zdirPin), 0, 1023, 4.0, -4.0);
344  if (dg == 0) dg = map(analogRead(gdirPin),  0, 1023, -4.0, 4.0);
345
346  if (abs(dx) < 1.5) dx = 0;
347  if (abs(dy) < 1.5)  dy = 0;
348  if (abs(dz) < 1.5) dz = 0;
349  if (abs(dg) < 1.5) dg = 0;
350
351  posBase += dx;
352  posShoulder += dy;
353  posElbow += dz;
354  posGripper +=  dg;
355
356  if ((play) | (cyclesPlay > 0))
357  {
358    if (cyclesPlay > 0)
359    {
360      cyclesPlay--;
361
362      if (cyclesPlay > 0)
363      {
364        if  (play)
365        {
366          playPos = blockPlayPos;
367        }
368        else
369        {
370          play = false;
371        }
372      }
373    }
374
375    if  (play)
376    {
377      if (playPos >= recPos)
378      {
379        playPos =  0;
380
381        if (repeatePlaying)
382        {
383          if (debug)
384          {
385            Serial.println("Auto start ");
386          }
387
388          delay(delayBetweenCycles);
389          // StartPosition();
390        }
391        else
392        {
393          play  = false;
394          repeatePlaying = false;
395        }
396      }
397
398      blockPlayPos  = playPos;
399      playBuffCount = 0;
400
401      bool endOfData = false;
402
403      while (!endOfData)
404      {
405        if (playPos >= buffSize - 1) break;
406        if (playPos >= recPos) break;
407
408        uint16_t data = buff[playPos];
409        byte angle = data & 0xFF;
410        uint16_t servoNumber = data & 0xf00;
411        endOfData = data & 0x8000;
412        uint16_t repeatCycles = data & 0x7000;
413        repeatCycles = repeatCycles >> 12;
414
415        if (play & cyclesPlay  <= 0)
416        {
417          cyclesPlay = repeatCycles;
418        }
419
420        servoNumber = servoNumber >> 8;
421
422        playBuff[playBuffCount].ServoNumber  = servoNumber;
423        playBuff[playBuffCount].Angle = angle;
424
425        playBuffCount++;
426
427        switch (servoNumber)
428        {
429        case 0:
430          posBase  = angle;
431          break;
432
433        case 1:
434          posShoulder = angle;
435          break;
436
437        case 2:
438          posElbow = angle;
439          break;
440
441        case 3:
442          posGripper = angle;
443          dg = posGripper -  lastGripper;
444          break;
445        }
446
447        //        if (debug)
448        //        {
449        //          Serial.print("Play data ");
450        //          Serial.print(data, BIN);
451        //          Serial.print(" servo  ");
452        //          Serial.print(servoNumber);
453        //          Serial.print("  angle ");
454        //          Serial.print(angle);
455        //          Serial.print("  end=");
456        //          Serial.println(endOfData);
457        //        }
458        //
459
460        playPos++;
461
462        if (playPos >= recPos)
463        {
464          play = false;
465
466          break;
467        }
468      }
469    }
470  }
471  else
472  {
473    cyclesPlay = 0;
474  }
475
476  if (posBase > maxBase)  posBase = maxBase;
477  if (posShoulder > maxShoulder) posShoulder = maxShoulder;
478  if (posElbow > maxElbow) posElbow = maxElbow;
479  if (posGripper > maxGripper)  posGripper = maxGripper;
480
481  if (posBase < minBase) posBase = minBase;
482  if (posShoulder < minShoulder) posShoulder = minShoulder;
483  if (posElbow <  minElbow) posElbow = minElbow;
484  if (posGripper < minGripper) posGripper = minGripper;
485
486  bool waitGripper = false;
487  if (dg < 0) {
488    posGripper = minGripper;
489    waitGripper = true;
490  }
491  else if (dg > 0) {
492    posGripper = maxGripper;
493    waitGripper = true;
494  }
495
496  if (play && waitGripper)
497  {
498    delay(1000);
499  }
500
501  currentPos[0] = posBase;
502  currentPos[1] = posShoulder;
503  currentPos[2]  = posElbow;
504  currentPos[3] = posGripper;
505
506  bool positionChanged = false;
507
508  if ((play) | (cyclesPlay > 0))
509  {
510    for (int i = 0; i < playBuffCount;  i++)
511    {
512      MoveServo(servoPin[playBuff[i].ServoNumber], playBuff[i].Angle,  servoTime);
513    }
514  }
515  else {
516    for (int i = 0; i < countServo; i++)
517    {
518      if (currentPos[i] > maxPos[i]) currentPos[i] = maxPos[i];
519      if  (currentPos[i] < minPos[i]) currentPos[i] = minPos[i];
520
521      if (currentPos[i]  != lastPos[i])
522      {
523        positionChanged = true;
524        MoveServo(servoPin[i],  currentPos[i], servoTime);
525      }
526    }
527  }
528
529  if (positionChanged)
530  {
531    if (record)
532    {
533      buffPos = 0;
534      cyclesRecord = 0;
535
536      for (int i = 0; i < countServo; i++)
537      {
538        if (lastPos[i]  != currentPos[i])
539        {
540          uint16_t value = (currentPos[i] | (0x100  * i)) & 0x0FFF;
541          buffAdd[buffPos] = value;
542          buffPos++;
543        }
544      }
545      buffAdd[buffPos - 1] = buffAdd[buffPos - 1] | 0x8000;
546
547      AddToBuff();
548    }
549  }
550  else
551  {
552    if (record)
553    {
554      if (recPos > 0)
555      {
556        cyclesRecord ++;
557
558        bool  added = false;
559        bool first = true;
560
561        for (int i = recPos  - 1; i >= 0; i--)
562        {
563          bool endOfData = buff[i] & 0x8000;
564
565          if (!first && endOfData) break;
566
567          if (cyclesRecord > 7)
568          {
569            cyclesRecord = 0;
570
571            AddToBuff();
572            added = true;
573          }
574
575          if (!added) {
576            uint16_t  val = cyclesRecord & 7;
577            val = val << 12;
578            val = val  & 0x7000;
579
580            buff[i] = buff[i] & 0x8FFF;
581            buff[i]  = buff[i] | val;
582
583            //            if (debug)
584            //            {
585            //              Serial.print(" i=");
586            //              Serial.print(i);
587            //              Serial.print(" Buff[i]=");
588            //              Serial.print(buff[i],  BIN);
589            //              Serial.print(" val=");
590            //              Serial.println(val, BIN);
591            //            }
592
593            first  = false;
594          }
595        }
596      }
597    }
598  }
599
600  lastBase  = posBase;
601  lastShoulder = posShoulder;
602  lastElbow = posElbow;
603  lastGripper  = posGripper;
604
605  for (int i = 0; i < countServo; i++)
606  {
607    lastPos[i]  = currentPos[i];
608  }
609
610  if ( repeatePlaying)
611  {
612    ledLight = !ledLight;
613  }
614  else
615  {
616    if (ledLight)
617    {
618      ledLight = false;
619    }
620
621    if (record)
622    {
623      ledLight = true;
624    }
625  };
626
627  digitalWrite(pinLedRecord, ledLight);
628  delay(50);
629}
630
631void AddToBuff()
632{
633  for (int i = 0; i < buffPos; i++)
634  {
635    buff[recPos + i] = buffAdd[i];
636  }
637
638  recPos += buffPos;
639
640  if (recPos >= buffSize - countServo)
641  {
642    record = false;
643  }
644}
645
646void MoveServo(uint8_t idServo, int  angle, int timeMs)
647{
648  switch (idServo)
649  {
650  case basePin:
651    servoBase.write(angle);
652    break;
653  case shoulderPin:
654    servoShoulder.write(angle);
655    break;
656  case elbowPin:
657    servoElbow.write(angle);
658    break;
659  case gripperPin:
660    servoGripper.write(angle);
661    break;
662  }
663
664  if (debug)
665  {
666    if (record) Serial.print(" Record ");
667    if (play) Serial.print(" Play  ");
668    if (repeatePlaying) Serial.print(" Auto ");
669
670    Serial.print("  Servo ");
671    Serial.print(idServo);
672    Serial.print(" Angle ");
673    Serial.println(angle);
674  }
675}
676
677void PrintBuffer()
678{
679  for (int i = 0; i < recPos; i++)
680  {
681    uint16_t data = buff[i];
682    byte angle = data & 0xFF;
683    uint16_t  servoNumber = data & 0xF00;
684    servoNumber = servoNumber >> 8;
685    bool endOfData  = data & 0x8000;
686
687    Serial.print("Servo=");
688    Serial.print(servoNumber,  HEX);
689    Serial.print("\	Angle=");
690    Serial.print(angle);
691    Serial.print("\	End=");
692    Serial.print(endOfData);
693    Serial.print("\	Data=");
694    Serial.print(data,  BIN);
695    Serial.println();
696  }
697}
698
699void StartPosition()
700{
701  int angleBase = servoBase.read();
702  int angleShoulder = servoShoulder.read();
703  int angleElbow = servoElbow.read();
704  int angleGripper = servoGripper.read();
705
706  Serial.print(angleBase);
707  Serial.print("\	");
708  Serial.print(angleShoulder);
709  Serial.print("\	");
710  Serial.print(angleElbow);
711  Serial.print("\	");
712  Serial.print(angleGripper);
713  Serial.println("\	");
714
715  posBase = startBase;
716  posShoulder = startShoulder;
717  posElbow = startElbow;
718  posGripper = startGripper;
719
720  servoBase.write(posBase);
721  servoShoulder.write(posShoulder);
722  servoElbow.write(posElbow);
723  servoGripper.write(posGripper);
724}
725

/* meArm analog joysticks version 1.6.1.4 - UtilStudio.com Jan 2019
  Uses two analogue joysticks and four servos.
 
 Version 1.6.1.x is targeted  to MeArm Joystick board version 1.6.1.
 
 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.
 
 Please  send a short message as a feedback, if you uses this code. 
 Thank you. ;-)
  
 
 Pins:
 Arduino  Stick1 Stick2 Base   Shoulder Elbow  Gripper Record/    Infrared
 GND      GND    GND    Brown  Brown    Brown  Brown   Auto play  sensor
 5V       VCC    VCC    Red    Red      Red    Red     LED
 A0       HOR
  A1       VER
 PD2      BUTT
 A2              HOR
 A3              VER
  PD4             BUTT
 11                     Yellow
 10                            Yellow
  9                                      Yellow
 5                                             Yellow
  PD3                                                   X
 12                                                               X
  */

//#define infraredSensor true; // Uncomment this line, if you want to  use Infrared controller. 

/* 
 And replace codes of infrared controller  with your own codes in function CheckIrController.
 Set debug to true and use  a Serial monitor of your Arduino IDE to show received codes of your remote controller.
  */

#include <Servo.h>

#if defined infraredSensor
#include <IRremote.h>  
#endif                      

bool debug = false;

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

const int basePin = 11;       /* Base  servo */
const int shoulderPin = 10;   /* Shoulder servo */
const int elbowPin  = 9;       /* Elbow servo */
const int gripperPin = 5;     /* Gripper servo -  changed */

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

int pinRecord = PD2;     /* Button record */
int pinPlay = PD4;       /* Button play - changed */
int  pinLedRecord = PD3;  /* LED - indicates recording (light) or auto play mode (blink  ones) - changed */

int RECV_PIN = 12;      /* Infrared sensor */

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

int startBase = 90;
int  startShoulder = 42;
int startElbow = 50;
int startGripper = 30;

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

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

int minBase = 0;
int maxBase = 150;
int minShoulder  = 25;
int maxShoulder = 135;
int minElbow = 10;
int maxElbow = 118;
int  minGripper = 10;
int maxGripper = 61;

const int countServo = 4;
byte  currentPos[countServo] = {
  0, 0, 0, 0};
byte lastPos[countServo] = {
  255, 255, 255, 255};
byte minPos[countServo] = {
  minBase, minShoulder,  minElbow, minGripper};
byte maxPos[countServo] = {
  maxBase, maxShoulder,  maxElbow, maxGripper};
int servoPin[countServo] = {
  basePin, shoulderPin,  elbowPin, gripperPin};


uint16_t buff[buffSize];
uint16_t buffAdd[countServo];
int  recPos = 0;
int playPos = 0;
int blockPlayPos = 0;

int buffPos = 0;

struct  tPlayBuff
{
  byte ServoNumber;
  byte Angle;
};

tPlayBuff playBuff[countServo];
int  playBuffCount = 0;

byte cyclesRecord = 0;
byte cyclesPlay = 0;

int  buttonRecord = HIGH;
int buttonPlay = HIGH;

int buttonRecordLast = LOW;
int  buttonPlayLast = LOW;

bool record = false;
bool play = false;

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

int buttonPlayDelay = 20;
int  buttonPlayCount = 0;

bool ledLight = false;

int servoTime = 0;

float  dx = 0;
float dy = 0;
float dz = 0;
float dg = 0;

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

#if defined infraredSensor
IRrecv  irrecv(RECV_PIN);

decode_results results;

// Please, replace codes  of infrared controller with your own codes 
// in function CheckIrController
void  CheckIrController()
{
  if (irrecv.decode(&results))
  {
    switch  (results.value)
    {
    case 0xBE15326E:
    case 0xDB9D3097:
      dx  = 5;
      break;
    case 0x9912B99A:
    case 0x9FA96F:
      dx =  -5;
      break;
    case 0xE5139CA7:
    case 0xAC516266:
      dy  = 5;
      break;
    case 0xE4139B12:
    case 0xAD5163FB:
      dy  = -5;
      break;
    case 0x21:
    case 0x821:
      dz = -5;
      break;
    case 0x20:
    case 0x820:
      dz = 5;
      break;
    case 0x11:
    case 0x811:
      dg = -5;
      break;
    case  0x10:
    case 0x810:
      dg = 5;
      break;
    case 0x1C102884:
    case 0x4B995E59:
      buttonRecord = !buttonRecord;
      break;
    case 0xC460AC26:
    case 0x6626F67F:
      buttonPlay = !buttonPlay;
      break;
    case 0xDF50BC53:
    case 0xF04EE0E6:
      repeatePlaying  = !repeatePlaying;

      if (repeatePlaying)
      {
        record  = false;
        play = true;  
      }
      else
      {
        play  = false;
      }
      break;
    }
  }
}
#endif

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

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

  pinMode(pinRecord,  INPUT_PULLUP);
  pinMode(pinPlay, INPUT_PULLUP);

  pinMode(pinLedRecord,  OUTPUT);

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

  StartPosition();

#if  defined infraredSensor
  irrecv.enableIRIn(); // Start the receiver
#endif  

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

void loop() {  
  buttonRecord = digitalRead(pinRecord);
  buttonPlay = digitalRead(pinPlay);

  dx = 0;
  dy = 0;
  dz = 0;
  dg = 0;


#if defined infraredSensor
  CheckIrController();

  if (debug)
  {
    Serial.println(results.value, HEX);
  }

  irrecv.resume();  // Receive the next value
#endif  

  //  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();
      }
    }
    buttonPlayLast = buttonPlay;
  }

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

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

  if (repeatePlaying)
  {
    play = true;
  }

  if (dx == 0) dx = map(analogRead(xdirPin), 0, 1023, 3.5, -3.5);
  if (dy == 0) dy = map(analogRead(ydirPin), 0, 1023, 4.0, -4.0);
  if (dz ==  0) dz = map(analogRead(zdirPin), 0, 1023, 4.0, -4.0);
  if (dg == 0) dg = map(analogRead(gdirPin),  0, 1023, -4.0, 4.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) | (cyclesPlay > 0))
  {
    if (cyclesPlay > 0)
    {
      cyclesPlay--;

      if (cyclesPlay > 0)
      {
        if  (play)
        {
          playPos = blockPlayPos;
        }
        else
        {
          play = false;
        }
      }
    }

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

        if (repeatePlaying)
        {
          if (debug)
          {
            Serial.println("Auto start ");
          }

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

      blockPlayPos  = playPos;
      playBuffCount = 0;

      bool endOfData = false;

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

        uint16_t data = buff[playPos];
        byte angle = data & 0xFF;
        uint16_t servoNumber = data & 0xf00;
        endOfData = data & 0x8000;
        uint16_t repeatCycles = data & 0x7000;
        repeatCycles = repeatCycles >> 12;

        if (play & cyclesPlay  <= 0)
        {
          cyclesPlay = repeatCycles;
        }

        servoNumber = servoNumber >> 8;

        playBuff[playBuffCount].ServoNumber  = servoNumber;
        playBuff[playBuffCount].Angle = angle;

        playBuffCount++;

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

        case 1:
          posShoulder = angle;
          break;

        case 2:
          posElbow = angle;
          break;

        case 3:
          posGripper = angle;
          dg = posGripper -  lastGripper;
          break;
        }

        //        if (debug)
        //        {
        //          Serial.print("Play data ");
        //          Serial.print(data, BIN);
        //          Serial.print(" servo  ");
        //          Serial.print(servoNumber);
        //          Serial.print("  angle ");
        //          Serial.print(angle);
        //          Serial.print("  end=");
        //          Serial.println(endOfData);
        //        }
        //

        playPos++;

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

          break;
        }
      }
    }
  }
  else
  {
    cyclesPlay = 0;
  }

  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;

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

  if (play && waitGripper)
  {
    delay(1000);
  }

  currentPos[0] = posBase;
  currentPos[1] = posShoulder;
  currentPos[2]  = posElbow;
  currentPos[3] = posGripper;

  bool positionChanged = false;

  if ((play) | (cyclesPlay > 0))
  {
    for (int i = 0; i < playBuffCount;  i++)
    {
      MoveServo(servoPin[playBuff[i].ServoNumber], playBuff[i].Angle,  servoTime);
    }
  }
  else {
    for (int i = 0; i < countServo; i++)
    {
      if (currentPos[i] > maxPos[i]) currentPos[i] = maxPos[i];
      if  (currentPos[i] < minPos[i]) currentPos[i] = minPos[i];

      if (currentPos[i]  != lastPos[i])
      {
        positionChanged = true;
        MoveServo(servoPin[i],  currentPos[i], servoTime);
      }
    }
  }

  if (positionChanged)
  {
    if (record)
    {
      buffPos = 0;
      cyclesRecord = 0;

      for (int i = 0; i < countServo; i++)
      {
        if (lastPos[i]  != currentPos[i])
        {
          uint16_t value = (currentPos[i] | (0x100  * i)) & 0x0FFF;
          buffAdd[buffPos] = value;
          buffPos++;
        }
      }
      buffAdd[buffPos - 1] = buffAdd[buffPos - 1] | 0x8000;

      AddToBuff();
    }
  }
  else
  {
    if (record)
    {
      if (recPos > 0)
      {
        cyclesRecord ++;

        bool  added = false;
        bool first = true;

        for (int i = recPos  - 1; i >= 0; i--)
        {
          bool endOfData = buff[i] & 0x8000;

          if (!first && endOfData) break;

          if (cyclesRecord > 7)
          {
            cyclesRecord = 0;

            AddToBuff();
            added = true;
          }

          if (!added) {
            uint16_t  val = cyclesRecord & 7;
            val = val << 12;
            val = val  & 0x7000;

            buff[i] = buff[i] & 0x8FFF;
            buff[i]  = buff[i] | val;

            //            if (debug)
            //            {
            //              Serial.print(" i=");
            //              Serial.print(i);
            //              Serial.print(" Buff[i]=");
            //              Serial.print(buff[i],  BIN);
            //              Serial.print(" val=");
            //              Serial.println(val, BIN);
            //            }

            first  = false;
          }
        }
      }
    }
  }

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

  for (int i = 0; i < countServo; i++)
  {
    lastPos[i]  = currentPos[i];
  }

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

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

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

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

  recPos += buffPos;

  if (recPos >= buffSize - countServo)
  {
    record = false;
  }
}

void MoveServo(uint8_t idServo, int  angle, int timeMs)
{
  switch (idServo)
  {
  case basePin:
    servoBase.write(angle);
    break;
  case shoulderPin:
    servoShoulder.write(angle);
    break;
  case elbowPin:
    servoElbow.write(angle);
    break;
  case gripperPin:
    servoGripper.write(angle);
    break;
  }

  if (debug)
  {
    if (record) Serial.print(" Record ");
    if (play) Serial.print(" Play  ");
    if (repeatePlaying) Serial.print(" Auto ");

    Serial.print("  Servo ");
    Serial.print(idServo);
    Serial.print(" Angle ");
    Serial.println(angle);
  }
}

void PrintBuffer()
{
  for (int i = 0; i < recPos; i++)
  {
    uint16_t data = buff[i];
    byte angle = data & 0xFF;
    uint16_t  servoNumber = data & 0xF00;
    servoNumber = servoNumber >> 8;
    bool endOfData  = data & 0x8000;

    Serial.print("Servo=");
    Serial.print(servoNumber,  HEX);
    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 ® 1.6.1 robot joystick board recording movements with infrared sensor

Scheme diagram with using of infrared sensor (it is not necessary).

MeArm ® 1.6.1 robot joystick board recording movements with infrared sensor

Comments

Only logged in users can leave comments

utilstudio

0 Followers

•

0 Projects

Intro