本站搬迁,新网址https://via-dean.com即将涵盖更多内容,敬请访问

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

 二维码 58
发表时间: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
分享到:
联系电话:18112932078
微信号:Via_Dean
邮箱:kiyo84001@163.com
会员登录
登录
我的资料
留言
回到顶部