#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Wire.h>
#include <SoftwareSerial.h>
#include <Servo.h>//使用舵机库
#define Oled_Reset 4
Adafruit_SSD1306 display=( 128,64,&Wire,Oled_Reset);
const int RX = 3 ;
const int TX = 2 ;
const int DSD = 15 ;//Default Servo Delay
const int LarmMin = 80 ;
const int LarmMax = 170 ;
const int BarmMin = 60 ;
const int BarmMax = 160 ;
const int ZarmMin = 0 ;
const int ZarmMax = 180;
bool mode;//mode =1
int moveStep = 3 ;//手柄模式移动角度
Servo Larm,Barm,Zarm ;//创建舵机对象
SoftwareSerial bluetooth(TX,RX);//蓝牙链接 命名定义引脚
/*char* strings[]={
"MK2 bluetooth Connected","wraing!serial mode!",
"Switch to Joy-Stick Mode.","unknown command",
"Waring!,Out of Value","Command: Switch to Instruction Mode",
"Waring!,Out of Value","Resotre Initial Position","MK2 Connected"
};*/
void setup() {
Wire.begin();
display.begin(SSD1306_SWITCHCAPVCC, 0x3C);//初始化OLED地址，设置串行IIC地址为0x3c
Larm.attach(10);//链接伺服舵机10
delay(1000); //稳定性等待
Barm.attach(11);//链接伺服舵机11
delay(1000);
Zarm.attach(9);
delay(1000);
Larm.write(90);//小臂初始上电默认位置
Barm.write(90);//大臂初始上电默认位置
delay(10);
Zarm.write(90);//大臂初始上电默认位置
delay(10);
bluetooth.begin(9600);//启动软件串口通讯
Serial.begin(9600);
Serial.println("strings[1]");
bluetooth.println("strings[1]");
}
void loop()
{
words_display();
if(bluetooth.available()>0)//检测串口有无数据
{
char bluetoothCmd = bluetooth.read() ;//读取字符型存入 bluetoothCmd
if(mode==1)
{
armDataCmd(bluetoothCmd);//自建函数 （电机数据）
}else{
armJoyCmd(bluetoothCmd);//手柄模式
}
}
}
void armDataCmd(char bluetoothCmd) //自建函数接收 字符编号
{
if(bluetoothCmd=='f'||bluetoothCmd=='b'||bluetoothCmd=='u'//串行模式下检测到手柄模式指令报错
||bluetoothCmd=='d'||bluetoothCmd=='g'||bluetoothCmd=='r')
{
bluetooth.println("wraing!serial mode!");
delay(100);
while(bluetooth.available()>0) char wrongCmd = bluetooth.read();//错误数据存放
return;
}
if(bluetoothCmd == 'L'||bluetoothCmd == 'B'||bluetoothCmd == 'Z')
{
int ServoData = bluetooth.parseInt();
servoCmd(bluetoothCmd,ServoData,DSD);
}
else
{
switch(bluetoothCmd){
case '0':
reportStatus(); //报告位置信息
break;
case 'i':
{
resetIniPos();
break;
case'm':
{
mode=0;
bluetooth.println(" Switch to Joy-Stick Mode.");
break;
}
}
default:
bluetooth.println("unknown command");
Serial.println("unknown command");
}
}
}
void armJoyCmd(char bluetoothCmd)//手柄模式指令
{
if(bluetoothCmd=='B'||bluetoothCmd=='L'||bluetoothCmd=='Z')
{
bluetooth.println("wraing!Joy mode!");
delay(100);
while(bluetooth.available()>0) char wrongCmd = bluetooth.read();
}
int BarmJoyPos;
int LarmJoyPos;
int ZarmJoyPos;
switch(bluetoothCmd)
{
case'm':
mode=1;
bluetooth.println("Command: Switch to Instruction Mode");
break;
case 'f'://front
BarmJoyPos = Barm.read()-moveStep ;
servoCmd('B',BarmJoyPos,DSD);
break;
case'b'://back
BarmJoyPos = Barm.read()+moveStep ;
servoCmd('B',BarmJoyPos,DSD);
break;
case 'u'://up
LarmJoyPos = Larm.read()-moveStep ;
servoCmd('L',LarmJoyPos,DSD);
break;
case 'd'://down
LarmJoyPos = Larm.read()+moveStep ;
servoCmd('L',LarmJoyPos,DSD);
break;
case 'g'://grasp
ZarmJoyPos = Zarm.read()-moveStep ;
servoCmd('Z',ZarmJoyPos,DSD);
break;
case 'r'://release
ZarmJoyPos = Zarm.read()+moveStep ;
servoCmd('Z',ZarmJoyPos,DSD);
break;
}
}
void servoCmd(char servoName ,int toPos ,int servoDelay)// 电机运行参数程序
{
Servo servo2go; //定义一个通用舵机名
int fromPos ; //建立起始角度变量
switch(servoName)
{
case 'L' :
if(toPos>= LarmMin&&toPos<=LarmMax)
{
servo2go = Larm ;
fromPos = Larm.read() ;
break;
}
else
{
bluetooth.println("Waring!,Out of Value");
return;
}
case 'B':
if(toPos>=BarmMin&&toPos<= BarmMax)
{
servo2go = Barm;
fromPos = Barm.read();
break;
}else
{
bluetooth.println("Waring!,Out of Value");
return;
}
case 'Z':
if(toPos>=BarmMin&&toPos<= BarmMax)
{
servo2go = Zarm;
fromPos = Zarm.read();
break;
}else
{
bluetooth.println("Waring!,Out of Value");
return;
}
}
if(fromPos<=toPos){
for(int i=fromPos;i<=toPos;i++)
{
servo2go.write(i);
delay(servoDelay);
}
}else{
for(int i=fromPos;i >= toPos;i--)
{
servo2go.write(i);
delay(servoDelay);
}
}
}
void reportStatus()//报告位置信息函数
{
bluetooth.print("Larm :");
bluetooth.println(Larm.read());
bluetooth.print("Barm :");
bluetooth.println(Barm.read());
bluetooth.print("Zarm :");
bluetooth.println(Zarm.read());
}
void resetIniPos()//重置机械臂位置
{
bluetooth.println("Resotre Initial Position");
int intialArray[4][3]= // 素组
{
{'L',145,DSD},
{'B',80,DSD},
{'Z',90,DSD},
};
for(int i;i<=3;i++)
{
servoCmd(intialArray[i][0],intialArray[i][1],intialArray[i][2]);
}
bluetooth.println("reset succse!");
}
void words_display()
{
display.clearDisplay();
display.setTextColor(WHITE);
display.setTextSize(1.5);
display.setCursor(30,0);
display.print("MK2 Connected");
display.setCursor(0,55);
display.print("open time:");
display.print(millis() / 1000);
display.print("s");
display.setCursor(100,40);
display.print("test");
display.display();
}
|
发表于 2022-2-2 12:00
楼主