久久精品国产99,欧美日本不卡,日韩免费网站

#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.初始化電機(jī)模塊引腳13,12,11,10為輸出模式******************/
/*
* 編寫代碼
*/
/********************2.設(shè)置小車啟動(dòng)速度為140---set_speed()******************/
/*
* 編寫代碼
*/
/********************3.初始化循跡模塊引腳A0, A1, A2, A3, A4為輸入模式******************/
/*
* 編寫代碼
*/
//3.設(shè)置PID參數(shù)
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次傳感器的傳感器的數(shù)據(jù)
//減少取平均值，濾波。
for (j = 0; j < 9; j ++) {
for (i = 0; i < 5; i++) {
level = digitalRead(IR_PIN[i]);
if (level) {
bitSet(irs[j], i); // 設(shè)置irs[j]的第jbit 為1
} else {
bitClear(irs[j], i); //清除irs[j]的第jbit為0
}
}
}
//對(duì)irs中的數(shù)據(jù)，進(jìn)行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)
{
//右邊壓線為負(fù)，左邊壓線為正。
//要左轉(zhuǎn)為負(fù)，要右轉(zhuǎn)為正
int curError = pid.error; //獲得上一次的pid誤差值
//壓線輸出低電平0，沒有壓線輸出高電平1
switch (irs) { //h'h
case B11110: curError = -8; break; //最右邊壓線
case B10000:
case B11000: curError = -7; break; //右四個(gè)或者右3個(gè)沿線
case B11100: curError = -6; break; //右兩個(gè)沿線
case B11101: curError = -4; break; //右倒數(shù)第二個(gè)沿線
case B11001: curError = -2; break; //
case B00000: //全部壓線和中間壓線，對(duì)應(yīng)"十字"彎道，直接郭即可。
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值來計(jì)算
case B11111: curError = pid.error > 0 ? 9 : - 9; break;
}
return curError;
}
void _sortData(int *p, int n)
{
int temp;
int i, j;
/**編寫冒泡排序，對(duì)采集到的錯(cuò)誤碼，進(jìn)行十次從小到大排序 **/
/*
* 編寫代碼
*/
return;
}
//計(jì)算當(dāng)前的誤差值，得到當(dāng)前的偏移值
void calcCurrentError(void)
{
int i;
uint8_t irs;
float sum = 0;
int errorData[10];
//1.采集10次循跡模塊獲得的錯(cuò)誤碼
for (i = 0; i < 10; i ++) {
irs = getIrData(); //得到這次傳感器采集的數(shù)據(jù)（5路數(shù)據(jù)）
errorData[i] = calcErrorByIrsValue(irs);
}
//2.要求對(duì)errorData中的數(shù)據(jù)進(jìn)行排序（從小到大排序）
/*
* 編寫代碼
*/
//3.十次中去處一個(gè)最高數(shù)，去除一個(gè)最低數(shù)，求中間8次錯(cuò)誤碼數(shù)據(jù)的和。
/*
* 編寫代碼
*/
//4.求8次錯(cuò)誤碼數(shù)據(jù)中的偏差。
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); //右正轉(zhuǎn)
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;
//獲得機(jī)器從啟動(dòng)到現(xiàn)在所運(yùn)行的時(shí)間
unsigned long now = millis();
int timeChange = now - lastTime;//得到這次的時(shí)間
//若是我們的這次變化的時(shí)間，小于pid的
//采樣時(shí)間，就需要等待一下。
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()函數(shù)的功能：若是*pvalue > motorSpeed,取 motorSpeed，
// 若是*pvalue < -motorSpeed,取 -motorSpeed,
// 若是-pValue<=*pvalue <= motorSpeed,取*pvalue
*pValue = constrain(*pValue, -motorSpeed,motorSpeed);
pid.previous_error = pid.error; //記錄本次的pid.error，作為下一次的歷史差值。
lastTime = now; // 記錄本次的運(yùn)行時(shí)間，作為下一次時(shí)間的歷史時(shí)間參考。
return true;
}
void calcDirection(void) //判斷小車的轉(zhuǎn)向
{
//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); //計(jì)算當(dāng)前pid的差值。，若是就緒了
if (ok) {
calcDirection();
motorControl(pidValue, turnFlag);
}
return;
}

復(fù)制代碼

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