Li Weiyi
/
BalanceCar
虽然移植完毕,但是不work。需要细调……
Diff: main.cpp
- Revision:
- 2:99785a1007a4
- Parent:
- 0:a4d8f5b3c546
--- a/main.cpp Sat Jun 04 04:06:36 2016 +0000 +++ b/main.cpp Tue Jun 07 05:26:03 2016 +0000 @@ -1,11 +1,16 @@ #include "mbed.h" + +//#define BALANCE_CAR_TEST + +#ifndef BALANCE_CAR_TEST #include "PID_v2.h" //PID控制库 -#include "Microduino_Stepper.h" //步进电机库 +#include "Microduino_Stepper_PWM.h" //步进电机库 #include "userDef.h" //用户自定义 #include "RollPitch.h" #include "Protocol.h" //通讯协议库 +#include "MicroduinoPinNames.h" -MPU6050 imu; +//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); //速度环控制器 @@ -13,30 +18,48 @@ uint16_t channalData[CHANNEL_NUM]; //8通道数据 bool mode = 0; //nrf或者ble模式 -int16_t throttle; //油门值 -int16_t steering; //转向值 +int16_t throttle = 0; //油门值 +int16_t steering = 0; //转向值 unsigned long safe_ms = 0; -float robotSpeedFilter; //速度估计值 -float robotAngle; -float targetSpeed; //目标速度 +float robotSpeedFilter = 0; //速度估计值 +float robotAngle = 0; +float targetSpeed = 0; //目标速度 #ifdef BAT_DETECT float batteryValue; //电池电压 #endif float ypr[3]; //Yaw,Pitch,Roll三个轴的角度值 +#endif //BALANCE_CAR_TEST +DigitalOut myled(P0_13); -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) + +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; } + +#ifdef BALANCE_CAR_TEST +#include "SomeTest.h" int main() { g_Timer.start(); + //testMPU6050(); + //testTicker(); + testStepper1(); + while (1) { + + } +} +#else +extern Serial mpc; +int main() +{ + //mpc.baud(115200); + g_Timer.start(); dmpSetup(); - mode = protocolSetup(); //遥控接收器初始化 + //mode = protocolSetup(); //遥控接收器初始化 speedPID.SetMode(AUTOMATIC); //速度还控制器初始化 anglePID.SetMode(AUTOMATIC); //角度环控制器初始化 speedPID.SetITermLimits(-10, 10); @@ -45,22 +68,27 @@ stepperL.begin(); //左电机初始化 stepperR.begin(); //右电机初始化 for (uint8_t k = 0; k < 3; k++) { - stepperR.setSpeed(3); - stepperL.setSpeed(-3); + stepperR.setSpeed(4); + stepperL.setSpeed(-4); //delay(150); wait_ms(150); - stepperR.setSpeed(-3); - stepperL.setSpeed(3); + stepperR.setSpeed(-4); + stepperL.setSpeed(4); //delay(150); wait_ms(150); } + //myled = 1; while(1) { + //myled = !myled; + //wait(0.1); + #if 0 if (protocolRead(channalData, mode)) { //判断是否接收到遥控信号 - throttle = map(channalData[CHANNEL_THROTTLE], 1000, 2000, MAX_THROTTLE, -MAX_THROTTLE); + 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(); } + #endif if (safe_ms > g_Timer.read_ms()) safe_ms = g_Timer.read_ms(); if (g_Timer.read_ms() - safe_ms > SAFE_TIME_OUT) { @@ -84,6 +112,7 @@ int16_t motorR = targetSpeed + steering; //计算右电机转速 int16_t motorL = -targetSpeed + steering; //计算左电机转速 + //mpc.printf("robotAngle: %f\r\n", robotAngle); if ((robotAngle < 65) && (robotAngle > -65)) { stepperAllEnable(); stepperR.setSpeed(motorR); //更新右电机转速 @@ -105,3 +134,4 @@ } } } +#endif