|
- #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;
- }
复制代码 |
|