Li Weiyi / Mbed 2 deprecated BalanceCar

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

Dependencies:   mbed

main.cpp@2:99785a1007a4, 2016-06-07 (annotated)

Committer:
lixianyu
Date:
Tue Jun 07 05:26:03 2016 +0000
Revision:
2:99785a1007a4
Parent:
0:a4d8f5b3c546 
Still not work......commit first.

Who changed what in which revision?

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