国外课栈 - 国外电子信息技术视频教程、电子书和博文栈

MeArm 1.6.1机器人操纵杆板移动记录

 二维码 88
发表时间:2019-01-09 16:25


MeArm机器人使用MeArm v1.6.1板,具有移动记录和红外遥控。

本项目中使用的东西



硬件组件


Arduino UNO和Genuino UNO x 1

MeArm操纵杆板(版本1.6.1)x 1

红外模块KY-022 x 1

微型伺服SG90 x4


软件应用程序和在线服务

Arduino IDE


故事介绍

该项目使用MeArm v1 .6.1操纵杆板。

操纵杆板MeArm v1.6.1

允许4种模式:

手动模式

记录坐标 - 由LED二极管的恒光指示。

播放模式 - 播放一次录制的动作

重复播放 - 重复播放录制的动作 - 通过LED二极管闪烁指示。

按下joystick 1上的按钮开始/停止记录坐标。

按下joystick 2上的按钮开始/停止游戏坐标。

在joystick 2上按住按钮2秒,重复开始/停止播放坐标。


原理图


代码


/* 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                     

booldebug=false;

boolrepeatePlaying=false;   /* Repeatedly is running recorded cycle */
intdelayBetweenCycles=2000; /* Delay between cycles */

constintbasePin=11;       /* Base servo */
constintshoulderPin=10;   /* Shoulder servo */
constintelbowPin=9;       /* Elbow servo */
constintgripperPin=5;     /* Gripper servo - changed */

intxdirPin=0;        /* Base - joystick1*/
intydirPin=1;        /* Shoulder - joystick1 */
intzdirPin=2;        /* Elbow - joystick2 */
intgdirPin=3;        /* Gripper - joystick2 */

intpinRecord=PD2;     /* Button record */
intpinPlay=PD4;       /* Button play - changed */
intpinLedRecord=PD3;   /* LED - indicates recording (light) or auto play mode (blink ones) - changed */

intRECV_PIN=12;      /* Infrared sensor */

constintbuffSize=412; /* Size of recording buffer */

intstartBase=90;
intstartShoulder=42;
intstartElbow=50;
intstartGripper=30;

intposBase=90;
intposShoulder=90;
intposElbow=90;
intposGripper=90;

intlastBase=90;
intlastShoulder=90;
intlastElbow=90;
intlastGripper=90;

intminBase=0;
intmaxBase=150;
intminShoulder=25;
intmaxShoulder=135;
intminElbow=10;
intmaxElbow=118;
intminGripper=10;
intmaxGripper=61;

constintcountServo=4;
bytecurrentPos[countServo] = {
  0, 0, 0, 0};
bytelastPos[countServo] = {
  255, 255, 255, 255};
byteminPos[countServo] = {
  minBase, minShoulder, minElbow, minGripper};
bytemaxPos[countServo] = {
  maxBase, maxShoulder, maxElbow, maxGripper};
intservoPin[countServo] = {
  basePin, shoulderPin, elbowPin, gripperPin};


uint16_tbuff[buffSize];
uint16_tbuffAdd[countServo];
intrecPos=0;
intplayPos=0;
intblockPlayPos=0;

intbuffPos=0;

structtPlayBuff
{
  byteServoNumber;
  byteAngle;
};

tPlayBuffplayBuff[countServo];
intplayBuffCount=0;

bytecyclesRecord=0;
bytecyclesPlay=0;

intbuttonRecord=HIGH;
intbuttonPlay=HIGH;

intbuttonRecordLast=LOW;
intbuttonPlayLast=LOW;

boolrecord=false;
boolplay=false;

Stringcommand="Manual";
intprintPos=0;

intbuttonPlayDelay=20;
intbuttonPlayCount=0;

boolledLight=false;

intservoTime=0;

floatdx=0;
floatdy=0;
floatdz=0;
floatdg=0;

ServoservoBase;
ServoservoShoulder;
ServoservoElbow;
ServoservoGripper;

#if defined infraredSensor
IRrecvirrecv(RECV_PIN);

decode_resultsresults;

// Please, replace codes of infrared controller with your own codes
// in function CheckIrController
voidCheckIrController()
{
  if (irrecv.decode(&results))
  {
    switch (results.value)
    {
    case0xBE15326E:
    case0xDB9D3097:
      dx=5;
      break;
    case0x9912B99A:
    case0x9FA96F:
      dx=-5;
      break;
    case0xE5139CA7:
    case0xAC516266:
      dy=5;
      break;
    case0xE4139B12:
    case0xAD5163FB:
      dy=-5;
      break;
    case0x21:
    case0x821:
      dz=-5;
      break;
    case0x20:
    case0x820:
      dz=5;
      break;
    case0x11:
    case0x811:
      dg=-5;
      break;
    case0x10:
    case0x810:
      dg=5;
      break;
    case0x1C102884:
    case0x4B995E59:
      buttonRecord=!buttonRecord;
      break;
    case0xC460AC26:
    case0x6626F67F:
      buttonPlay=!buttonPlay;
      break;
    case0xDF50BC53:
    case0xF04EE0E6:
      repeatePlaying=!repeatePlaying;

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

voidsetup() {
  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);
}

voidloop() {  
  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("\t");
  //   Serial.println(buttonPlay);
  //   for testing purposes

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

    if (buttonPlayCount>=buttonPlayDelay)
    {
      repeatePlaying=true;
    }
  }
  elsebuttonPlayCount=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;

      boolendOfData=false;

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

        uint16_tdata=buff[playPos];
        byteangle=data&0xFF;
        uint16_tservoNumber=data&0xf00;
        endOfData=data&0x8000;
        uint16_trepeatCycles=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)
        {
        case0:
          posBase=angle;
          break;

        case1:
          posShoulder=angle;
          break;

        case2:
          posElbow=angle;
          break;

        case3:
          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;

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

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

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

  boolpositionChanged=false;

  if ((play) | (cyclesPlay>0))
  {
    for (inti=0; i<playBuffCount; i++)
    {
      MoveServo(servoPin[playBuff[i].ServoNumber], playBuff[i].Angle, servoTime);
    }
  }
  else {
    for (inti=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 (inti=0; i<countServo; i++)
      {
        if (lastPos[i] !=currentPos[i])
        {
          uint16_tvalue= (currentPos[i] | (0x100*i)) &0x0FFF;
          buffAdd[buffPos] =value;
          buffPos++;
        }
      }
      buffAdd[buffPos-1] =buffAdd[buffPos-1] |0x8000;

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

        booladded=false;
        boolfirst=true;

        for (inti=recPos-1; i>=0; i--)
        {
          boolendOfData=buff[i] &0x8000;

          if (!first&&endOfData) break;

          if (cyclesRecord>7)
          {
            cyclesRecord=0;

            AddToBuff();
            added=true;
          }

          if (!added) {
            uint16_tval=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 (inti=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);
}

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

  recPos+=buffPos;

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

voidMoveServo(uint8_tidServo, intangle, inttimeMs)
{
  switch (idServo)
  {
  casebasePin:
    servoBase.write(angle);
    break;
  caseshoulderPin:
    servoShoulder.write(angle);
    break;
  caseelbowPin:
    servoElbow.write(angle);
    break;
  casegripperPin:
    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);
  }
}

voidPrintBuffer()
{
  for (inti=0; i<recPos; i++)
  {
    uint16_tdata=buff[i];
    byteangle=data&0xFF;
    uint16_tservoNumber=data&0xF00;
    servoNumber=servoNumber>>8;
    boolendOfData=data&0x8000;

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

voidStartPosition()
{
  intangleBase=servoBase.read();
  intangleShoulder=servoShoulder.read();
  intangleElbow=servoElbow.read();
  intangleGripper=servoGripper.read();

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

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

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







文章分类: 平台创客Arduino
分享到:
会员登录
登录
我的资料
留言
回到顶部