arduino舵机摆锤

arduino程序功能：

arduino代码：

#include <Servo.h>
#define hongwai_pin 4
#define servo_pin 5
#define beep_pin 6
#define servo_z_rag 20
#define servo_f_rag 65
//#define beep_d 500
#define servo_speed 10

int servo_z_s=0;

class Hongwai{
  public:
    bool last_status=true;          //红外传暗器输出高电平代表未检测到障碍物
    bool now_status=true;
    void hongwai_init();
    bool hongwai_jiance();
};

void Hongwai::hongwai_init(){
  pinMode(hongwai_pin,INPUT);
};

//障碍物检测函数，如果出现障碍物则返回1，未出现障碍物返回0
bool Hongwai::hongwai_jiance(){
  if(digitalRead(hongwai_pin)==false)
    {
      delay(5);
      if(digitalRead(hongwai_pin)==false)
      {
        if(last_status==true)
        {
          last_status=false;
          Serial.println("zhangaiwu chuxian");
          return true;
        }
      }
    }
    if(digitalRead(hongwai_pin)==true)
    {
      delay(5);
      if(digitalRead(hongwai_pin)==true)
      {
        if(last_status==false)
        {
          Serial.println("zhangaiwu xiaoshi");
          last_status=true;
        }
      }
    }
    return false;
};

Hongwai hongwai;

class Beep_s{
  public:
    int beep_d;
    void beep_init();
    void di(int beep_d){
        digitalWrite(beep_pin,HIGH);
        delay(beep_d);
        digitalWrite(beep_pin,LOW);
      };
    void didi();
};

void Beep_s::beep_init(){
  pinMode(beep_pin,OUTPUT);
  digitalWrite(beep_pin,LOW);
};



void Beep_s::didi(){
  digitalWrite(beep_pin,HIGH);
  delay(beep_d);
  digitalWrite(beep_pin,HIGH);
  delay(beep_d);
  digitalWrite(beep_pin,LOW);
};

Beep_s beep;

Servo myservo;

//舵机正常运转
void servo_init(){
  pinMode(servo_pin,OUTPUT);
  myservo.attach(servo_pin);
  myservo.write(90);
};
//舵机正常运转
void servo_zy(){
  for(int i=90;i<=90+servo_z_rag;i++)
    {
      myservo.write(i);
      delay(servo_speed);
      if(hongwai.hongwai_jiance())
        servo_z_s=1;
    }
   for(int i=90+servo_z_rag;i>=90-servo_z_rag;i--)
   {
      myservo.write(i);
      delay(servo_speed);
      if(hongwai.hongwai_jiance())
        servo_z_s=1;
   }
   for(int i=90-servo_z_rag;i<=90;i++)
   {
      myservo.write(i);
      delay(servo_speed);
      if(hongwai.hongwai_jiance())
        servo_z_s=1;
   }
   
};
//舵机65度运转
void servo_fy(){
  for(int i=90;i<=90+servo_f_rag;i++)
    {
      myservo.write(i);
      delay(servo_speed);
    }
    beep.di(500);
   for(int i=90+servo_f_rag;i>=90-servo_f_rag;i--)
   {
      myservo.write(i);
      delay(servo_speed);
   }
   beep.di(500);
   for(int i=90-servo_f_rag;i<=90;i++)
   {
      myservo.write(i);
      delay(servo_speed);
   }
   beep.di(1000);
   delay(4600);
   beep.di(100);
   delay(200);
   beep.di(100);
   servo_z_s=0;
};




void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  servo_init();
  hongwai.hongwai_init();
  beep.beep_init();
  Serial.println("Init successful!");

}

void loop() {
  // put your main code here, to run repeatedly:
  if(servo_z_s==1)
      servo_fy();
  servo_zy();  
}