Contrôleur joystick de précision pour robot MeArm – Enregistrement et relecture de coordonnées avec Arduino Uno

Composants et fournitures

Composant	Quantité
Arduino UNO	1
Joystick analogique (générique)	2
Résistance 10 kΩ	2
Résistance 680 Ω	1
LED 5 mm rouge	1
Micro servo SG90	4

Applications et services en ligne

• Arduino IDE

À propos de ce projet

Contrôleur joystick Arduino Uno pour bras robotique (MeArm®) avec enregistrement des coordonnées. Permet de rejouer les coordonnées enregistrées une fois sur simple clic ou en boucle. Utilise la carte Arduino Uno de base sans carte additionnelle. Nécessite une connexion USB (2A) ou une alimentation 6V / 2A. Testé avec quatre servos SG90.

Fonctionnement :

• Appuyez sur le bouton du joystick 1 pour démarrer l'enregistrement des coordonnées.
• Appuyez à nouveau sur le bouton du joystick 1 pour arrêter l'enregistrement.
• Appuyez sur le bouton du joystick 2 pour démarrer la relecture des coordonnées enregistrées.
• Appuyez à nouveau sur le bouton du joystick 2 pour arrêter la relecture.
• Maintenez le bouton du joystick 2 enfoncé (2 secondes) pour lancer la relecture automatique en boucle.

Note : Nous avons également réalisé une version simplifiée ne nécessitant pas de résistances et de LED supplémentaires : MeArm Robot - Recording of Coordinates (Minimalized Version).

Si vous possédez une carte joystick MeArm, voici une version pour vous : MeArm 1.6.1 Robot Joystick Board Recording Movements (IR).

Code

Contrôleur joystick Arduino pour robot MeArm® avec enregistrement de coordonnées (version 1.3.1)

/* meArm joysticks analogiques version 1.3.1 - UtilStudio.com Dec 2018
 * Utilise deux joysticks analogiques et quatre servos.
 * Amélioration de l'enregistrement des coordonnées en v1.3. Correction de bugs.
 * Premier joystick : avance, recule, gauche, droite du gripper ; bouton = start/stop enregistrement.
 * Second joystick : haut, bas, ouverture/fermeture gripper ; bouton = start/stop lecture.
 * Appui 2 sec sur bouton 2 = lecture auto en boucle.
 * Broches :
 * Arduino     Stick1      Stick2      Base   Shoulder  Elbow  Gripper  Record/
 * GND         GND         GND         Brown  Brown     Brown  Brown    Auto play
 * 5V          VCC         VCC         Red    Red       Red    Red
 * A0          HOR         A2 HOR
 * A1          VER         A3 VER
 * PD2         BUTT        PD3 BUTT
 * 11          Yellow                      Base
 * 10          Yellow                      Shoulder
 * 9           Yellow                      Elbow
 * 6           Yellow                      Gripper
 * PD4         X                           LED
 */
#include <Servo.h>

bool repeatePlaying = false;
int delayBetweenCycles = 2000;

int basePin = 11;
int shoulderPin = 10;
int elbowPin = 9;
int gripperPin = 6;

int xdirPin = 0;
int ydirPin = 1;
int zdirPin = 3;
int gdirPin = 2;

int pinRecord = 2;  // PD2 - recommandé (A4 déprécié)
int pinPlay = 3;    // PD3 - recommandé (A5 déprécié)
int pinLedRecord = 4; // PD4 - LED indique enregistrement (allumée) ou auto-play (clignotement)

bool useInternalPullUpResistors = false;
const int buffSize = 512;

int startBase = 90, startShoulder = 90, startElbow = 90, startGripper = 0;
int posBase = 90, posShoulder = 90, posElbow = 90, posGripper = 0;
int lastBase = 90, lastShoulder = 90, lastElbow = 90, lastGripper = 90;
int minBase = 0, maxBase = 150;
int minShoulder = 0, maxShoulder = 150;
int minElbow = 0, maxElbow = 150;
int minGripper = 0, maxGripper = 150;

const int countServo = 4;
int buff[buffSize];
int buffAdd[countServo];
int recPos = 0, playPos = 0;
int buttonRecord = HIGH, buttonPlay = HIGH;
int buttonRecordLast = LOW, buttonPlayLast = LOW;
bool record = false, play = false, debug = false;
String command = "Manual";
int printPos = 0;
int buttonPlayDelay = 20, buttonPlayCount = 0;
bool ledLight = false;

Servo servoBase, servoShoulder, servoElbow, 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);

  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++;
    }
  }

  // Limites
  posBase = constrain(posBase, minBase, maxBase);
  posShoulder = constrain(posShoulder, minShoulder, maxShoulder);
  posElbow = constrain(posElbow, minElbow, maxElbow);
  posGripper = constrain(posGripper, minGripper, maxGripper);

  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 && 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] |= 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("\t");
    Serial.print(printPos); Serial.print("\t");
    Serial.print(posBase); Serial.print("\t");
    Serial.print(posShoulder); Serial.print("\t");
    Serial.print(posElbow); Serial.print("\t");
    Serial.print(posGripper); Serial.print("\t");
    Serial.print(record); Serial.print("\t");
    Serial.println(play);
  }

  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("\tAngle="); Serial.print(angle);
    Serial.print("\tEnd="); Serial.print(endOfData);
    Serial.print("\tData="); Serial.println(data, BIN);
  }
}

void StartPosition() {
  int angleBase = servoBase.read();
  int angleShoulder = servoShoulder.read();
  int angleElbow = servoElbow.read();
  int angleGripper = servoGripper.read();
  Serial.print(angleBase); Serial.print("\t");
  Serial.print(angleShoulder); Serial.print("\t");
  Serial.print(angleElbow); Serial.print("\t");
  Serial.println(angleGripper);
  posBase = startBase; posShoulder = startShoulder;
  posElbow = startElbow; posGripper = startGripper;
  servoBase.write(posBase);
  servoShoulder.write(posShoulder);
  servoElbow.write(posElbow);
  servoGripper.write(posGripper);
}

Schémas

Schéma du contrôleur joystick pour robot MeArm® avec enregistrement de coordonnées

Carte de test avec seulement deux servos connectés. Voir le schéma pour connecter 4 servomoteurs.