基于arduino的PID平衡车程序

ID:931725 · 发表于 2021-6-3 11:24

#include <PS2X_lib.h>  //for v1.6
#define PS2_DAT       A0  //14
#define PS2_CMD       A1  //15
#define PS2_SEL       A2  //16
#define PS2_CLK       A3  //17
#define pressures true
#define rumble    true
PS2X ps2x;
int error = 0;
byte type = 0;
byte vibrate = 0;
#define left_turn 0x06//按键左旋转
#define right_turn  0x07//按键右旋转
struct _pid{
float SetSpeed;          //定义设定值
float ActualSpeed;       //定义实际值
float err;             //定义偏差值
float err_last;          //定义上一个偏差值
float Kp,Ki,Kd;          //定义比例、积分、微分系数
float voltage;       //定义电压值（控制执行器的变量）
float integral;          //定义积分值
}pid;
void PID_init(){             //PID初始化,调整参数
printf("PID_init begin \n");
pid.SetSpeed=0.0;
pid.ActualSpeed=0.0;
pid.err=0.0;
pid.err_last=0.0;
pid.voltage=0.0;
pid.integral=0.0;
pid.Kp=0.2;
pid.Ki=0.015;
pid.Kd=0.2;
printf("PID_init end \n");
}
float PID_realize(float speed){
pid.SetSpeed=speed;
pid.err=pid.SetSpeed-pid.ActualSpeed;
pid.integral+=pid.err;
pid.voltage=pid.Kp*pid.err+pid.Ki*pid.integral+pid.Kd*(pid.err-pid.err_last);
pid.err_last=pid.err;
pid.ActualSpeed=pid.voltage*1.0;
return pid.ActualSpeed;
}
enum {
  enSTOP = 1,
  enRUN,
  enBACK,
  enLEFT,
  enRIGHT,
  enUPLEFT,
  enUPRIGHT,
  enDOWNLEFT,
  enDOWNRIGHT
} enCarState;
#define level1  0x08//速度控制标志位1
#define level2  0x09//速度控制标志位2
#define level3  0x0A//速度控制标志位3
#define level4  0x0B//速度控制标志位4
#define level5  0x0C//速度控制标志位5
#define level6  0x0D//速度控制标志位6
#define level7  0x0E//速度控制标志位7
#define level8  0x0F//速度控制标志位8
int Left_motor_back = 9;    //左电机后退(IN1)
int Left_motor_go = 5;    //左电机前进(IN2)
int Right_motor_go = 6; // 右电机前进(IN3)
int Right_motor_back = 10; // 右电机后退(IN4)
int buzzer = 8;//设置控制蜂鸣器的数字IO脚

int control = 150;//PWM控制量

int g_carstate = enSTOP; //  1前2后3左4右0停止
int g_servostate = 0;  //1左摇 2 右摇
/*超声波*/
int Echo = A5;  // Echo回声脚(P2.0)
int Trig = A4; //  Trig 触发脚(P2.1)
int Distance = 0;

/*舵机*/
int servopin = 2;  //设置舵机驱动脚到数字口2

/*点灯*/
int Led = 13; //
/*灭火*/
int Fire = 12; //
int speakerPin = 3;  //11
void PS2_Ctrol(void);

void setup()
{
  //初始化电机驱动IO为输出方式
  pinMode(Left_motor_go, OUTPUT); // PIN 5 (PWM)
  pinMode(Left_motor_back, OUTPUT); // PIN 9 (PWM)
  pinMode(Right_motor_go, OUTPUT); // PIN 6 (PWM)
  pinMode(Right_motor_back, OUTPUT); // PIN 10 (PWM)
  pinMode(buzzer, OUTPUT); //设置数字IO脚模式，OUTPUT为输出
  pinMode(Echo, INPUT); // 定义超声波输入脚
  pinMode(Trig, OUTPUT); // 定义超声波输出脚
  pinMode(servopin, OUTPUT);    //设定舵机接口为输出接口
  pinMode(Led, OUTPUT); // 定义点灯输出脚
  pinMode(Fire, OUTPUT); // 定义灭火输出
  pinMode(speakerPin, OUTPUT); //定义唱歌引脚

  //digitalWrite(buzzer,HIGH); //不发声
  // digitalWrite(Led,HIGH);
  // digitalWrite(Fire,HIGH);
  Serial.begin(9600);       //波特率9600 （蓝牙通讯设定波特率）
  /*PS2初始化*/
  error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT, pressures, rumble);
  type = ps2x.readType();
  switch (type) {
case 0:
   Serial.print("Unknown Controller type found ");
   break;
case 1:
   Serial.print("DualShock Controller found ");  //这种
   break;
case 2:
   Serial.print("GuitarHero Controller found ");
   break;
case 3:
   Serial.print("Wireless Sony DualShock Controller found ");
   break;
  }

}

void Distance_test() // 量出前方距离
{
  digitalWrite(Trig, LOW); // 给触发脚低电平2μs
  delayMicroseconds(2);
  digitalWrite(Trig, HIGH);  // 给触发脚高电平10μs，这里至少是10μs
  delayMicroseconds(10);
  digitalWrite(Trig, LOW); // 持续给触发脚低电
  float Fdistance = pulseIn(Echo, HIGH);  // 读取高电平时间(单位：微秒)
  Fdistance = Fdistance / 58; //为什么除以58等于厘米，  Y米=（X秒*344）/2
  // X秒=（ 2*Y米）/344 ==》X秒=0.0058*Y米 ==》厘米=微秒/58
  //Serial.print("Distance:");    //输出距离（单位：厘米）
  //Serial.println(Fdistance);       //显示距离
  Distance = Fdistance;
}

void run()    // 前进
{
  int speed=PID_realize(200.0);
  analogWrite(Right_motor_back, speed);
  analogWrite(Left_motor_back, speed);
  analogWrite(Right_motor_go, control); //PWM比例0~255调速，左右轮差异略增减
  analogWrite(Left_motor_go, control - 25); //PWM比例0~255调速，左右轮差异略增减
  //delay(time * 100); //执行时间，可以调整
}

void brake()       //刹车，停车
{
  int speed=PID_realize(0.0);
  analogWrite(Right_motor_back, speed);
  analogWrite(Left_motor_back, speed);
  //delay(time * 100);//执行时间，可以调整
}

void left()       //左转(左轮不动，右轮前进)
{
  int speed=PID_realize(200.0);
  analogWrite(Right_motor_back, speed);
  analogWrite(Left_motor_back, 0);
}

void right()       //右转(右轮不动，左轮前进)
{
  int speed=PID_realize(200.0);
  analogWrite(Right_motor_back, speed);
  analogWrite(Left_motor_back, speed);
}
void upright()       //右上(右轮不动，左轮前进)
{
  digitalWrite(Right_motor_back, LOW);
  digitalWrite(Left_motor_back, LOW);
  digitalWrite(Right_motor_go, HIGH); // 右电机前进
  digitalWrite(Left_motor_go, HIGH); // 左电机前进

  analogWrite(Right_motor_go, 120); //PWM比例0~255调速，左右轮差异略增减  右电机减速
  analogWrite(Left_motor_go, 180); //PWM比例0~255调速，左右轮差异略增减
}
void front_detection()
{
  //此处循环次数减少，为了增加小车遇到障碍物的反应速度
  for (int i = 0; i <= 5; i++) //产生PWM个数，等效延时以保证能转到响应角度
  {
// servopulse(servopin, 90); //模拟产生PWM
  }
}

void left_detection()
{
  for (int i = 0; i <= 15; i++) //产生PWM个数，等效延时以保证能转到响应角度
  {
// servopulse(servopin, 175); //模拟产生PWM
  }
}

void right_detection()
{
  for (int i = 0; i <= 15; i++) //产生PWM个数，等效延时以保证能转到响应角度
  {
// servopulse(servopin, 5); //模拟产生PWM
  }
}

void loop()
{
  int temp = 0;
  PS2_Ctrol();
  switch (g_carstate)
  {
case enRUN: run();  break;
case enLEFT: left();  break;
case enRIGHT: right(); break;
//case enBACK: back(); break;
default: break;
  }
  switch (g_servostate)
  {
case 0: if (temp != 0) {
      temp = 0;
      front_detection();
   } break;
case 1: if (temp != 1) {
      temp = 1;
      left_detection();
   } break;
case 2: if (temp != 2) {
      temp = 2;
      right_detection();
   } break;
default: break;
  }
  delay(50);
}

void PS2_Ctrol(void)
{
  int X1, Y1, X2, Y2;
  if (error == 1) //skip loop if no controller found
return;
  if (type != 1) //skip loop if no controller found
return;
  //DualShock Controller
  ps2x.read_gamepad(false, vibrate); //read controller and set large motor to spin at 'vibrate' speed

  if (ps2x.Button(PSB_START))       //will be TRUE as long as button is pressed
Serial.println("Start is being held");
  if (ps2x.Button(PSB_SELECT))
Serial.println("Select is being held");

  if (ps2x.Button(PSB_PAD_UP)) {    //will be TRUE as long as button is pressed
Serial.print("Up held this hard: ");
Serial.println(ps2x.Analog(PSAB_PAD_UP), DEC);
g_carstate = enRUN;
  }
  else if (ps2x.Button(PSB_PAD_RIGHT)) {
Serial.print("Right held this hard: ");
Serial.println(ps2x.Analog(PSAB_PAD_RIGHT), DEC);
g_carstate = enRIGHT;
  }
  else if (ps2x.Button(PSB_PAD_LEFT)) {
Serial.print("LEFT held this hard: ");
Serial.println(ps2x.Analog(PSAB_PAD_LEFT), DEC);
g_carstate = enLEFT;
  }
  else if (ps2x.Button(PSB_PAD_DOWN)) {
Serial.print("DOWN held this hard: ");
Serial.println(ps2x.Analog(PSAB_PAD_DOWN), DEC);
g_carstate = enBACK;
  }
  else
  {
g_carstate = enSTOP;
  }

  vibrate = ps2x.Analog(PSAB_CROSS);  //this will set the large motor vibrate speed based on how hard you press the blue (X) button
  if (ps2x.NewButtonState())
  { //will be TRUE if any button changes state (on to off, or off to on)
if (ps2x.Button(PSB_L3))//停止
{
   g_carstate = enSTOP;
   Serial.println("L3 pressed");
}
if (ps2x.Button(PSB_R3)) //复位
{
   front_detection();
   Serial.println("R3 pressed");
}
if (ps2x.Button(PSB_L2)) //加速
{
   Serial.println("L2 pressed");
   control += 50;
   if (control > 255)
   {
      control = 255;
   }
}
if (ps2x.Button(PSB_R2))
{
   Serial.println("R2 pressed");
   control -= 50;
   if (control < 50)
   {
      control = 100;
   }
}
if (ps2x.Button(PSB_TRIANGLE)) //点灯
{
   Serial.println("Triangle pressed");
   digitalWrite(Led, !digitalRead(Led));  //反转电平
}
  }

  if (ps2x.ButtonPressed(PSB_CIRCLE))//灭火             //will be TRUE if button was JUST pressed
  {
Serial.println("Circle just pressed");
digitalWrite(Fire, !digitalRead(Fire)); //反转电平
  }
  if (ps2x.NewButtonState(PSB_CROSS))//鸣笛             //will be TRUE if button was JUST pressed OR released
  {
Serial.println("X just changed");
// whistle();
  }
  if (ps2x.ButtonReleased(PSB_SQUARE))//唱歌          //will be TRUE if button was JUST released
  {
Serial.println("Square just released");
//PlayTest();
  }

  if (ps2x.Button(PSB_L1) || ps2x.Button(PSB_R1))
  { //print stick values if either is TRUE
Serial.print("Stick Values:");
Serial.print(ps2x.Analog(PSS_LY), DEC); //Left stick, Y axis. Other options: LX, RY, RX
Serial.print(",");
Serial.print(ps2x.Analog(PSS_LX), DEC);
Serial.print(",");
Serial.print(ps2x.Analog(PSS_RY), DEC);
Serial.print(",");
Serial.println(ps2x.Analog(PSS_RX), DEC);
Y1 = ps2x.Analog(PSS_LY);
X1 = ps2x.Analog(PSS_LX);
Y2 = ps2x.Analog(PSS_RY);
X2 = ps2x.Analog(PSS_RX);

/*左摇杆*/
if (Y1 < 5 && X1 > 80 && X1 < 180) //上
{
   g_carstate = enRUN;
}
else if (Y1 > 230 && X1 > 80 && X1 < 180) //下
{
   g_carstate = enBACK;
}
else if (X1 < 5 && Y1 > 80 && Y1 < 180) //左
{
   g_carstate = enLEFT;
}
else if (Y1 > 80 && Y1 < 180 && X1 > 230)//右
{
   g_carstate = enRIGHT;
}
else if (Y1 <= 80 && X1 <= 80) //左上
{
   g_carstate = enUPLEFT;
}
else if (Y1 <= 80 && X1 >= 180) //右上
{
   g_carstate = enUPRIGHT;
}
else if (X1 <= 80 && Y1 >= 180) // 左下
{
   g_carstate = enDOWNLEFT;
}
else if (Y1 >= 180 && X1 >= 180) //右下
{
   g_carstate = enDOWNRIGHT;
}
else//停
{
   g_carstate = enSTOP;
}

/*右摇杆*/
if (X2 < 5 && Y2 > 110 && Y2 < 150) //左
{
   g_servostate = 1;
}
else if (Y2 > 110 && Y2 < 150 && X2 > 230)//右
{
   g_servostate = 2;
}
else//归位
{
   g_servostate = 0;
}

  }
}