Arduino智能小車程序+資料 pid算法

ID:559809 · 發表于 2019-7-11 15:06

#include "pid.h"
#ifdef ARDUINO_DEBUG
int debugLeftSpeed;
int debugRightSpeed;
uint8_t debugIrs = 0;
#endif
const float motorSpeed = 140; //
const int IR_PIN[] = {A0, A1, A2, A3, A4}; //
const int IN_A1 = 13; //
const int IN_A2 = 12; //
const int IN_B1 = 11; //
const int IN_B2 = 10; //
const int _pwmLeftPin = 5;
const int _pwmRightPin = 6;
pid_t pid;
float pidValue = 0;
bool turnFlag = false;
void setup(void)
{
int i;
/********************1.初始化電機模塊引腳13,12,11,10為輸出模式******************/
/*
* 編寫代碼
*/
/********************2.設置小車啟動速度為140---set_speed()******************/
/*
* 編寫代碼
*/
/********************3.初始化循跡模塊引腳A0, A1, A2, A3, A4為輸入模式******************/
/*
* 編寫代碼
*/
//3.設置PID參數
pid.sampleTime = SAMPLE_TIME;
pid.Kp = KP_VALUE;
pid.Ki = KI_VALUE;
pid.Kd = KD_VALUE;
pid.error = 0;
pid.previous_error = 0;
Serial.begin(115200);
delay(5000);
goForward();
return;
}
//get ir data and middle filter
uint8_t getIrData(void) //
{
int i, j;
uint8_t level;
uint8_t temp;
uint8_t irs[9] = {0};//采集9次傳感器的傳感器的數據
//減少取平均值，濾波。
for (j = 0; j < 9; j ++) {
for (i = 0; i < 5; i++) {
level = digitalRead(IR_PIN[i]);
if (level) {
bitSet(irs[j], i); // 設置irs[j]的第jbit 為1
} else {
bitClear(irs[j], i); //清除irs[j]的第jbit為0
}
}
}
//對irs中的數據，進行9次排序，取最中間的值。
for (i = 0; i < 9 - 1; i ++) {
for (j = 0; j < 9 - i - 1; j ++) {
if (irs[j] > irs[j + 1]) {
temp = irs[j];
irs[j] = irs[j + 1];
irs[j + 1] = temp;
}
}
}
#ifdef ARDUINO_DEBUG
debugIrs = irs[4];
#endif
return irs[4];
}
int calcErrorByIrsValue(uint8_t irs)
{
//右邊壓線為負，左邊壓線為正。
//要左轉為負，要右轉為正
int curError = pid.error; //獲得上一次的pid誤差值
//壓線輸出低電平0，沒有壓線輸出高電平1
switch (irs) { //h'h
case B11110: curError = -8; break; //最右邊壓線
case B10000:
case B11000: curError = -7; break; //右四個或者右3個沿線
case B11100: curError = -6; break; //右兩個沿線
case B11101: curError = -4; break; //右倒數第二個沿線
case B11001: curError = -2; break; //
case B00000: //全部壓線和中間壓線，對應"十字"彎道，直接郭即可。
case B11011: curError = 0; break;
case B10011: curError = 2; break;
case B10111: curError = 4; break;
case B00111: curError = 6; break;
case B00011:
case B00001: curError = 7; break;
case B01111: curError = 8; break;
//都沒有沿線，按照上次的PID值來計算
case B11111: curError = pid.error > 0 ? 9 : - 9; break;
}
return curError;
}
void _sortData(int *p, int n)
{
int temp;
int i, j;
/**編寫冒泡排序，對采集到的錯誤碼，進行十次從小到大排序 **/
/*
* 編寫代碼
*/
return;
}
//計算當前的誤差值，得到當前的偏移值
void calcCurrentError(void)
{
int i;
uint8_t irs;
float sum = 0;
int errorData[10];
//1.采集10次循跡模塊獲得的錯誤碼
for (i = 0; i < 10; i ++) {
irs = getIrData(); //得到這次傳感器采集的數據（5路數據）
errorData[i] = calcErrorByIrsValue(irs);
}
//2.要求對errorData中的數據進行排序（從小到大排序）
/*
* 編寫代碼
*/
//3.十次中去處一個最高數，去除一個最低數，求中間8次錯誤碼數據的和。
/*
* 編寫代碼
*/
//4.求8次錯誤碼數據中的偏差。
pid.error = sum / 8;
return;
}
void turnRight(void)
{
digitalWrite(IN_B1, HIGH); //左正
digitalWrite(IN_B2, LOW);
digitalWrite(IN_A1, LOW); //右反
digitalWrite(IN_A2, HIGH);
}
void turnLeft(void)
{
digitalWrite(IN_B1, LOW);// 左反
digitalWrite(IN_B2, HIGH);
digitalWrite(IN_A1, HIGH); //右正
digitalWrite(IN_A2, LOW);
}
void goForward(void)
{
digitalWrite(IN_B1, HIGH); //左正傳
digitalWrite(IN_B2, LOW);
digitalWrite(IN_A1, HIGH); //右正轉
digitalWrite(IN_A2, LOW);
}
void motorControl(float pidValue, bool turnFlag)
{
int leftMotorSpeed = 0;
int rightMotorSpeed = 0;
leftMotorSpeed = constrain((motorSpeed + pidValue), -255, 255);
rightMotorSpeed = constrain((motorSpeed - pidValue), -255, 255);
if (turnFlag) {
if (abs(leftMotorSpeed) > abs(rightMotorSpeed)) {
leftMotorSpeed = abs(leftMotorSpeed);
rightMotorSpeed = leftMotorSpeed;
} else {
rightMotorSpeed = abs(rightMotorSpeed);
leftMotorSpeed = rightMotorSpeed;
}
} else {
leftMotorSpeed = leftMotorSpeed > 0 ? leftMotorSpeed : -leftMotorSpeed;
rightMotorSpeed = rightMotorSpeed > 0 ? rightMotorSpeed : -rightMotorSpeed;
}
analogWrite(_pwmLeftPin, leftMotorSpeed );
analogWrite(_pwmRightPin, rightMotorSpeed);
#ifdef ARDUINO_DEBUG
debugLeftSpeed = leftMotorSpeed ;
debugRightSpeed = rightMotorSpeed;
#endif
return;
}
bool calculatePid(float *pValue)
{
float P = 0;
static float I = 0 ;
float D = 0 ;
static unsigned long lastTime = 0;
//獲得機器從啟動到現在所運行的時間
unsigned long now = millis();
int timeChange = now - lastTime;//得到這次的時間
//若是我們的這次變化的時間，小于pid的
//采樣時間，就需要等待一下。
if (timeChange < pid.sampleTime) {
return false;
}
P = pid.error; //本次小車傳感器的偏移大小
I = I + pid.error;//歷史的偏移
D = pid.error - pid.previous_error; //最近兩次的差值
*pValue = (pid.Kp * P) + (pid.Ki * I) + (pid.Kd * D) + 1;
//constrain()函數的功能：若是*pvalue > motorSpeed,取 motorSpeed，
// 若是*pvalue < -motorSpeed,取 -motorSpeed,
// 若是-pValue<=*pvalue <= motorSpeed,取*pvalue
*pValue = constrain(*pValue, -motorSpeed,motorSpeed);
pid.previous_error = pid.error; //記錄本次的pid.error，作為下一次的歷史差值。
lastTime = now; // 記錄本次的運行時間，作為下一次時間的歷史時間參考。
return true;
}
void calcDirection(void) //判斷小車的轉向
{
//pid.error > 0 說明小車左邊壓線多，要
if (pid.error >= 7 && pid.error <= 8) {
turnRight();
turnFlag = true;
} else if (pid.error >= -8 && pid.error <= -7) {
turnLeft();
turnFlag = true;
} else if(pid.error == 9 || pid.error == -9){
turnRight();
turnFlag = true;
}else {
goForward();
turnFlag = false;
}
return;
}
void loop(void)
{
bool ok;
float pidValue;
calcCurrentError();
ok = calculatePid(&pidValue); //計算當前pid的差值。，若是就緒了
if (ok) {
calcDirection();
motorControl(pidValue, turnFlag);
}
return;
}

復制代碼

ID:560595 · 發表于 2019-7-16 13:46

這個要頂

ID:505027 · 發表于 2019-7-16 21:26

這個好，記錄一下

ID:594846 · 發表于 2019-8-4 22:31

真好，贊一個！

ID:609660 · 發表于 2019-9-9 19:22

這個資源確實好

ID:149774 · 發表于 2019-9-21 21:44

做個記號，回頭學習一下

ID:614623 · 發表于 2019-9-22 00:16

這個可以參考

ID:652804 · 發表于 2019-12-15 15:42

謝謝，學習學習。

ID:28942 · 發表于 2020-9-15 19:24

編輯不過是什么原因？

ID:1011808 · 發表于 2022-4-5 13:47

arduino如何設置小車啟動速度

帳號		自動登錄	找回密碼
密碼			立即注冊

久久久久久久999_99精品久久精品一区二区爱城_成人欧美一区二区三区在线播放_国产精品日本一区二区不卡视频_国产午夜视频_欧美精品在线观看免费

Arduino智能小車程序+資料 pid算法