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

<input id="utcnr"></input>

<kbd id="utcnr"><abbr id="utcnr"><strike id="utcnr"></strike></abbr></kbd>

<source id="utcnr"></source>

標題: 基于arduino的多足機器人程序 [打印本頁]

---

作者: 買哦    時間: 2018-11-19 21:25
標題: 基于arduino的多足機器人程序
//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();    // 轉輪
}

---

歡迎光臨 (http://www.zg4o1577.cn/bbs/)

Powered by Discuz! X3.1

感谢您访问我们的网站，您可能还对以下资源感兴趣：

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

主站蜘蛛池模板： 91麻豆精品国产91久久久久久 | 欧美a级成人淫片免费看 | 成人精品啪啪欧美成 | 男女国产视频 | 99精品一区二区三区 | 操操操日日日 | 九九久久这里只有精品 | 91在线精品秘密一区二区 | 黄视频网址 | 欧美激情精品久久久久久变态 | 国产欧美在线观看 | 亚洲狠狠爱一区二区三区 | 亚洲精品一区久久久久久 | 视频三区 | 青春草在线 | 成人精品一区二区 | 国产精品69久久久久水密桃 | 成人激情视频在线播放 | 欧美午夜精品理论片a级按摩 | 国产视频中文字幕在线观看 | 亚洲成人第一页 | 国产成人99久久亚洲综合精品 | 久久久www成人免费精品 | 在线免费观看成人 | 91精品国产日韩91久久久久久 | 亚洲国产成人精品女人久久久 | 国产中文原创 | 久久精品久久久久久 | 国产羞羞视频在线观看 | 免费在线观看成人 | 国产精品久久久久久久7电影 | 啪啪毛片| 中文字幕人成人 | 亚洲欧美中文日韩在线v日本 | 91在线第一页 | 亚洲一区毛片 | 91精品国产一区二区在线观看 | 欧美极品在线 | 亚洲综合一区二区三区 | 亚洲精品成人网 | 久久一区 |

<strike id="ughfb"></strike>