Arduino平衡車——只角度環(huán)pid的代碼

但是僅靠一個PID控制會出現(xiàn)一個問題:因為小車的平衡角度Setpoint是手動設(shè)定的,所以會有一個微小的誤差。當小車以Setpoint為基準進行調(diào)整時,可能有一個細微的傾斜甸箱,從而導致小車一直向一邊跑。建議引入另一套速度的PID參數(shù):Sp,Si,Sd迅脐。

MPU6050接線
 SCL -- A5
 SDA -- A4
 INT -- PIN 2
 VCC -- 3.3V
 GND -- GND
*******************************************/

#include <PID_v1.h>
#include <LMotorController.h> //L298N控制庫
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
#define MIN_ABS_SPEED 10//20

MPU6050 mpu;
// MPU control/status vars
bool dmpReady = false; //如果DMP初始化成功芍殖,設(shè)置為true
uint8_t mpuIntStatus; // 保存來自MPU的實際中斷狀態(tài)字節(jié)
uint8_t devStatus; // 每次設(shè)備操作后返回狀態(tài) (0 = success, !0 = error)
uint16_t packetSize; //預期DMP包大小(默認為42字節(jié))
uint16_t fifoCount; // 當前FIFO中所有字節(jié)的計數(shù)
uint8_t fifoBuffer[64]; // FIFO存儲緩沖區(qū)

//  定向運動/motion vars
Quaternion q; // [w, x, y, z] 四維
VectorFloat gravity; // [x, y, z] 重力三維矢量
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll 和重力矢量

//PID
double originalSetpoint = 183; //173, 調(diào)節(jié)小車的穩(wěn)定中值, 180為默認值, 根據(jù)小車實際情況調(diào)試
double setpoint = originalSetpoint;
double movingAngleOffset = 0.1;
double input, output;

//adjust these values to fit your own design
//PID參數(shù)整定需要經(jīng)過多次嘗試, 首先調(diào)試P, 后D, 最后I
double Kp = 65;
double Kd = 1.2;
double Ki = 90;
PID pid(&input, &output, &setpoint, Kp, Ki, Kd, DIRECT);

//電機速度偏差調(diào)整
double motorSpeedFactorLeft = 0.6;
double motorSpeedFactorRight = 0.6;
//MOTOR CONTROLLER, L298N引腳定義
int ENA = 10;
int IN1 = 13;
int IN2 = 12;
int IN3 = 6;
int IN4 = 7;
int ENB = 9;
LMotorController motorController(ENA, IN1, IN2, ENB, IN3, IN4, motorSpeedFactorLeft, motorSpeedFactorRight);

volatile bool mpuInterrupt = false; // 指示MPU中斷引腳是否過高

void dmpDataReady()
{
  mpuInterrupt = true;
}


void setup()
{
  // 加入I2C總線(I2Cdev庫不會自動這樣做)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
  Wire.begin();
  TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
  Fastwire::setup(400, true);
#endif

  Serial.begin(115200);
  delay(1000);
  Serial.println("Start!");

  mpu.initialize(); //初始化MPU6050
  devStatus = mpu.dmpInitialize();
   // 校正mpu6050模塊
  mpu.setXGyroOffset(165);
  mpu.setYGyroOffset(-14);
  mpu.setZGyroOffset(5);
  mpu.setZAccelOffset(528); // 出廠默認
  // 確保它工作正常(如果是,返回0)
  if (devStatus == 0)
  {
    // turn on the DMP, now that it's ready
    mpu.setDMPEnabled(true);

    // 啟用Arduino中斷檢測
    attachInterrupt(0, dmpDataReady, RISING); //開啟INT0中斷, mpu6050INT引腳需要接到arduino的PIN 2
    mpuIntStatus = mpu.getIntStatus();

    // 設(shè)置DMP就緒標志谴蔑,以便main loop()函數(shù)知道可以使用它
    dmpReady = true;

    // 獲得期望的DMP包大小以便稍后比較
    packetSize = mpu.dmpGetFIFOPacketSize();

    //setup PID
    //設(shè)置PID調(diào)節(jié)
    pid.SetMode(AUTOMATIC);
    pid.SetSampleTime(5); //調(diào)節(jié)間隔為5ms
    pid.SetOutputLimits(-250, 250); //PWM限速
  }
  else
  {
    // 錯誤豌骏!
    // 1 = 初始內(nèi)存加載失敗
    // 2 = DMP配置更新失敗
    // (如果它要中斷,通常代碼是1)
    Serial.print(F("DMP Initialization failed (code "));
    Serial.print(devStatus);
    Serial.println(F(")"));
  }
}

void loop()
{
  // 如果編程失敗了隐锭,就停在這里
  if (!dmpReady) return;

  // 等待MPU中斷或額外數(shù)據(jù)包可用
  while (!mpuInterrupt && fifoCount < packetSize)
  {
    //沒有mpu數(shù)據(jù)-執(zhí)行PID計算和輸出到電機
    pid.Compute();
    motorController.move(output, MIN_ABS_SPEED);
    Serial.print(output);
    Serial.print(",");
  }

  // 重置中斷標志并獲得INT_STATUS字節(jié)
  mpuInterrupt = false;
  mpuIntStatus = mpu.getIntStatus();

  // 獲取當前FIFO計數(shù)
  fifoCount = mpu.getFIFOCount();

  // 檢查是否溢出(除非代碼效率太低窃躲,否則不會發(fā)生)
  if ((mpuIntStatus & 0x10) || fifoCount == 1024)
  {
    //重置
    mpu.resetFIFO();
    Serial.println(F("FIFO overflow!"));

    // 否則,檢查DMP數(shù)據(jù)就緒中斷(經(jīng)常發(fā)生)
  }
  else if (mpuIntStatus & 0x02)
  {
    // 等待正確的可用數(shù)據(jù)長度
    while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

    // 從FIFO讀取數(shù)據(jù)包
    mpu.getFIFOBytes(fifoBuffer, packetSize);

    // 跟蹤FIFO計數(shù)在這里钦睡,以防有多余1個包可用
    // (可以幫助我們立即閱讀更多內(nèi)容而無需等待中斷)
    fifoCount -= packetSize;

    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
    input = ypr[1] * 180 / M_PI + 180;
  }
最后編輯于
?著作權(quán)歸作者所有,轉(zhuǎn)載或內(nèi)容合作請聯(lián)系作者
  • 序言:七十年代末蒂窒,一起剝皮案震驚了整個濱河市,隨后出現(xiàn)的幾起案子荞怒,更是在濱河造成了極大的恐慌洒琢,老刑警劉巖,帶你破解...
    沈念sama閱讀 216,843評論 6 502
  • 序言:濱河連續(xù)發(fā)生了三起死亡事件褐桌,死亡現(xiàn)場離奇詭異衰抑,居然都是意外死亡,警方通過查閱死者的電腦和手機撩嚼,發(fā)現(xiàn)死者居然都...
    沈念sama閱讀 92,538評論 3 392
  • 文/潘曉璐 我一進店門停士,熙熙樓的掌柜王于貴愁眉苦臉地迎上來挖帘,“玉大人完丽,你說我怎么就攤上這事∧匆ǎ” “怎么了逻族?”我有些...
    開封第一講書人閱讀 163,187評論 0 353
  • 文/不壞的土叔 我叫張陵,是天一觀的道長骄崩。 經(jīng)常有香客問我聘鳞,道長薄辅,這世上最難降的妖魔是什么? 我笑而不...
    開封第一講書人閱讀 58,264評論 1 292
  • 正文 為了忘掉前任抠璃,我火速辦了婚禮站楚,結(jié)果婚禮上,老公的妹妹穿的比我還像新娘搏嗡。我一直安慰自己窿春,他們只是感情好,可當我...
    茶點故事閱讀 67,289評論 6 390
  • 文/花漫 我一把揭開白布采盒。 她就那樣靜靜地躺著旧乞,像睡著了一般。 火紅的嫁衣襯著肌膚如雪磅氨。 梳的紋絲不亂的頭發(fā)上尺栖,一...
    開封第一講書人閱讀 51,231評論 1 299
  • 那天,我揣著相機與錄音烦租,去河邊找鬼延赌。 笑死,一個胖子當著我的面吹牛叉橱,可吹牛的內(nèi)容都是我干的皮胡。 我是一名探鬼主播,決...
    沈念sama閱讀 40,116評論 3 418
  • 文/蒼蘭香墨 我猛地睜開眼赏迟,長吁一口氣:“原來是場噩夢啊……” “哼屡贺!你這毒婦竟也來了掷倔?” 一聲冷哼從身側(cè)響起耙饰,我...
    開封第一講書人閱讀 38,945評論 0 275
  • 序言:老撾萬榮一對情侶失蹤缠犀,失蹤者是張志新(化名)和其女友劉穎聚唐,沒想到半個月后愈涩,有當?shù)厝嗽跇淞掷锇l(fā)現(xiàn)了一具尸體撒顿,經(jīng)...
    沈念sama閱讀 45,367評論 1 313
  • 正文 獨居荒郊野嶺守林人離奇死亡慢显,尸身上長有42處帶血的膿包…… 初始之章·張勛 以下內(nèi)容為張勛視角 年9月15日...
    茶點故事閱讀 37,581評論 2 333
  • 正文 我和宋清朗相戀三年持际,在試婚紗的時候發(fā)現(xiàn)自己被綠了突想。 大學時的朋友給我發(fā)了我未婚夫和他白月光在一起吃飯的照片殴蹄。...
    茶點故事閱讀 39,754評論 1 348
  • 序言:一個原本活蹦亂跳的男人離奇死亡,死狀恐怖猾担,靈堂內(nèi)的尸體忽然破棺而出袭灯,到底是詐尸還是另有隱情,我是刑警寧澤绑嘹,帶...
    沈念sama閱讀 35,458評論 5 344
  • 正文 年R本政府宣布稽荧,位于F島的核電站,受9級特大地震影響工腋,放射性物質(zhì)發(fā)生泄漏姨丈。R本人自食惡果不足惜畅卓,卻給世界環(huán)境...
    茶點故事閱讀 41,068評論 3 327
  • 文/蒙蒙 一、第九天 我趴在偏房一處隱蔽的房頂上張望蟋恬。 院中可真熱鬧翁潘,春花似錦、人聲如沸歼争。這莊子的主人今日做“春日...
    開封第一講書人閱讀 31,692評論 0 22
  • 文/蒼蘭香墨 我抬頭看了看天上的太陽矾飞。三九已至一膨,卻和暖如春,著一層夾襖步出監(jiān)牢的瞬間洒沦,已是汗流浹背豹绪。 一陣腳步聲響...
    開封第一講書人閱讀 32,842評論 1 269
  • 我被黑心中介騙來泰國打工, 沒想到剛下飛機就差點兒被人妖公主榨干…… 1. 我叫王不留申眼,地道東北人瞒津。 一個月前我還...
    沈念sama閱讀 47,797評論 2 369
  • 正文 我出身青樓,卻偏偏與公主長得像括尸,于是被迫代替她去往敵國和親巷蚪。 傳聞我的和親對象是個殘疾皇子,可洞房花燭夜當晚...
    茶點故事閱讀 44,654評論 2 354

推薦閱讀更多精彩內(nèi)容