Arduino koda pomoč.

Na začetku kode v globalnih spremenljivkaj moraš definirati pine, kamor si priklopil dodaten joystick in servota ter parametre, v setup funkciji določiti, kaj je input/output, nato pa copy/paste obstoječih delov kode od vrstice 197 dalje.

Skratka, na danem primeru 2J in 4S si oglej kodo in premisli, čemu je namenjena posamezna vrstica. Nato ne bi smelo biti pretežko dodati dopolnitev za tvoj scenarij.

Oglej si switch, ki se začne v vrstici 237 ter if od vrstice 304 dalje in print funkcijo od vrstice 405 naprej. Povečaj buffer in dodaj vnose za dodatna servota.

Pa priporočam branje: Bitwise operation @ Wikipedia

To je zdaj spremenjena koda , vendar težava ostaja. Nevem kaj bi še lahko spremenil, dodal? Servoti morajo biti priklopljeni na PWM pine , ali to ni obvezno?

/*

   meArm analog joysticks version 1.5.4 - UtilStudio.com Dec 2018

   Uses two analogue joysticks and four servos.

   In version 1.5.4 was replaced an external resistors by internal pull up resistors.
   External LED diode was replaced with using internal "L" diode on Arduino UNO board.
   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
       13                                                             X
*/


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 forearmPin = 5;        /* ForeArm servo */
int wristPin = 4;          /* Wrist servo*/

int xdirPin = 0;        /* Base - joystick1*/
int ydirPin = 1;        /* Shoulder - joystick1 */
int zdirPin = 3;        /* Elbow - joystick2 */
int gdirPin = 2;        /* Gripper - joystick2 */
int sdirPin = 4;        /* ForeArm - joystick3 */
int rdirPin = 5;        /* Wrist - joystick3 */

//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 = 13;   /* LED - indicates recording (light) or auto play mode (blink ones) */

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

int startBase = 135;
int startShoulder = 95;
int startElbow = 90;
int startGripper = 0;
int startForeArm = 155;
int startWrist = 150;

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

int lastBase = 90;
int lastShoulder = 90;
int lastElbow = 90;
int lastGripper = 0;
int lastForeArm = 90;
int lastWrist = 90;

int minBase = 10;
int maxBase = 150;
int minShoulder = 10;
int maxShoulder = 150;
int minElbow = 0;
int maxElbow = 180;
int minGripper = 100;
int maxGripper = 150;
int minForeArm = 0;
int maxForeArm = 180;
int minWrist = 0;
int maxWrist = 180;

const int countServo = 6;
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;
Servo servoForeArm;
Servo servoWrist;

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

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

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

  pinMode(pinLedRecord, OUTPUT);

  servoBase.attach(basePin);
  servoShoulder.attach(shoulderPin);
  servoElbow.attach(elbowPin);
  servoGripper.attach(gripperPin);
  servoForeArm.attach(forearmPin);
  servoWrist.attach(wristPin);
  
 

  StartPosition();

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

void loop() {

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

  //  Serial.print(buttonRecord);
  //  Serial.print("\t");
  //  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);
  float ds = map(analogRead(sdirPin), 0, 1023, -5.0, 5.0);
  float dr = map(analogRead(rdirPin), 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;
  if (abs(ds) < 1.5) ds = 0;
  if (abs(dr) < 1.5) dr = 0;

  posBase += dx;
  posShoulder += dy;
  posElbow += dz;
  posGripper += dg;
  posForeArm += ds;
  posWrist += dr;

  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 & 0x5000;
      endOfData = data & 0x6000;

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

        case 0x1000:
          posShoulder = angle;
          break;

        case 0x2000:
          posElbow = angle;
          break;

        case 0x3000:
          posGripper = angle;
           break;

        case 0x4000:
          posForeArm = angle;
          break;

        case 0x5000:
          posWrist = angle;
          ds = posWrist - lastWrist;
          break;    
      }

      playPos++;
    }
  }

  if (posBase > maxBase) posBase = maxBase;
  if (posShoulder > maxShoulder) posShoulder = maxShoulder;
  if (posElbow > maxElbow) posElbow = maxElbow;
  if (posGripper > maxGripper) posGripper = maxGripper;
  if (posForeArm > maxForeArm) posForeArm = maxForeArm;
  if (posWrist > maxWrist) posWrist = maxWrist;

  if (posBase < minBase) posBase = minBase;
  if (posShoulder < minShoulder) posShoulder = minShoulder;
  if (posElbow < minElbow) posElbow = minElbow;
  if (posGripper < minGripper) posGripper = minGripper;
  if (posForeArm < minForeArm) posForeArm = minForeArm;
  if (posWrist < minWrist) posWrist = minWrist;

  servoBase.write(posBase);
  servoShoulder.write(posShoulder);
  servoElbow.write(posElbow);
  servoForeArm.write(posForeArm);
  servoWrist.write(posWrist);

  //  if (dg < -3.0) { 
  //    posGripper = minGripper;
  //    servoGripper.write(posGripper);
  //    Serial.println(posGripper);
  //  }
  //  else if (dg > 3.0) {
  //    posGripper = maxGripper;
  //    servoGripper.write(posGripper);
  //    Serial.println(posGripper);
  //  }

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

  //Serial.println(posGripper);

  if ((lastBase != posBase) | (lastShoulder != posShoulder) | (lastElbow != posElbow) | (lastGripper != posGripper) | (lastForeArm != posForeArm) | (lastWrist != posWrist))
  {
    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++;
        }

        if (lastForeArm != posForeArm)
        {
          buffAdd[buffPos] = posForeArm | 0x4000;
          buffPos++;
        }
         if (lastWrist != posWrist)
        {
          buffAdd[buffPos] = posWrist | 0x5000;
          buffPos++;
        }   

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

        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(posForeArm);
    Serial.print("\t");
    Serial.print(posWrist);
    Serial.print("\t");
    Serial.print(record);
    Serial.print("\t");
    Serial.print(play);
    Serial.println();
  }

  lastBase = posBase;
  lastShoulder = posShoulder;
  lastElbow = posElbow;
  lastGripper = posGripper;
  lastForeArm = posForeArm;
  lastWrist = posWrist;

  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 & 0x5000;
    bool endOfData = data & 0x6000;

    Serial.print("Servo=");
    Serial.print(servoNumber);
    Serial.print("\tAngle=");
    Serial.print(angle);
    Serial.print("\tEnd=");
    Serial.print(endOfData);
    Serial.print("\tData=");
    Serial.print(data, BIN);
    Serial.println();
  }
}

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

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

  posBase = startBase;
  posShoulder = startShoulder;
  posElbow = startElbow;
  posGripper = startGripper;
  posForeArm = startForeArm;
  posWrist = startWrist;

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