|
|
//WenHao 2017/9/15 框架建立
//WenHao 2017/10/9 第一次調(diào)試開始
//WenHao 2017/10/15 電機(jī)模塊測(cè)試
//WenHao 2017/11/1 電機(jī)+舵機(jī)整體測(cè)試
//WenHao 2017/11/4 整體遙控測(cè)試
//WenHao 2017/11/9 揮手模塊 搖頭避障 姿態(tài)自適應(yīng)調(diào)整
//WenHao 2017/11/11 傳感器整體整合
#include <LobotServoController.h>
LobotServoController myse(Serial2);
/**********PS2手柄初始設(shè)置**********/
#include <PS2X_lib.h>
PS2X ps2x;
#define pressures false
#define rumble false
int error = 0;
byte type = 0;
byte vibrate = 0;
char ch='z';
int test=0;
int PS_LX;
int PS_LY;
int PS_RX;
int PS_RY;
/**********ps2手柄接口************/
#define PS2_DAT 3
#define PS2_CMD 36
#define PS2_SEL 4
#define PS2_CLK 5
/*********車輪初始設(shè)置************/
//1 B:up and down A: wheel
int PWMA = 13;
int AIN1 = 53;
int AIN2 = 51;
int PWMB = 12;
int BIN1 = 49;
int BIN2 = 47;
//2 C:TURN D: wheel
int PWMD = 11;
int DIN1 = 43;
int DIN2 = 45;
int PWMC = 2;
int CIN1 = 41;
int CIN2 = 39;
//3 F: TURN E : 不轉(zhuǎn) (LEFT)
int PWMF = 9;
int FIN1 = 52;
int FIN2 = 50;
int PWME = 8;
int EIN1 = 48;
int EIN2 = 46;
//4 G: TURN H : 不轉(zhuǎn) (LEFT)
int PWMH = 7;
int HIN1 = 44;
int HIN2 = 42;
int PWMG = 6;
int GIN1 = 38;
int GIN2 = 40;
/***********超聲波初始設(shè)置*************/
#define TRIG 22 /*超聲波TRIG引腳為 9號(hào)IO*/
#define ECHO 30 /*超聲波ECHO引腳為 8號(hào)IO*/
#define MAX_DISTANCE 150 /*最大檢測(cè)距離為150cm*/
#include <Servo.h>
#include <NewPing.h>
int servoPosition;
/***********避障動(dòng)作組****************/
#define GO_FORWARD 5 /*直走的動(dòng)作組*/
#define GO_BACK 6 /*后退動(dòng)作組*/
#define TURN_LEFT 7 /*左轉(zhuǎn)動(dòng)作組*/
#define TURN_RIGHT 8 /*右轉(zhuǎn)動(dòng)作組*/
#define MIN_DISTANCE_TURN 35 /*避障距離，就是小于多少距離的時(shí)候進(jìn)行避障*/
#define BIAS 3 /*舵機(jī)偏差，根據(jù)實(shí)際情況調(diào)整大小以使超聲波朝向正前方*/
NewPing Sonar(TRIG, ECHO, MAX_DISTANCE); //實(shí)例化超聲波測(cè)距類
Servo sonarServo; //超聲波云臺(tái)舵機(jī)控制類實(shí)例
int gDistance; //全局變量，用于存儲(chǔ)中間位置超聲波測(cè)得的距離
int gLDistance; //用于存儲(chǔ)機(jī)器人左側(cè)測(cè)得的距離
int gRDistance; //用于存儲(chǔ)機(jī)器人右側(cè)測(cè)得的距離
/**********傳感器初始設(shè)置***************/
#define leftfront_infraredsensor 33 //距離很近之后發(fā)送低電平信號(hào)
#define leftback_infraredsensor 35
#define rightfront_infraredsensor 37
#define rightback_infraredsensor 31
#define upright_infraredsensor 18
#define upleft_infraredsensor 19
int irqNum = 3;
NewPing Sonar1(TRIG,ECHO,MAX_DISTANCE);
/********搖頭舵機(jī)控制**************/
int servopin=26;
int myangle;//定義角度變量
int pulsewidth;//定義脈寬變量
int val;
/********水平姿態(tài)**************/
#include <I2Cdev.h>
#include <MPU6050.h>
#include <Wire.h>
#define Gx_offset -3.06
#define Gy_offset 0
#define Gz_offset -0.88
MPU6050 accelgyro;
int16_t ax,ay,az;
int16_t gx,gy,gz; //存儲(chǔ)原始數(shù)據(jù)
float aax,aay,aaz,ggx,ggy,ggz;//存儲(chǔ)量化后的數(shù)據(jù)
float Ax,Ay,Az; //單位 g(9.8m/s^2)
float Gx,Gy,Gz;
float Angel_accX,Angel_accY,Angel_accZ;
long LastTime,NowTime,TimeSpan;
bool blinkState=false;
int wen=0;
int wen1=0;
int wencha=0;
void servopulse(int servopin,int myangle)//定義一個(gè)脈沖函數(shù)
{
pulsewidth=(myangle*11)+500;//將角度轉(zhuǎn)化為500-2480 的脈寬值
digitalWrite(servopin,HIGH);//將舵機(jī)接口電平至高
delayMicroseconds(pulsewidth);//延時(shí)脈寬值的微秒數(shù)
digitalWrite(servopin,LOW);//將舵機(jī)接口電平至低
delay(20-pulsewidth/1000);
}
int getDistance()
{ //獲得距離
uint16_t lEchoTime; //變量 ，用于保存檢測(cè)到的脈沖高電平時(shí)間
lEchoTime = Sonar1.ping_median(3); //檢測(cè)6次超聲波，排除錯(cuò)誤的結(jié)果
int lDistance = Sonar1.convert_cm(lEchoTime);//轉(zhuǎn)換檢測(cè)到的脈沖高電平時(shí)間為厘米
Serial.print("distance_front:");
Serial.print(lDistance);
Serial.println("cm");
return lDistance; //返回檢測(cè)到的距離
}
void getAllDistance()//獲得前及左右三個(gè)方向的距離
{
int tDistance; //用于暫存測(cè)得距離
for(int i=0;i<=20;i++)
servopulse(servopin,90); //超聲波云臺(tái)舵機(jī)轉(zhuǎn)到90度即中間位置
delay(200); //等待200ms，等待舵機(jī)轉(zhuǎn)動(dòng)到位
gDistance = getDistance(); //測(cè)量距離，保存到全局變臉gDistance
for(int i=0;i<=20;i++)
servopulse(servopin,130); //超聲波云臺(tái)舵機(jī)轉(zhuǎn)到130度位置即機(jī)器人左面40度位置
delay(200); //延時(shí)，等待舵機(jī)轉(zhuǎn)動(dòng)到位
tDistance = getDistance(); //測(cè)量距離，保存到 tDistance
for(int i=0;i<=20;i++)
servopulse(servopin,170); //轉(zhuǎn)動(dòng)到170度，即機(jī)器人左側(cè)80度位置
delay(200); //延時(shí)，等待舵機(jī)轉(zhuǎn)動(dòng)到位
gLDistance = getDistance(); //測(cè)量距離，保存到 gLDistance
if(tDistance < gLDistance) //比較左側(cè)測(cè)得的兩個(gè)距離，取小的一個(gè)，保存到gLDistance作為左側(cè)距離
gLDistance = tDistance;
for(int i=0;i<=20;i++)
servopulse(servopin,50); //超聲波云臺(tái)舵機(jī)轉(zhuǎn)到50度位置即機(jī)器人右面40度位置
delay(200); //延時(shí)，等待舵機(jī)轉(zhuǎn)動(dòng)到位
tDistance = getDistance(); //測(cè)量距離，保存到tDistance
for(int i=0;i<=20;i++)
servopulse(servopin,10); //轉(zhuǎn)到10度，即機(jī)器人右面80度位置
delay(200); //延時(shí)，等待舵機(jī)轉(zhuǎn)動(dòng)到位
gRDistance = getDistance(); //測(cè)量距離，保存到gRDistance
if(tDistance < gRDistance) //比較兩個(gè)距離，將較小的一個(gè)保存到gRDistance
gRDistance = tDistance;
for(int i=0;i<=7;i++)
servopulse(servopin,90); //超聲波云臺(tái)舵機(jī)轉(zhuǎn)回中間位置
}
void pin32_irq()
{
irqNum = 4; //記錄中斷號(hào)
detachInterrupt(5); //失能另一個(gè)中斷，這樣irqNum就不會(huì)被另一個(gè)中斷賦值
}
void pin34_irq()
{
irqNum = 5; //記錄中斷號(hào)
detachInterrupt(4);
}
void move1() { //程序的主要邏輯
static uint32_t timer = 0; //定義靜態(tài)變量Timer， 用于計(jì)時(shí)
static int step = 0; //靜態(tài)變量，用于記錄步驟
if(timer > millis()) //如果設(shè)定時(shí)間大于當(dāng)前毫秒數(shù)，則返回，否則繼續(xù)
return;
switch (step) { //根據(jù)步驟step做分支
case 0: //步驟0
step = 1; //步驟轉(zhuǎn)移到1
attachInterrupt(5, pin34_irq, FALLING); //配置1號(hào)中斷，下降沿觸發(fā)
attachInterrupt(4, pin32_irq, FALLING); //配置0號(hào)中斷，下降沿觸發(fā)
break; //結(jié)束switch循環(huán)
case 1: //步驟
if (irqNum == 4) myse.runActionGroup(7,1);
if (irqNum == 5) myse.runActionGroup(8,1);
irqNum = 3; //賦值為一個(gè)既不是0也不是1的數(shù)值
timer = millis() + 300; //延時(shí)300毫秒
step = 0; //重新回到步驟0
break; //結(jié)束switch語(yǔ)句
}
}
void motor_start()
{ pinMode(PWMA,OUTPUT);
pinMode(AIN1,OUTPUT);
pinMode(AIN2,OUTPUT);
pinMode(PWMB,OUTPUT);
pinMode(BIN1,OUTPUT);
pinMode(BIN2,OUTPUT);
pinMode(PWMC,OUTPUT);
pinMode(CIN1,OUTPUT);
pinMode(CIN2,OUTPUT);
pinMode(PWMD,OUTPUT);
pinMode(DIN1,OUTPUT);
pinMode(DIN2,OUTPUT);
pinMode(PWME,OUTPUT);
pinMode(EIN1,OUTPUT);
pinMode(EIN2,OUTPUT);
pinMode(PWMF,OUTPUT);
pinMode(FIN1,OUTPUT);
pinMode(FIN2,OUTPUT);
pinMode(PWMG,OUTPUT);
pinMode(GIN1,OUTPUT);
pinMode(GIN2,OUTPUT);
pinMode(PWMH,OUTPUT);
pinMode(HIN1,OUTPUT);
pinMode(HIN2,OUTPUT);
sonarServo.attach(10); //綁定10號(hào)io為舵機(jī)控制iO
sonarServo.write(90 + BIAS); //轉(zhuǎn)到中間位置，加上了偏差
digitalWrite(2,LOW);
}
void sensor_start()
{
pinMode(upleft_infraredsensor,INPUT);
pinMode(upright_infraredsensor,INPUT);
pinMode(leftfront_infraredsensor,INPUT);
pinMode(leftback_infraredsensor,INPUT);
pinMode(rightfront_infraredsensor,INPUT);
pinMode(rightback_infraredsensor,INPUT);
pinMode(ECHO,INPUT);
pinMode(TRIG,OUTPUT);
}
void move(int motor, int speed, int direction)
{
boolean inPin1 = LOW;
boolean inPin2 = HIGH;
if(direction == 1){
inPin1 = HIGH;
inPin2 = LOW;
}
if(motor == 1){
digitalWrite(BIN1, inPin1);
digitalWrite(BIN2, inPin2);
analogWrite(PWMB, speed);
}
if(motor == 2){
digitalWrite(CIN1, inPin1);
digitalWrite(CIN2, inPin2);
analogWrite(PWMC, speed);
}
if(motor == 3){
digitalWrite(FIN1, inPin1);
digitalWrite(FIN2, inPin2);
analogWrite(PWMF, speed);
}
if(motor == 4){
digitalWrite(HIN1, inPin1);
digitalWrite(HIN2, inPin2);
analogWrite(PWMH, speed);
}
if(motor == 5){
digitalWrite(DIN1, inPin1);
digitalWrite(DIN2, inPin2);
analogWrite(PWMD, speed);
digitalWrite(AIN1, inPin1);
digitalWrite(AIN2, inPin2);
analogWrite(PWMA, speed);
digitalWrite(EIN1, inPin1);
digitalWrite(EIN2, inPin2);
analogWrite(PWME, speed);
digitalWrite(GIN1, inPin1);
digitalWrite(GIN2, inPin2);
analogWrite(PWMG, speed);
}
if(motor == 6){
digitalWrite(DIN1, inPin1);
digitalWrite(DIN2, inPin2);
analogWrite(PWMD, speed);
digitalWrite(AIN2, inPin1);
digitalWrite(AIN1, inPin2);
analogWrite(PWMA, speed);
digitalWrite(EIN1, inPin1);
digitalWrite(EIN2, inPin2);
analogWrite(PWME, speed);
digitalWrite(GIN2, inPin1);
digitalWrite(GIN1, inPin2);
analogWrite(PWMG, speed);
}
}
void stop(){
analogWrite(PWMA, 0);
analogWrite(PWMB, 0);
analogWrite(PWMC, 0);
analogWrite(PWMD, 0);
analogWrite(PWME, 0);
analogWrite(PWMF, 0);
analogWrite(PWMG, 0);
analogWrite(PWMH, 0);
}
void sonar() //避障邏輯
{
static uint32_t timer = 0; //靜態(tài)變量，用于計(jì)時(shí)
static uint8_t step = 0; //靜態(tài)變量，用于記錄步驟
if (timer > millis()) //如果設(shè)定時(shí)間大于當(dāng)前毫秒數(shù)則返回，否側(cè)繼續(xù)后續(xù)操作
return;
switch (step) //根據(jù)step分支
{
case 0: //步驟0
gDistance = getDistance(); //測(cè)量距離，保存到gDistance
if (gDistance > MIN_DISTANCE_TURN || gDistance == 0) { //如果測(cè)得距離大于指定的避障距離，前進(jìn)
{
myse.runActionGroup(GO_FORWARD, 0); //一直前進(jìn)
step = 1; //轉(zhuǎn)移到步驟1
timer = millis() + 500; //延時(shí)500ms
}
} else { //如果測(cè)得距離小于指定距離
step = 2; //轉(zhuǎn)移到步驟2
timer = millis() + 100; //延時(shí)100ms
}
break; //結(jié)束switch語(yǔ)句
case 1: //步驟1
gDistance = getDistance(); //測(cè)量距離
if (gDistance < MIN_DISTANCE_TURN && gDistance > 0) { //如果測(cè)得距離小于指定的避障距離，則停止所有動(dòng)作組，轉(zhuǎn)移到步驟2
myse.stopActionGroup();
step = 2;
}
break; //結(jié)束switch語(yǔ)句
case 2: //步驟2
{ //沒有動(dòng)作組在運(yùn)行，即等待動(dòng)作組運(yùn)行完畢
getAllDistance(); //獲得三個(gè)方向的距離
Serial.print(gDistance); //打印測(cè)得距離
Serial.print("cm ");
Serial.print(gLDistance);
Serial.print("cm ");
Serial.print(gRDistance);
Serial.println("cm ");
step = 3; //轉(zhuǎn)移到步驟3
//此處沒有break，執(zhí)行完后直接之心case 3
}
case 3: //步驟3
static bool lastActionIsGoBack = false; //靜態(tài)變量，記錄最后的動(dòng)作是不是后退
if (((gDistance > MIN_DISTANCE_TURN) || (gDistance == 0)) && lastActionIsGoBack == false) {
//中間距離大于指定避障距離且最后的一個(gè)動(dòng)作不是后退，那么就回到步驟0，
//此處判斷最后一個(gè)動(dòng)作是不是后退，是避免程序陷入后退-》前進(jìn)-》后退-》前進(jìn)...這樣的死循環(huán)
//當(dāng)最后一步是后退是就不執(zhí)行前進(jìn)
step = 0;
timer = millis() + 200;
lastActionIsGoBack = false;
return; //返回，結(jié)束函數(shù)
}
if ((((gLDistance > gRDistance) && (gLDistance > MIN_DISTANCE_TURN)) || gLDistance == 0) && gDistance > 15) {
//超聲波測(cè)得左側(cè)的最小距離大于右側(cè)的最小距離大于指定的避障距離，并且中間測(cè)得距離大于15時(shí)
//檢測(cè)中間的距離目的是避免有物體處于機(jī)器人兩個(gè)前腿之間，導(dǎo)致機(jī)器人無法轉(zhuǎn)向
{ //等待動(dòng)作組運(yùn)行完畢
myse.runActionGroup(TURN_LEFT, 2); //左轉(zhuǎn)4次，
lastActionIsGoBack = false; //標(biāo)識(shí)最后一個(gè)動(dòng)作不是后退
step = 2; //轉(zhuǎn)移到步驟2
}
timer = millis() + 500; //延時(shí)500ms
return; //返回，結(jié)束函數(shù)
}
if ((((gRDistance > gLDistance) && (gRDistance > MIN_DISTANCE_TURN)) || gRDistance == 0) && gDistance > 15) {
//超聲波測(cè)得左側(cè)的最小距離大于右側(cè)的最小距離大于指定的避障距離，并且中間測(cè)得距離大于15時(shí)
{ //等待動(dòng)作組運(yùn)行完畢
myse.runActionGroup(TURN_RIGHT, 2); //右轉(zhuǎn)4次
lastActionIsGoBack = false; //標(biāo)識(shí)最后一個(gè)動(dòng)作不是后退
step = 2; //轉(zhuǎn)移到步驟2
}
timer = millis() + 500; //延時(shí)500ms
return; //返回，結(jié)束函數(shù)
}
//當(dāng)前面的都不符合，所有的return都沒有被執(zhí)行
//程序就會(huì)執(zhí)行到這里
myse.runActionGroup(GO_BACK, 3); //執(zhí)行后退動(dòng)作組3次
lastActionIsGoBack = true; //標(biāo)識(shí)最后一個(gè)動(dòng)作是后退
step = 2; //轉(zhuǎn)移到步驟2
timer = millis() + 500; //延時(shí)500ms
}
}
void tuoluo()
{
accelgyro.getMotion6(&ax,&ay,&az,&gx,&gy,&gz);//獲取三個(gè)軸的加速度和角速度
//======一下三行是對(duì)加速度進(jìn)行量化，得出單位為g的加速度值
Ax=ax/16384.00;
Ay=ay/16384.00;
Az=az/16384.00;
Angel_accX=atan(Ax/sqrt(Az*Az+Ay*Ay))*180/3.14;
Angel_accY=atan(Ay/sqrt(Ax*Ax+Az*Az))*180/3.14;
Angel_accZ=atan(Az/sqrt(Ax*Ax+Ay*Ay))*180/3.14;
Angel_accY=Angel_accY-0.8;
Serial.print(Angel_accY);
Serial.println(" ");
delay(100);//這個(gè)用來控制采樣速erial
Angel_accY=Angel_accY+wen1;
// if (Angel_accY>=9) delay(50); if (Angel_accY>=9) delay(50); if (Angel_accY>=9) wen1=3;
// if (Angel_accY>=6 && Angel_accY<=8 ) delay(50);
// if (Angel_accY>=6 && Angel_accY<=8) delay(50);
// if (Angel_accY>=6 && Angel_accY<=8)
// wen1=2;
// if (Angel_accY>=4.5 && Angel_accY<=5.5 ) delay(50);
// if (Angel_accY>=4.5 && Angel_accY<=5.5) delay(50);
// if (Angel_accY>=4.5 && Angel_accY<=5.5)
// wen1=1;
// if (Angel_accY<=2 ) delay(50); if (Angel_accY<=2) delay(50); if (Angel_accY<=2) wen1=0;
// Serial.print(wen1);
// Serial.print(" ");
//
// if (wen1==0) if(wencha=0) {myse.runActionGroup(14,1); wencha=0;}
// if (wen1==1) if(wencha=0) {myse.runActionGroup(42,1); wencha=1;}
// if (wen1==2) if(wencha=0) {myse.runActionGroup(44,1); wencha=2;}
// if (wen1==3) if(wencha=0) {myse.runActionGroup(45,1); wencha=3;}
// if (wen1==0) if(wencha=1) {myse.runActionGroup(42,1); wencha=1;}
// if (wen1==1) if(wencha=1) {myse.runActionGroup(44,1); wencha=2;}
// if (wen1==2) if(wencha=1) {myse.runActionGroup(45,1); wencha=3;}
// if (wen1==3) if(wencha=1) {myse.runActionGroup(45,1); wencha=3;}
// if (wen1==0) if(wencha=2) {myse.runActionGroup(44,1); wencha=2;}
// if (wen1==1) if(wencha=2) {myse.runActionGroup(45,1); wencha=3;}
// if (wen1==2) if(wencha=2) {myse.runActionGroup(45,1); wencha=3;}
// if (wen1==3) if(wencha=2) {myse.runActionGroup(45,1); wencha=3;}
// if (wen1==0) if(wencha=3) {myse.runActionGroup(45,1); wencha=3;}
// if (wen1==1) if(wencha=3) {myse.runActionGroup(45,1); wencha=3;}
// if (wen1==2) if(wencha=3) {myse.runActionGroup(45,1); wencha=3;}
// if (wen1==3) if(wencha=3) {myse.runActionGroup(45,1); wencha=3;}
// Serial.println(wencha);
if (Angel_accY<3 )
delay(50);
if (Angel_accY<3 )
delay(50);
if (Angel_accY<3 )
delay(50);
if (Angel_accY<3 )
delay(50);
if (Angel_accY<3 )
delay(50);
if (Angel_accY<3 )
delay(50);
if (Angel_accY<3 )
myse.runActionGroup(14,1);
if (Angel_accY>=4.5 && Angel_accY<=5.5)
delay(50);
if (Angel_accY>=4.5 && Angel_accY<=5.5)
delay(50);
if (Angel_accY>=4.5 && Angel_accY<=5.5)
delay(50);
if (Angel_accY>=4.5 && Angel_accY<=5.5)
delay(50);
if (Angel_accY>=4.5 && Angel_accY<=5.5)
delay(50);
if (Angel_accY>=4.5 && Angel_accY<=5.5)
delay(50);
if (Angel_accY>=4.5 && Angel_accY<=5.5)
{myse.runActionGroup(42,1);
wen1=5; }
if (Angel_accY>=6.5 && Angel_accY<=7.5)
delay(50);
if (Angel_accY>=6.5 && Angel_accY<=7.5)
delay(50);
if (Angel_accY>=6.5 && Angel_accY<=7.5)
delay(50);
if (Angel_accY>=6.5 && Angel_accY<=7.5)
delay(50);
if (Angel_accY>=6.5 && Angel_accY<=7.5)
delay(50);
if (Angel_accY>=6.5 && Angel_accY<=7.5)
delay(50);
if (Angel_accY>=6.5 && Angel_accY<=7.5)
{myse.runActionGroup(44,1);
wen1=7; }
if (Angel_accY>=9.5)
delay(50);
if (Angel_accY>=9.5 )
delay(50);
if (Angel_accY>=9.5)
delay(50);
if (Angel_accY>=9.5 )
delay(50);
if (Angel_accY>=9.5)
delay(50);
if (Angel_accY>=9.5 )
delay(50);
if (Angel_accY>=9.5)
{myse.runActionGroup(45,1);
wen1=9; }
}
void button_set()
{
ch='z';
if(type==2)
return;
else {
ps2x.read_gamepad(false, vibrate);
PS_LX=ps2x.Analog(PSS_LX);
PS_RX=ps2x.Analog(PSS_RX);
PS_LY=ps2x.Analog(PSS_LY);
PS_RY=ps2x.Analog(PSS_RY);
vibrate = ps2x.Analog(PSAB_BLUE);
if (ps2x.Button(PSB_START)) {ch='w';Serial.println("Start is being held");myse.runActionGroup(13,1);test=1;}
if (ps2x.Button(PSB_SELECT)) {ch='x';Serial.println("Select is being held");myse.runActionGroup(0,1);test=2; }
if (ps2x.Button(PSB_PAD_UP)) {ch='a';myse.runActionGroup(1,1);}
if (ps2x.Button(PSB_PAD_DOWN) ) {ch='b';myse.runActionGroup(2,1);}
if (ps2x.Button(PSB_PAD_LEFT) ) {ch='c';myse.runActionGroup(3,1);}
if (ps2x.Button(PSB_PAD_RIGHT)) {ch='d';myse.runActionGroup(4,1);}
if (ps2x.Button(PSB_GREEN)) {ch='e';myse.runActionGroup(15,1);}
if (ps2x.NewButtonState(PSB_BLUE)){ch='f';}
if (ps2x.Button(PSB_PINK)) {ch='g';tuoluo();}
if (ps2x.Button(PSB_RED)) {ch='h';}
if (ps2x.Button(PSB_R2) ) {ch='i';}
if (ps2x.Button(PSB_L2)) {ch='j';test=3;}
if (ps2x.Button(PSB_R1) ) {ch='k';}
if (ps2x.Button(PSB_L1)) {ch='l';myse.runActionGroup(14,1);test=0;}
if (ps2x.Button(PSB_R3) ) {ch='m';}
if (ps2x.Button(PSB_L3)) {ch='n';}
if(PS_RX<5) {ch='o';}
if(PS_RX>250) {ch='p';}
if(PS_RY<5) {ch='q';}
if(PS_RY>250) {ch='r';}
if(PS_LX<5) {ch='s';}
if(PS_LX>250) {ch='t';}
if(PS_LY<5) {ch='u';}
if(PS_LY>250) {ch='v';}
if (ch=='z') stop();
Serial.println(ch);
delay(50);
}
}
void button_set2() ////高姿態(tài)
{
ch='z';
if(type==2)
return;
else {
ps2x.read_gamepad(false, vibrate);
PS_LX=ps2x.Analog(PSS_LX);
PS_RX=ps2x.Analog(PSS_RX);
PS_LY=ps2x.Analog(PSS_LY);
PS_RY=ps2x.Analog(PSS_RY);
vibrate = ps2x.Analog(PSAB_BLUE);
if (ps2x.Button(PSB_START)) {ch='w';Serial.println("Start is being held");myse.runActionGroup(13,1);test=1;}
if (ps2x.Button(PSB_SELECT)) {ch='x';Serial.println("Select is being held");myse.runActionGroup(0,1);test=2; }
if (ps2x.Button(PSB_PAD_UP)) {ch='a';myse.runActionGroup(9,1);}
if (ps2x.Button(PSB_PAD_DOWN) ) {ch='b';myse.runActionGroup(10,1);}
if (ps2x.Button(PSB_PAD_LEFT) ) {ch='c';myse.runActionGroup(11,1);}
if (ps2x.Button(PSB_PAD_RIGHT)) {ch='d';myse.runActionGroup(12,1);}
if (ps2x.Button(PSB_GREEN)) {ch='e';}
if (ps2x.NewButtonState(PSB_BLUE)){ch='f';}
if (ps2x.Button(PSB_PINK)) {ch='g';}
if (ps2x.Button(PSB_RED)) {ch='h';}
if (ps2x.Button(PSB_R2) ) {ch='i';}
if (ps2x.Button(PSB_L2)) {ch='j';test=3;}
if (ps2x.Button(PSB_R1) ) {ch='k';}
if (ps2x.Button(PSB_L1)) {ch='l';myse.runActionGroup(14,1);test=0;}
if (ps2x.Button(PSB_R3) ) {ch='m';}
if (ps2x.Button(PSB_L3)) {ch='n';}
if(PS_RX<5) {ch='o';}
if(PS_RX>250) {ch='p';}
if(PS_RY<5) {ch='q';}
if(PS_RY>250) {ch='r';}
if(PS_LX<5) {ch='s';}
if(PS_LX>250) {ch='t';}
if(PS_LY<5) {ch='u';}
if(PS_LY>250) {ch='v';}
if (ch=='z') stop();
Serial.print(" ");
Serial.println(ch);
delay(50);
}
}
void button_set3() ///中姿態(tài)
{
ch='z';
if(type==2)
return;
else {
ps2x.read_gamepad(false, vibrate);
PS_LX=ps2x.Analog(PSS_LX);
PS_RX=ps2x.Analog(PSS_RX);
PS_LY=ps2x.Analog(PSS_LY);
PS_RY=ps2x.Analog(PSS_RY);
vibrate = ps2x.Analog(PSAB_BLUE);
if (ps2x.Button(PSB_START)) {ch='w';Serial.println("Start is being held");myse.runActionGroup(13,1);test=1;}
if (ps2x.Button(PSB_SELECT)) {ch='x';Serial.println("Select is being held");myse.runActionGroup(0,1);test=2; }
if (ps2x.Button(PSB_PAD_UP)) {ch='a';myse.runActionGroup(5,1);}
if (ps2x.Button(PSB_PAD_DOWN) ) {ch='b';myse.runActionGroup(6,1);}
if (ps2x.Button(PSB_PAD_LEFT) ) {ch='c';myse.runActionGroup(7,1);}
if (ps2x.Button(PSB_PAD_RIGHT)) {ch='d';myse.runActionGroup(8,1);}
if (ps2x.Button(PSB_GREEN)) {ch='e';myse.runActionGroup(16,1);}
if (ps2x.NewButtonState(PSB_BLUE)){ch='f';}
if (ps2x.Button(PSB_PINK)) {ch='g';move1(); }
if (ps2x.Button(PSB_RED)) {ch='h';sonar();}
if (ps2x.Button(PSB_R2) ) {ch='i';}
if (ps2x.Button(PSB_L2)) {ch='j';test=3;}
if (ps2x.Button(PSB_R1) ) {ch='k';}
if (ps2x.Button(PSB_L1)) {ch='l';myse.runActionGroup(14,1);test=0;}
if (ps2x.Button(PSB_R3) ) {ch='m';}
if (ps2x.Button(PSB_L3)) {ch='n';}
if(PS_RX<5) {ch='o';for(int i=0;i<=7;i++) servopulse(servopin,160);}
else if(PS_RX>250) {ch='p';for(int i=0;i<=8;i++) servopulse(servopin,20);}
else for(int i=0;i<=5;i++) servopulse(servopin,90);
if(PS_RY<5) {ch='q';}
if(PS_RY>250) {ch='r';}
if(PS_LX<5) {ch='s';}
if(PS_LX>250) {ch='t';}
if(PS_LY<5) {ch='u';}
if(PS_LY>250) {ch='v';}
if (ch=='z') stop();
Serial.print(" ");
Serial.println(ch);
delay(50);
}
}
void button_set4() //////轉(zhuǎn)輪
{
if(type==2)
return;
else {
ch='z';
ps2x.read_gamepad(false, vibrate);
PS_LX=ps2x.Analog(PSS_LX);
PS_RX=ps2x.Analog(PSS_RX);
PS_LY=ps2x.Analog(PSS_LY);
PS_RY=ps2x.Analog(PSS_RY);
vibrate = ps2x.Analog(PSAB_BLUE);
if (ps2x.Button(PSB_START)) {ch='w';Serial.println("Start is being held");myse.runActionGroup(13,1);test=1;}
if (ps2x.Button(PSB_SELECT)) {ch='x';Serial.println("Select is being held");myse.runActionGroup(0,1);test=2; }
if (ps2x.Button(PSB_PAD_UP)) {ch='a';}
if (ps2x.Button(PSB_PAD_DOWN) ) {ch='b';}
if (ps2x.Button(PSB_PAD_LEFT) ) {ch='c';move(6, 255, 1);}
if (ps2x.Button(PSB_PAD_RIGHT)) {ch='d';move(6, 255, 0);}
if (ps2x.Button(PSB_GREEN)) {ch='e'; stop();}
if (ps2x.NewButtonState(PSB_BLUE)){ch='f';}
if (ps2x.Button(PSB_PINK)) {ch='g';}
if (ps2x.Button(PSB_RED)) {ch='h';}
if (ps2x.Button(PSB_R2) ) {ch='i';move(1, 255, 0);move(2, 255, 0);move(3, 255, 1);move(4, 255, 1);}
if (ps2x.Button(PSB_L2)) {ch='j';test=3;}
if (ps2x.Button(PSB_R1) ) {ch='k';move(1, 255, 1);move(2, 255, 1);move(3, 255, 0);move(4, 255, 0);}
if (ps2x.Button(PSB_L1)) {ch='l';myse.runActionGroup(14,1);test=0;}
if (ps2x.Button(PSB_R3) ) {ch='m';move(5, 255, 0);}
if (ps2x.Button(PSB_L3)) {ch='n';move(5, 255, 1);}
if(PS_RX<5) {ch='o';move(2, 255, 0);}
if(PS_RX>250) {ch='p';move(2, 255, 1);}
if(PS_RY<5) {ch='q';move(1, 255, 1);}
if(PS_RY>250) { ch='r';move(1, 255, 0);}
if(PS_LX<5) { ch='s';move(3, 255, 0);}
if(PS_LY<5) {ch='u';move(4,255,0);}
if(PS_LY>250) {ch='y';move(4,255,1);}
if (ch=='z') stop();
Serial.print(" ");
Serial.println(ch);
delay(50);
}
}
/**********主程序**************/
void setup()
{
Serial.begin(9600);
Serial2.begin(9600);
motor_start();
delay(300);
sensor_start();
delay(300);
pinMode(servopin,OUTPUT);//設(shè)定舵機(jī)接口為輸出接口
Wire.begin();
accelgyro.initialize();
delay(300);
error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT, pressures, rumble);
delay(50);
}
void loop()
{
if (test==0) button_set(); // 低姿態(tài)
if (test==1) button_set2(); // 高姿態(tài)
if (test==2) button_set3(); // 中姿態(tài) 打架 搖頭 揮手轉(zhuǎn)向 搖頭避障
if (test==3) button_set4(); // 轉(zhuǎn)輪
}
|
|
QQ好友和群
QQ空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
