Li Weiyi / Mbed 2 deprecated BalanceCar

虽然移植完毕，但是不work。需要细调……

Dependencies:   mbed

main.cpp@0:a4d8f5b3c546, 2016-06-04 (annotated)

Committer:
lixianyu
Date:
Sat Jun 04 03:16:52 2016 +0000
Revision:
0:a4d8f5b3c546
Child:
2:99785a1007a4
Pass compile!!

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lixianyu 0:a4d8f5b3c546 1 #include "mbed.h"
lixianyu 0:a4d8f5b3c546 2 #include "PID_v2.h" //PID控制库
lixianyu 0:a4d8f5b3c546 3 #include "Microduino_Stepper.h" //步进电机库
lixianyu 0:a4d8f5b3c546 4 #include "userDef.h" //用户自定义
lixianyu 0:a4d8f5b3c546 5 #include "RollPitch.h"
lixianyu 0:a4d8f5b3c546 6 #include "Protocol.h" //通讯协议库
lixianyu 0:a4d8f5b3c546 7
lixianyu 0:a4d8f5b3c546 8 MPU6050 imu;
lixianyu 0:a4d8f5b3c546 9 Stepper stepperL(PIN_DIRB, PIN_STEPB); //左电机，使用stepper底板A接口
lixianyu 0:a4d8f5b3c546 10 Stepper stepperR(PIN_DIRA, PIN_STEPA); //右电机，使用stepper底板D接口
lixianyu 0:a4d8f5b3c546 11 PID speedPID((double)KP_SPD, (double)KI_SPD, (double)KD_SPD, DIRECT); //速度环控制器
lixianyu 0:a4d8f5b3c546 12 PID anglePID((double)KP_ANG, (double)KI_ANG, (double)KD_ANG, DIRECT); //角度环控制器
lixianyu 0:a4d8f5b3c546 13
lixianyu 0:a4d8f5b3c546 14 uint16_t channalData[CHANNEL_NUM]; //8通道数据
lixianyu 0:a4d8f5b3c546 15 bool mode = 0; //nrf或者ble模式
lixianyu 0:a4d8f5b3c546 16 int16_t throttle; //油门值
lixianyu 0:a4d8f5b3c546 17 int16_t steering; //转向值
lixianyu 0:a4d8f5b3c546 18 unsigned long safe_ms = 0;
lixianyu 0:a4d8f5b3c546 19
lixianyu 0:a4d8f5b3c546 20 float robotSpeedFilter; //速度估计值
lixianyu 0:a4d8f5b3c546 21 float robotAngle;
lixianyu 0:a4d8f5b3c546 22 float targetSpeed; //目标速度
lixianyu 0:a4d8f5b3c546 23 #ifdef BAT_DETECT
lixianyu 0:a4d8f5b3c546 24 float batteryValue; //电池电压
lixianyu 0:a4d8f5b3c546 25 #endif
lixianyu 0:a4d8f5b3c546 26 float ypr[3]; //Yaw,Pitch,Roll三个轴的角度值
lixianyu 0:a4d8f5b3c546 27
lixianyu 0:a4d8f5b3c546 28 DigitalOut myled(D13);
lixianyu 0:a4d8f5b3c546 29 #define constrain(x,a,b) ((x) < (a) ? (a) : ((x) > (b) ? (b) : (x)))
lixianyu 0:a4d8f5b3c546 30 Timer g_Timer;
lixianyu 0:a4d8f5b3c546 31 static long map(long x, long in_min, long in_max, long out_min, long out_max)
lixianyu 0:a4d8f5b3c546 32 {
lixianyu 0:a4d8f5b3c546 33 return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
lixianyu 0:a4d8f5b3c546 34 }
lixianyu 0:a4d8f5b3c546 35 int main()
lixianyu 0:a4d8f5b3c546 36 {
lixianyu 0:a4d8f5b3c546 37 g_Timer.start();
lixianyu 0:a4d8f5b3c546 38 dmpSetup();
lixianyu 0:a4d8f5b3c546 39 mode = protocolSetup(); //遥控接收器初始化
lixianyu 0:a4d8f5b3c546 40 speedPID.SetMode(AUTOMATIC); //速度还控制器初始化
lixianyu 0:a4d8f5b3c546 41 anglePID.SetMode(AUTOMATIC); //角度环控制器初始化
lixianyu 0:a4d8f5b3c546 42 speedPID.SetITermLimits(-10, 10);
lixianyu 0:a4d8f5b3c546 43 speedPID.SetOutputLimits(-MAX_TARGET_ANGLE, MAX_TARGET_ANGLE);
lixianyu 0:a4d8f5b3c546 44 anglePID.SetOutputLimits(-MAX_SPEED, MAX_SPEED);
lixianyu 0:a4d8f5b3c546 45 stepperL.begin(); //左电机初始化
lixianyu 0:a4d8f5b3c546 46 stepperR.begin(); //右电机初始化
lixianyu 0:a4d8f5b3c546 47 for (uint8_t k = 0; k < 3; k++) {
lixianyu 0:a4d8f5b3c546 48 stepperR.setSpeed(3);
lixianyu 0:a4d8f5b3c546 49 stepperL.setSpeed(-3);
lixianyu 0:a4d8f5b3c546 50 //delay(150);
lixianyu 0:a4d8f5b3c546 51 wait_ms(150);
lixianyu 0:a4d8f5b3c546 52 stepperR.setSpeed(-3);
lixianyu 0:a4d8f5b3c546 53 stepperL.setSpeed(3);
lixianyu 0:a4d8f5b3c546 54 //delay(150);
lixianyu 0:a4d8f5b3c546 55 wait_ms(150);
lixianyu 0:a4d8f5b3c546 56 }
lixianyu 0:a4d8f5b3c546 57 while(1) {
lixianyu 0:a4d8f5b3c546 58 if (protocolRead(channalData, mode)) { //判断是否接收到遥控信号
lixianyu 0:a4d8f5b3c546 59 throttle = map(channalData[CHANNEL_THROTTLE], 1000, 2000, MAX_THROTTLE, -MAX_THROTTLE);
lixianyu 0:a4d8f5b3c546 60 steering = map(channalData[CHANNEL_STEERING], 1000, 2000, -MAX_STEERING, MAX_STEERING);
lixianyu 0:a4d8f5b3c546 61 //safe_ms = millis();
lixianyu 0:a4d8f5b3c546 62 safe_ms = g_Timer.read_ms();
lixianyu 0:a4d8f5b3c546 63 }
lixianyu 0:a4d8f5b3c546 64
lixianyu 0:a4d8f5b3c546 65 if (safe_ms > g_Timer.read_ms()) safe_ms = g_Timer.read_ms();
lixianyu 0:a4d8f5b3c546 66 if (g_Timer.read_ms() - safe_ms > SAFE_TIME_OUT) {
lixianyu 0:a4d8f5b3c546 67 throttle = 0;
lixianyu 0:a4d8f5b3c546 68 steering = 0;
lixianyu 0:a4d8f5b3c546 69 }
lixianyu 0:a4d8f5b3c546 70
lixianyu 0:a4d8f5b3c546 71 #ifdef BAT_DETECT
lixianyu 0:a4d8f5b3c546 72 batteryValue = (analogRead(A7) / 1024.0) * BAT_DETECT; //采集电池电压
lixianyu 0:a4d8f5b3c546 73 #endif
lixianyu 0:a4d8f5b3c546 74 dmpGetYPR(ypr);
lixianyu 0:a4d8f5b3c546 75 float robotAngleCache = robotAngle;
lixianyu 0:a4d8f5b3c546 76 robotAngle = ypr[DIRECTION] + ANGLE_FIX; //取Roll轴角度
lixianyu 0:a4d8f5b3c546 77 float robotSpeed = (stepperL.getSpeed() - stepperR.getSpeed()) / 2.0; //计算小车速度
lixianyu 0:a4d8f5b3c546 78
lixianyu 0:a4d8f5b3c546 79 float angleVelocity = (robotAngle - robotAngleCache) * 90.0;
lixianyu 0:a4d8f5b3c546 80 robotSpeedFilter = robotSpeedFilter * 0.95 + (robotSpeed + angleVelocity) * 0.05; //小车速度滤波
lixianyu 0:a4d8f5b3c546 81 float targetAngle = speedPID.Compute(robotSpeedFilter, throttle); //速度环计算目标角度
lixianyu 0:a4d8f5b3c546 82 targetSpeed += anglePID.Compute(robotAngle, targetAngle); //角度环计算电机转速
lixianyu 0:a4d8f5b3c546 83 targetSpeed = constrain(targetSpeed, -MAX_SPEED, MAX_SPEED);
lixianyu 0:a4d8f5b3c546 84 int16_t motorR = targetSpeed + steering; //计算右电机转速
lixianyu 0:a4d8f5b3c546 85 int16_t motorL = -targetSpeed + steering; //计算左电机转速
lixianyu 0:a4d8f5b3c546 86
lixianyu 0:a4d8f5b3c546 87 if ((robotAngle < 65) && (robotAngle > -65)) {
lixianyu 0:a4d8f5b3c546 88 stepperAllEnable();
lixianyu 0:a4d8f5b3c546 89 stepperR.setSpeed(motorR); //更新右电机转速
lixianyu 0:a4d8f5b3c546 90 stepperL.setSpeed(motorL); //更新左电机转速
lixianyu 0:a4d8f5b3c546 91
lixianyu 0:a4d8f5b3c546 92 if ((robotAngle < 30) && (robotAngle > -30)) {
lixianyu 0:a4d8f5b3c546 93 anglePID.SetTunings(KP_ANG, KI_ANG, KD_ANG);
lixianyu 0:a4d8f5b3c546 94 speedPID.SetTunings(KP_SPD, KI_SPD, KD_SPD);
lixianyu 0:a4d8f5b3c546 95 } else {
lixianyu 0:a4d8f5b3c546 96 anglePID.SetTunings(KP_ANG_RAISUP, KI_ANG_RAISUP, KD_ANG_RAISUP);
lixianyu 0:a4d8f5b3c546 97 speedPID.SetTunings(KP_SPD_RAISUP, KI_SPD_RAISUP, KD_SPD_RAISUP);
lixianyu 0:a4d8f5b3c546 98 }
lixianyu 0:a4d8f5b3c546 99 } else {
lixianyu 0:a4d8f5b3c546 100 stepperAllDisable();
lixianyu 0:a4d8f5b3c546 101 stepperR.setSpeed(0); //更新右电机转速
lixianyu 0:a4d8f5b3c546 102 stepperL.setSpeed(0); //更新左电机转速
lixianyu 0:a4d8f5b3c546 103 anglePID.SetTunings(KP_ANG_RAISUP, KI_ANG_RAISUP, KD_ANG_RAISUP);
lixianyu 0:a4d8f5b3c546 104 speedPID.SetTunings(KP_SPD_RAISUP, KI_SPD_RAISUP, KD_SPD_RAISUP);
lixianyu 0:a4d8f5b3c546 105 }
lixianyu 0:a4d8f5b3c546 106 }
lixianyu 0:a4d8f5b3c546 107 }