一.简介
1.Ardunio:一款源自意大利的开放源代码硬件项目平台
2.LeArm:是幻尔科技旗下的一块产品,机身由六个舵机组成,的可编程机械臂
3.目的:实现电视遥控器控制机械臂移动和抓取
二.硬件
1.整体展示
2.控制设备
(突然想拿电视遥控器试试,没想到成功了)(数字控制移动,开关控制抓取)
3.接收装置和显示器
三.效果展示
1.预期效果
实现显示器展示遥控器输入的参数,实现机械臂可以移动至指定位置,实现机械臂的抓取动作和复位操作。
2.视频
这里我录了段视屏,发在动态里了。
点击这里
四.实现步骤
1.算法思路
2.收录电视红外遥控的按键信息
#define R_ZERO 1886388479 //红外传感器的解码值
#define R_ONE 1886421119
#define R_TWO 1886404799
#define R_THREE 1886437439
#define R_FOUR 1886396639
#define R_FIVE 1886429279
#define R_SIX 1886412959
#define R_SEVEN 1886445599
#define R_EIGHT 1886392559
#define R_NINE 1886425199
#define R_Previous 1886406839
#define R_LAST 1886439479
#define ok_zhua 1886413469
#define over_ok 1886400719
int CheckButton(uint32_t code)
{
int ret;
switch (code) {
case R_ZERO:
ret = 0;
break;
case R_ONE:
ret = 1;
break;
case R_TWO:
ret = 2;
break;
case R_THREE:
ret = 3;
break;
case R_FOUR:
ret = 4;
break;
case R_FIVE:
ret = 5;
break;
case R_SIX:
ret = 6;
break;
case R_SEVEN:
ret = 7;
break;
case R_EIGHT:
ret = 8;
break;
case R_NINE:
ret = 9;
break;
case R_Previous:
ret = -2;
break;
case R_LAST:
ret = -3;
break;
case ok_zhua:
ret = -4;
break;
case over_ok:
ret = -5;
break;
default:
ret = -1;
break;
}
return ret;
}
3.控制舵机
myController.moveServo(6, 1500, 1000);
//(舵机号,移动的位置,移动指定位置花费的时间ms)
4.完整代码
#include <IRremote.h>
#include <U8g2lib.h>
#include <SoftwareSerial.h>
#include <LobotServoController.h>
#define RxPin 9 //接受
#define TxPin 8 //发送
#define RECV_PIN 12
#define LED 13
#define R_ZERO 1886388479 //红外传感器的解码值
#define R_ONE 1886421119
#define R_TWO 1886404799
#define R_THREE 1886437439
#define R_FOUR 1886396639
#define R_FIVE 1886429279
#define R_SIX 1886412959
#define R_SEVEN 1886445599
#define R_EIGHT 1886392559
#define R_NINE 1886425199
#define R_Previous 1886406839
#define R_LAST 1886439479
#define ok_zhua 1886413469
#define over_ok 1886400719
uint8_t result;
uint8_t last_button;
uint8_t ok;
bool refreshNow = true;
bool ledON = true;
bool state = false;
SoftwareSerial mySerial(RxPin, TxPin);
LobotServoController myController(mySerial);
U8G2_SSD1306_128X64_NONAME_1_HW_I2C u8g2(U8G2_R0, /* reset=*/ U8X8_PIN_NONE);
IRrecv irrecv(RECV_PIN); //Set an example of IRrecv
decode_results results; //Set an example of decode_results, Results is used to save the decoding results
void setup() {
// put your setup code here, to run once:
u8g2.begin();
u8g2.setDisplayRotation(U8G2_R2);
u8g2.setFont(u8g2_font_fub20_tr);
u8g2.firstPage();
do {
u8g2.setCursor(22, 38);
u8g2.println("LOBOT");
} while ( u8g2.nextPage() );
mySerial.begin(9600);
Serial.begin(9600);
pinMode(LED, OUTPUT);
irrecv.enableIRIn();
myController.moveServo(3, 600, 1000);
myController.moveServo(4, 900, 1000);
myController.moveServo(5, 1700, 1000);
myController.moveServo(6, 1500, 1000);
myController.runActionGroup(0, 1);
delay(1500);
}
void ledFlash() {
static uint32_t timer;
if (timer > millis())
return;
timer = millis() + 30;
if (ledON)
{
digitalWrite(LED, HIGH);
ledON = false;
}
else
{
digitalWrite(LED, LOW);
}
}
int CheckButton(uint32_t code)
{
int ret;
switch (code) {
case R_ZERO:
ret = 0;
break;
case R_ONE:
ret = 1;
break;
case R_TWO:
ret = 2;
break;
case R_THREE:
ret = 3;
break;
case R_FOUR:
ret = 4;
break;
case R_FIVE:
ret = 5;
break;
case R_SIX:
ret = 6;
break;
case R_SEVEN:
ret = 7;
break;
case R_EIGHT:
ret = 8;
break;
case R_NINE:
ret = 9;
break;
case R_Previous:
ret = -2;
break;
case R_LAST:
ret = -3;
break;
case ok_zhua:
ret = -4;
break;
case over_ok:
ret = -5;
break;
default:
ret = -1;
break;
}
return ret;
}
void irReceive()
{
static uint32_t timer;
static uint8_t step = 0;
static uint16_t count = 0;
int n;
if (timer < millis())
{
switch(step)
{
case 0:
if (irrecv.decode(&results)) // 如果有接收到数据
{
state = false;
n = CheckButton(results.value);//获取接收的数据
Serial.println(n);
if (n == -2)//如果是前一个按键
n = last_button - 1;//将频道数减一
if (n == -3)
n = last_button + 1;
if (n == -4)
ok = 100;
if ( n == -5)
ok = 101;
if (n >= 0 && n <= 99)
{//如果频道数在0-99则显示
last_button = n;
result = n;
refreshNow = true;
}
ledON = true;
irrecv.resume();
step = 1;
}
timer = millis() + 120;
break;
case 1:
count ++;
if (count >= 120)
{
refreshNow = true;
count = 0;
state = true;
step = 0;
}
if (irrecv.decode(&results))
{//如果在120ms内有按键被再次按下,如果是0-9的按键则显示前后两个按键的组合,如第一个按键是1,第二个按键是1,则显示11
n = CheckButton(results.value);
if (n >= 0 && n <= 9)
{
result = last_button*10 + n;
last_button = result;
refreshNow = true;
}
ledON = true;
irrecv.resume();
}
timer = millis() + 1;
break;
}
}
}
//Screen drawing
void draw() {
if (refreshNow == true)
{
refreshNow = false;
u8g2.firstPage();
do {
u8g2.setFont(u8g2_font_courB24_tf);
u8g2.setCursor(110, 20);
if (result > 9)
u8g2.setCursor(90, 20);
u8g2.println(result);
if (result == 8 && state == true)
{//如果频道数是8则显示LOBOT字样
u8g2.setFont(u8g2_font_fub20_tr); //More fonts can be found here. https://github.com/olikraus/u8g2/wiki/fntlistall
//u8g2.setCursor(20, 38);
//u8g2.println("LOBOT");
}
} while ( u8g2.nextPage());
}
}
void run_arm(){
int pos;
if( 0<last_button && last_button<=99 ){
pos=last_button*30;
myController.moveServo(6, pos, 1000);
delay(1000);
ok=0;
}
}
void zhua(){
u8g2.setCursor(20, 38);
u8g2.println("catch");
myController.moveServo(1, 800, 1000);
delay(1000);
myController.moveServo(5, 1900, 1000);
delay(1000);
myController.moveServo(1, 1200, 1000);
delay(1500);
myController.moveServo(5, 1400, 1000);
delay(1000);
myController.moveServo(6, 2500, 1000);
delay(1000);
myController.moveServo(1, 800, 1000);
delay(1000);
myController.moveServo(5, 1500, 1000);
myController.moveServo(6, 1500, 1000);
ok=0;
delay(1000);
}
void loop()
{
irReceive();
draw();
ledFlash();
if( ok == 100 )run_arm();
if( ok == 101 )zhua();
}