久久久久久久999_99精品久久精品一区二区爱城_成人欧美一区二区三区在线播放_国产精品日本一区二区不卡视频_国产午夜视频_欧美精品在线观看免费

 找回密碼
 立即注冊

QQ登錄

只需一步，快速開始

搜索
查看: 6049|回復: 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 踩踩
回復

使用道具 舉報

您需要登錄后才可以回帖 登錄 | 立即注冊

本版積分規則

手機版|小黑屋|51黑電子論壇 |51黑電子論壇6群 QQ 管理員QQ:125739409;技術交流QQ群281945664

Powered by 單片機教程網

快速回復 返回頂部 返回列表
主站蜘蛛池模板： 欧美日韩免费一区二区三区 | 天天干狠狠操 | 九一在线| 精品国产乱码久久久久久久久 | 色天堂影院 | 国产精品久久久久久妇女6080 | 综合久久亚洲 | 久操av在线 | 国产精品一区二区三区四区 | 精品国产91乱码一区二区三区 | 九九亚洲精品 | 日韩视频在线免费观看 | 操皮视频 | 成人av高清 | 国产亚洲精品成人av久久ww | 亚洲精品乱码久久久久久按摩观 | 午夜免费观看体验区 | 亚洲免费毛片 | 久久国产精品无码网站 | 日韩精品一区二区三区视频播放 | 一级片免费观看 | 日韩在线一区二区三区 | 中文字幕久久精品 | 国产精品国产三级国产aⅴ原创 | 亚洲一区二区免费看 | 丁香久久 | 日韩a视频| 韩国久久 | 综合色播| 欧美日韩不卡在线 | 欧美一区二区免费 | 日韩精品一区二区三区中文在线 | 怡红院免费的全部视频 | 日本在线视频一区二区 | 精品一区二区三区91 | 欧美日韩精品一区二区 | 国产日韩欧美在线 | 免费国产一区二区视频 | 在线免费观看黄视频 | 欧美综合自拍 | 91在线看片 |