基于arduino的PID平衡车程序

#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;
   }


   }
}

给个图片

感觉很不错，可以用用试试

用什么硬件啊？

有硬件实物图么？

看着还不错，感谢楼主分享。

想请问一下楼主，有没有是基于51单片机来实现跷跷板上，pid的程序啊？