main.cpp

00001 #include "mbed.h"
00002 
00003 //#define BALANCE_CAR_TEST
00004 
00005 #ifndef BALANCE_CAR_TEST
00006 #include "PID_v2.h"         //PID控制库
00007 #include "Microduino_Stepper_PWM.h"  //步进电机库
00008 #include "userDef.h"        //用户自定义
00009 #include "RollPitch.h"
00010 #include "Protocol.h"       //通讯协议库
00011 #include "MicroduinoPinNames.h"
00012 
00013 //MPU6050 imu;
00014 Stepper stepperL(PIN_DIRB, PIN_STEPB);              //左电机，使用stepper底板A接口
00015 Stepper stepperR(PIN_DIRA, PIN_STEPA);              //右电机，使用stepper底板D接口
00016 PID speedPID((double)KP_SPD, (double)KI_SPD, (double)KD_SPD, DIRECT);     //速度环控制器
00017 PID anglePID((double)KP_ANG, (double)KI_ANG, (double)KD_ANG, DIRECT);     //角度环控制器
00018 
00019 uint16_t channalData[CHANNEL_NUM]; //8通道数据
00020 bool mode = 0; //nrf或者ble模式
00021 int16_t throttle = 0;         //油门值
00022 int16_t steering = 0;         //转向值
00023 unsigned long safe_ms = 0;
00024 
00025 float robotSpeedFilter = 0;   //速度估计值
00026 float robotAngle = 0;
00027 float targetSpeed = 0;        //目标速度
00028 #ifdef BAT_DETECT
00029 float batteryValue;       //电池电压
00030 #endif
00031 float ypr[3];             //Yaw,Pitch,Roll三个轴的角度值
00032 #endif //BALANCE_CAR_TEST
00033 DigitalOut myled(P0_13);
00034 
00035 #define constrain(x,a,b) ((x) < (a) ? (a) : ((x) > (b) ? (b) : (x)))
00036 Timer g_Timer;
00037 
00038 long map(long x, long in_min, long in_max, long out_min, long out_max)
00039 {
00040     return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
00041 }
00042 
00043 #ifdef BALANCE_CAR_TEST
00044 #include "SomeTest.h"
00045 int main()
00046 {
00047     g_Timer.start();
00048     //testMPU6050();
00049     //testTicker();
00050     testStepper1();
00051     while (1) {
00052         
00053     }
00054 }
00055 #else
00056 extern Serial mpc;
00057 int main()
00058 {
00059     //mpc.baud(115200);
00060     g_Timer.start();
00061     dmpSetup();
00062     //mode = protocolSetup();  //遥控接收器初始化
00063     speedPID.SetMode(AUTOMATIC);    //速度还控制器初始化
00064     anglePID.SetMode(AUTOMATIC);    //角度环控制器初始化
00065     speedPID.SetITermLimits(-10, 10);
00066     speedPID.SetOutputLimits(-MAX_TARGET_ANGLE, MAX_TARGET_ANGLE);
00067     anglePID.SetOutputLimits(-MAX_SPEED, MAX_SPEED);
00068     stepperL.begin();       //左电机初始化
00069     stepperR.begin();       //右电机初始化
00070     for (uint8_t k = 0; k < 3; k++) {
00071         stepperR.setSpeed(4);
00072         stepperL.setSpeed(-4);
00073         //delay(150);
00074         wait_ms(150);
00075         stepperR.setSpeed(-4);
00076         stepperL.setSpeed(4);
00077         //delay(150);
00078         wait_ms(150);
00079     }
00080     //myled = 1;
00081     while(1) {
00082         //myled = !myled;
00083         //wait(0.1);
00084         #if 0
00085         if (protocolRead(channalData, mode)) { //判断是否接收到遥控信号
00086             throttle = map(channalData[CHANNEL_THROTTLE], 1000, 2000, -MAX_THROTTLE, MAX_THROTTLE);
00087             steering = map(channalData[CHANNEL_STEERING], 1000, 2000, -MAX_STEERING, MAX_STEERING);
00088             //safe_ms = millis();
00089             safe_ms = g_Timer.read_ms();
00090         }
00091         #endif
00092 
00093         if (safe_ms > g_Timer.read_ms()) safe_ms = g_Timer.read_ms();
00094         if (g_Timer.read_ms() - safe_ms > SAFE_TIME_OUT) {
00095             throttle = 0;
00096             steering = 0;
00097         }
00098 
00099 #ifdef BAT_DETECT
00100         batteryValue = (analogRead(A7) / 1024.0) * BAT_DETECT;   //采集电池电压
00101 #endif
00102         dmpGetYPR(ypr);
00103         float robotAngleCache = robotAngle;
00104         robotAngle = ypr[DIRECTION] + ANGLE_FIX;          //取Roll轴角度
00105         float robotSpeed = (stepperL.getSpeed() - stepperR.getSpeed()) / 2.0;   //计算小车速度
00106 
00107         float angleVelocity = (robotAngle - robotAngleCache) * 90.0;
00108         robotSpeedFilter = robotSpeedFilter * 0.95 + (robotSpeed + angleVelocity) * 0.05;       //小车速度滤波
00109         float targetAngle = speedPID.Compute(robotSpeedFilter, throttle);       //速度环计算目标角度
00110         targetSpeed += anglePID.Compute(robotAngle, targetAngle);         //角度环计算电机转速
00111         targetSpeed = constrain(targetSpeed, -MAX_SPEED, MAX_SPEED);
00112         int16_t motorR = targetSpeed + steering;    //计算右电机转速
00113         int16_t motorL = -targetSpeed + steering;   //计算左电机转速
00114 
00115         //mpc.printf("robotAngle: %f\r\n", robotAngle);
00116         if ((robotAngle < 65) && (robotAngle > -65)) {
00117             stepperAllEnable();
00118             stepperR.setSpeed(motorR);        //更新右电机转速
00119             stepperL.setSpeed(motorL);        //更新左电机转速
00120 
00121             if ((robotAngle < 30) && (robotAngle > -30)) {
00122                 anglePID.SetTunings(KP_ANG, KI_ANG, KD_ANG);
00123                 speedPID.SetTunings(KP_SPD, KI_SPD, KD_SPD);
00124             } else {
00125                 anglePID.SetTunings(KP_ANG_RAISUP, KI_ANG_RAISUP, KD_ANG_RAISUP);
00126                 speedPID.SetTunings(KP_SPD_RAISUP, KI_SPD_RAISUP, KD_SPD_RAISUP);
00127             }
00128         } else {
00129             stepperAllDisable();
00130             stepperR.setSpeed(0);        //更新右电机转速
00131             stepperL.setSpeed(0);        //更新左电机转速
00132             anglePID.SetTunings(KP_ANG_RAISUP, KI_ANG_RAISUP, KD_ANG_RAISUP);
00133             speedPID.SetTunings(KP_SPD_RAISUP, KI_SPD_RAISUP, KD_SPD_RAISUP);
00134         }
00135     }
00136 }
00137 #endif