Diff: main.cpp

Revision:

2:99785a1007a4

Parent:

0:a4d8f5b3c546

diff -r 620da20b810b -r 99785a1007a4 main.cpp
--- 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