Li Weiyi
/
Cube
Microduino的cube小车。
Diff: main.cpp
- Revision:
- 3:e4ac7c1a14de
- Parent:
- 1:758ccab13947
- Child:
- 4:0670023d3f36
diff -r 70ca3e685cca -r e4ac7c1a14de main.cpp --- a/main.cpp Wed May 25 13:25:09 2016 +0000 +++ b/main.cpp Fri May 27 01:44:31 2016 +0000 @@ -1,18 +1,68 @@ #include "mbed.h" #include "rtos.h" #include "MicroduinoPinNames.h" +#include "userDef.h" +#include "Protocol.h" +#include "Microduino_Motor.h" + +Motor MotorLeft(motor_pin0A, motor_pin0B); +Motor MotorRight(motor_pin1A, motor_pin1B); +/////////////////////////////////////////////////////////// +#define CHANNEL_NUM 8 +uint16_t channalData[CHANNEL_NUM]; //8通道数据 +bool mode = 0; //nrf或者ble模式 +int16_t throttle = 0; //油门 +int16_t steering = 0; //转向 +unsigned long safe_ms; DigitalOut myled(D11); +Timer g_cubeTimer; //Serial pc(P0_4,P0_0); + +static 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; +} + int main() { - uint8_t a = 6; + g_cubeTimer.start(); + mode = protocolSetup(); //遥控接收器初始化 + + MotorLeft.Fix(motor_fixL); + MotorRight.Fix(motor_fixR); //pc.baud(115200); while(1) { - myled = 1; - wait(0.2); - myled = 0; - wait(0.8); + if (protocolRead(channalData, mode)) { //判断是否接收到遥控信号 + throttle = map(channalData[CHANNEL_THROTTLE], 1000, 2000, -MAX_THROTTLE, MAX_THROTTLE); + steering = map(channalData[CHANNEL_STEERING], 1000, 2000, -MAX_STEERING, MAX_STEERING); + + MotorLeft.Driver(MotorLeft.GetData(throttle, steering, CHAN_LEFT)); + MotorRight.Driver(MotorRight.GetData(throttle, steering, CHAN_RIGHT)); + +#ifdef _DEBUG + Serial.print("DATA OK :["); + for (int a = 0; a < CHANNEL_NUM; a++) { + Serial.print(channalData[a]); + Serial.print(" "); + } + Serial.print("],throttle:"); + Serial.print(throttle); + Serial.print(",steering:"); + Serial.println(steering); +#endif + safe_ms = g_cubeTimer.read_ms(); + } + + if (safe_ms > g_cubeTimer.read_ms()) { + safe_ms = g_cubeTimer.read_ms(); + } + if (g_cubeTimer.read_ms() - safe_ms > SAFE_TIME_OUT) { + MotorLeft.Free(); + MotorRight.Free(); + //MotorLeft.Brake(); + //MotorRight.Brake(); + } //pc.printf("Hello world\r\n"); //sleep(); }