Diff: main.cpp

Revision:

0:a4d8f5b3c546

Child:

2:99785a1007a4

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Jun 04 03:16:52 2016 +0000
@@ -0,0 +1,107 @@
+#include "mbed.h"
+#include "PID_v2.h"         //PID控制库
+#include "Microduino_Stepper.h"  //步进电机库
+#include "userDef.h"        //用户自定义
+#include "RollPitch.h"
+#include "Protocol.h"       //通讯协议库
+
+MPU6050 imu;
+Stepper stepperL(PIN_DIRB, PIN_STEPB);              //左电机，使用stepper底板A接口
+Stepper stepperR(PIN_DIRA, PIN_STEPA);              //右电机，使用stepper底板D接口
+PID speedPID((double)KP_SPD, (double)KI_SPD, (double)KD_SPD, DIRECT);     //速度环控制器
+PID anglePID((double)KP_ANG, (double)KI_ANG, (double)KD_ANG, DIRECT);     //角度环控制器
+
+uint16_t channalData[CHANNEL_NUM]; //8通道数据
+bool mode = 0; //nrf或者ble模式
+int16_t throttle;         //油门值
+int16_t steering;         //转向值
+unsigned long safe_ms = 0;
+
+float robotSpeedFilter;   //速度估计值
+float robotAngle;
+float targetSpeed;        //目标速度
+#ifdef BAT_DETECT
+float batteryValue;       //电池电压
+#endif
+float ypr[3];             //Yaw,Pitch,Roll三个轴的角度值
+
+DigitalOut myled(D13);
+#define constrain(x,a,b) ((x) < (a) ? (a) : ((x) > (b) ? (b) : (x)))
+Timer g_Timer;
+static long map(long x, long in_min, long in_max, long out_min, long out_max)
+{
+    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
+}
+int main()
+{
+    g_Timer.start();
+    dmpSetup();
+    mode = protocolSetup();  //遥控接收器初始化
+    speedPID.SetMode(AUTOMATIC);    //速度还控制器初始化
+    anglePID.SetMode(AUTOMATIC);    //角度环控制器初始化
+    speedPID.SetITermLimits(-10, 10);
+    speedPID.SetOutputLimits(-MAX_TARGET_ANGLE, MAX_TARGET_ANGLE);
+    anglePID.SetOutputLimits(-MAX_SPEED, MAX_SPEED);
+    stepperL.begin();       //左电机初始化
+    stepperR.begin();       //右电机初始化
+    for (uint8_t k = 0; k < 3; k++) {
+        stepperR.setSpeed(3);
+        stepperL.setSpeed(-3);
+        //delay(150);
+        wait_ms(150);
+        stepperR.setSpeed(-3);
+        stepperL.setSpeed(3);
+        //delay(150);
+        wait_ms(150);
+    }
+    while(1) {
+        if (protocolRead(channalData, mode)) { //判断是否接收到遥控信号
+            throttle = map(channalData[CHANNEL_THROTTLE], 1000, 2000, MAX_THROTTLE, -MAX_THROTTLE);
+            steering = map(channalData[CHANNEL_STEERING], 1000, 2000, -MAX_STEERING, MAX_STEERING);
+            //safe_ms = millis();
+            safe_ms = g_Timer.read_ms();
+        }
+
+        if (safe_ms > g_Timer.read_ms()) safe_ms = g_Timer.read_ms();
+        if (g_Timer.read_ms() - safe_ms > SAFE_TIME_OUT) {
+            throttle = 0;
+            steering = 0;
+        }
+
+#ifdef BAT_DETECT
+        batteryValue = (analogRead(A7) / 1024.0) * BAT_DETECT;   //采集电池电压
+#endif
+        dmpGetYPR(ypr);
+        float robotAngleCache = robotAngle;
+        robotAngle = ypr[DIRECTION] + ANGLE_FIX;          //取Roll轴角度
+        float robotSpeed = (stepperL.getSpeed() - stepperR.getSpeed()) / 2.0;   //计算小车速度
+
+        float angleVelocity = (robotAngle - robotAngleCache) * 90.0;
+        robotSpeedFilter = robotSpeedFilter * 0.95 + (robotSpeed + angleVelocity) * 0.05;       //小车速度滤波
+        float targetAngle = speedPID.Compute(robotSpeedFilter, throttle);       //速度环计算目标角度
+        targetSpeed += anglePID.Compute(robotAngle, targetAngle);         //角度环计算电机转速
+        targetSpeed = constrain(targetSpeed, -MAX_SPEED, MAX_SPEED);
+        int16_t motorR = targetSpeed + steering;    //计算右电机转速
+        int16_t motorL = -targetSpeed + steering;   //计算左电机转速
+
+        if ((robotAngle < 65) && (robotAngle > -65)) {
+            stepperAllEnable();
+            stepperR.setSpeed(motorR);        //更新右电机转速
+            stepperL.setSpeed(motorL);        //更新左电机转速
+
+            if ((robotAngle < 30) && (robotAngle > -30)) {
+                anglePID.SetTunings(KP_ANG, KI_ANG, KD_ANG);
+                speedPID.SetTunings(KP_SPD, KI_SPD, KD_SPD);
+            } else {
+                anglePID.SetTunings(KP_ANG_RAISUP, KI_ANG_RAISUP, KD_ANG_RAISUP);
+                speedPID.SetTunings(KP_SPD_RAISUP, KI_SPD_RAISUP, KD_SPD_RAISUP);
+            }
+        } else {
+            stepperAllDisable();
+            stepperR.setSpeed(0);        //更新右电机转速
+            stepperL.setSpeed(0);        //更新左电机转速
+            anglePID.SetTunings(KP_ANG_RAISUP, KI_ANG_RAISUP, KD_ANG_RAISUP);
+            speedPID.SetTunings(KP_SPD_RAISUP, KI_SPD_RAISUP, KD_SPD_RAISUP);
+        }
+    }
+}