Li Weiyi
/
BalanceCar
虽然移植完毕,但是不work。需要细调……
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?
User | Revision | Line number | New 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 |