设为首页收藏本站
开启辅助访问 切换到宽版

 找回密码
 立即注册

QQ登录

只需一步，快速开始

热门： 51单片机 | 24小时必答区 | 单片机教程 | 单片机DIY制作 | STM32 | Cortex M3 | 模数电子 | 电子DIY制作 | 音响/功放 | 拆机乐园 | Arduino | 嵌入式OS | 程序设计

搜索
»论坛 › 嵌入式/单片机论坛 › Arduino › 基于arduino的多足机器人程序
返回列表 发新帖
查看: 6106|回复: 0
打印 上一主题 下一主题
收起左侧

基于arduino的多足机器人程序

[复制链接]
跳转到指定楼层
楼主
ID:429509 发表于 2018-11-19 21:25 | 只看该作者 回帖奖励 |倒序浏览 |阅读模式
//WenHao 2017/9/15   框架建立
//WenHao 2017/10/9   第一次调试开始
//WenHao 2017/10/15  电机模块测试
//WenHao 2017/11/1   电机+舵机整体测试
//WenHao 2017/11/4   整体遥控测试  
//WenHao 2017/11/9   挥手模块 摇头避障 姿态自适应调整
//WenHao 2017/11/11  传感器整体整合
#include <LobotServoController.h>
LobotServoController myse(Serial2);
/**********PS2手柄初始设置**********/
#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
/*********车轮初始设置************/
//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  : 不转 (LEFT)
int PWMF = 9;  
int FIN1 = 52;
int FIN2 = 50;
int PWME = 8;
int EIN1 = 48;
int EIN2 = 46;
//4    G: TURN    H  : 不转 (LEFT)
int PWMH = 7;  
int HIN1 = 44;
int HIN2 = 42;
int PWMG = 6;
int GIN1 = 38;
int GIN2 = 40;
/***********超声波初始设置*************/
#define TRIG   22           /*超声波TRIG引脚为 9号IO*/
#define ECHO   30           /*超声波ECHO引脚为 8号IO*/
#define MAX_DISTANCE 150    /*最大检测距离为150cm*/
#include <Servo.h>
#include <NewPing.h>
int servoPosition;   
/***********避障动作组****************/
#define GO_FORWARD  5      /*直走的动作组*/
#define GO_BACK     6      /*后退动作组*/
#define TURN_LEFT   7      /*左转动作组*/
#define TURN_RIGHT  8      /*右转动作组*/
#define MIN_DISTANCE_TURN 35  /*避障距离，就是小于多少距离的时候进行避障*/
#define BIAS 3             /*舵机偏差，根据实际情况调整大小以使超声波朝向正前方*/
NewPing Sonar(TRIG, ECHO, MAX_DISTANCE);      //实例化超声波测距类
Servo sonarServo;          //超声波云台舵机控制类实例
int gDistance;             //全局变量，用于存储中间位置超声波测得的距离
int gLDistance;            //用于存储机器人左侧测得的距离
int gRDistance;            //用于存储机器人右侧测得的距离
/**********传感器初始设置***************/
#define leftfront_infraredsensor   33   //距离很近之后发送低电平信号
#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);
/********摇头舵机控制**************/
int servopin=26;
int myangle;//定义角度变量
int pulsewidth;//定义脉宽变量
int val;
/********水平姿态**************/
#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;             //存储原始数据
float aax,aay,aaz,ggx,ggy,ggz;//存储量化后的数据
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)//定义一个脉冲函数
{
pulsewidth=(myangle*11)+500;//将角度转化为500-2480 的脉宽值
digitalWrite(servopin,HIGH);//将舵机接口电平至高
delayMicroseconds(pulsewidth);//延时脉宽值的微秒数
digitalWrite(servopin,LOW);//将舵机接口电平至低
delay(20-pulsewidth/1000);
}
int getDistance()
{       //获得距离
  uint16_t lEchoTime;     //变量 ，用于保存检测到的脉冲高电平时间
  lEchoTime = Sonar1.ping_median(3);           //检测6次超声波，排除错误的结果
  int lDistance = Sonar1.convert_cm(lEchoTime);//转换检测到的脉冲高电平时间为厘米
  Serial.print("distance_front:");
  Serial.print(lDistance);
  Serial.println("cm");
  return lDistance;                           //返回检测到的距离
}
void getAllDistance()//获得前及左右三个方向的距离
{
  int tDistance;     //用于暂存测得距离
  for(int i=0;i<=20;i++)  
   servopulse(servopin,90);   //超声波云台舵机转到90度即中间位置
  delay(200);                    //等待200ms，等待舵机转动到位
  gDistance = getDistance();     //测量距离，保存到全局变脸gDistance
  
  for(int i=0;i<=20;i++)  
   servopulse(servopin,130);    //超声波云台舵机转到130度位置即机器人左面40度位置
  delay(200);                    //延时，等待舵机转动到位
  tDistance = getDistance();     //测量距离，保存到 tDistance
for(int i=0;i<=20;i++)  
   servopulse(servopin,170);    //转动到170度，即机器人左侧80度位置
  delay(200);                    //延时，等待舵机转动到位
  gLDistance = getDistance();    //测量距离，保存到 gLDistance
  if(tDistance < gLDistance)     //比较左侧测得的两个距离，取小的一个，保存到gLDistance作为左侧距离
    gLDistance = tDistance;
  
for(int i=0;i<=20;i++)  
   servopulse(servopin,50);     //超声波云台舵机转到50度位置即机器人右面40度位置   
  delay(200);                    //延时，等待舵机转动到位
  tDistance = getDistance();     //测量距离，保存到tDistance
  for(int i=0;i<=20;i++)  
   servopulse(servopin,10);      //转到10度，即机器人右面80度位置
  delay(200);                    //延时，等待舵机转动到位
  gRDistance = getDistance();    //测量距离，保存到gRDistance
  if(tDistance < gRDistance)     //比较两个距离，将较小的一个保存到gRDistance
    gRDistance = tDistance;      
   
  for(int i=0;i<=7;i++)  
   servopulse(servopin,90);     //超声波云台舵机转回中间位置
}
void pin32_irq()
{
  irqNum = 4;  //记录中断号
  detachInterrupt(5);  //失能另一个中断，这样irqNum就不会被另一个中断赋值
}
void pin34_irq()
{
  irqNum = 5; //记录中断号
  detachInterrupt(4);
}
void move1() {  //程序的主要逻辑
  static uint32_t timer = 0;   //定义静态变量Timer， 用于计时
  static int step = 0;         //静态变量，用于记录步骤  
  if(timer > millis())  //如果设定时间大于当前毫秒数，则返回，否则继续
    return;
  switch (step) {  //根据步骤step做分支
    case 0: //步骤0
       step = 1;  //步骤转移到1
        attachInterrupt(5, pin34_irq, FALLING); //配置1号中断，下降沿触发
        attachInterrupt(4, pin32_irq, FALLING); //配置0号中断，下降沿触发
      break; //结束switch循环
    case 1: //步骤
      if (irqNum == 4)  myse.runActionGroup(7,1);
      if (irqNum == 5)   myse.runActionGroup(8,1);
      irqNum = 3;  //赋值为一个既不是0也不是1的数值
      timer = millis() + 300;  //延时300毫秒
      step = 0;  //重新回到步骤0
      break;  //结束switch语句
  }
}
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号io为舵机控制iO
  sonarServo.write(90 + BIAS);                //转到中间位置，加上了偏差
  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;   //静态变量，用于计时
  static uint8_t step = 0;     //静态变量，用于记录步骤
  if (timer > millis())  //如果设定时间大于当前毫秒数则返回，否侧继续后续操作
    return;
  switch (step)  //根据step分支
  {
    case 0:  //步骤0
      gDistance = getDistance();   //测量距离，保存到gDistance
      if (gDistance > MIN_DISTANCE_TURN || gDistance == 0) {  //如果测得距离大于指定的避障距离，前进
        {
          myse.runActionGroup(GO_FORWARD, 0); //一直前进
          step = 1; //转移到步骤1
          timer = millis() + 500;  //延时500ms
        }
      } else {  //如果测得距离小于指定距离
        step = 2;  //转移到步骤2
        timer = millis() + 100; //延时100ms
      }
      break; //结束switch语句
    case 1:  //步骤1
      gDistance = getDistance(); //测量距离
      if (gDistance < MIN_DISTANCE_TURN && gDistance > 0) {  //如果测得距离小于指定的避障距离，则停止所有动作组，转移到步骤2
        myse.stopActionGroup();
        step = 2;
      }
      break; //结束switch语句
    case 2:  //步骤2
      {   //没有动作组在运行，即等待动作组运行完毕
        getAllDistance();            //获得三个方向的距离
        Serial.print(gDistance);  //打印测得距离
        Serial.print("cm   ");
       Serial.print(gLDistance);
       Serial.print("cm   ");
        Serial.print(gRDistance);
        Serial.println("cm   ");
        step = 3; //转移到步骤3
        //此处没有break，执行完后直接之心case 3
      }
    case 3:  //步骤3
      static bool lastActionIsGoBack = false;   //静态变量，记录最后的动作是不是后退
      if (((gDistance > MIN_DISTANCE_TURN) || (gDistance == 0)) && lastActionIsGoBack == false) {
        //中间距离大于指定避障距离且最后的一个动作不是后退，那么就回到步骤0，
        //此处判断最后一个动作是不是后退，是避免程序陷入后退-》前进-》后退-》前进...这样的死循环
        //当最后一步是后退是就不执行前进
        step = 0;
        timer = millis() + 200;
        lastActionIsGoBack = false;
        return; //返回，结束函数
      }
      if ((((gLDistance > gRDistance) && (gLDistance > MIN_DISTANCE_TURN)) || gLDistance == 0) && gDistance > 15) {
      //超声波测得左侧的最小距离大于右侧的最小距离大于指定的避障距离，并且中间测得距离大于15时
      //检测中间的距离目的是避免有物体处于机器人两个前腿之间，导致机器人无法转向
        {   //等待动作组运行完毕
          myse.runActionGroup(TURN_LEFT, 2);  //左转4次，
          lastActionIsGoBack = false;  //标识最后一个动作不是后退
          step = 2;  //转移到步骤2
        }
        timer = millis() + 500; //延时500ms
        return; //返回，结束函数
      }
      if ((((gRDistance > gLDistance) && (gRDistance > MIN_DISTANCE_TURN)) || gRDistance == 0) && gDistance > 15) {
      //超声波测得左侧的最小距离大于右侧的最小距离大于指定的避障距离，并且中间测得距离大于15时
         {   //等待动作组运行完毕
          myse.runActionGroup(TURN_RIGHT, 2);  //右转4次
          lastActionIsGoBack = false;  //标识最后一个动作不是后退
          step = 2;  //转移到步骤2
        }
        timer = millis() + 500;   //延时500ms
        return;  //返回，结束函数
      }
      //当前面的都不符合，所有的return都没有被执行
      //程序就会执行到这里
      myse.runActionGroup(GO_BACK, 3);  //执行后退动作组3次
      lastActionIsGoBack = true;  //标识最后一个动作是后退
      step = 2;     //转移到步骤2
      timer = millis() + 500;     //延时500ms
  }
}
void tuoluo()
{
  accelgyro.getMotion6(&ax,&ay,&az,&gx,&gy,&gz);//获取三个轴的加速度和角速度
//======一下三行是对加速度进行量化，得出单位为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);//这个用来控制采样速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()   ////高姿态
{
    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()         ///中姿态
{
    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()   //////转轮
{  
    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);//设定舵机接口为输出接口
    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();     // 低姿态
    if (test==1)  button_set2();    // 高姿态
    if (test==2)  button_set3();    // 中姿态 打架 摇头  挥手转向   摇头避障
    if (test==3)  button_set4();    // 转轮
}


分享到:  QQ好友和群QQ好友和群 QQ空间QQ空间 腾讯微博腾讯微博 腾讯朋友腾讯朋友
收藏收藏3 分享淘帖 顶顶1 踩踩
回复

使用道具 举报

返回列表 发新帖
高级模式
B Color Image Link Quote Code Smilies
您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

手机版|小黑屋|51黑电子论坛 |51黑电子论坛6群 QQ 管理员QQ:125739409;技术交流QQ群281945664

Powered by 单片机教程网

快速回复 返回顶部 返回列表