基于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;
}

  }
}