Li Weiyi
/
Cube
Microduino的cube小车。
main.cpp@4:0670023d3f36, 2016-05-28 (annotated)
- Committer:
- lixianyu
- Date:
- Sat May 28 05:09:18 2016 +0000
- Revision:
- 4:0670023d3f36
- Parent:
- 3:e4ac7c1a14de
??work????
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
lixianyu | 0:403ac413a86d | 1 | #include "mbed.h" |
lixianyu | 4:0670023d3f36 | 2 | //#include "rtos.h" |
lixianyu | 0:403ac413a86d | 3 | #include "MicroduinoPinNames.h" |
lixianyu | 3:e4ac7c1a14de | 4 | #include "userDef.h" |
lixianyu | 3:e4ac7c1a14de | 5 | #include "Protocol.h" |
lixianyu | 3:e4ac7c1a14de | 6 | #include "Microduino_Motor.h" |
lixianyu | 3:e4ac7c1a14de | 7 | |
lixianyu | 4:0670023d3f36 | 8 | Timer g_cubeTimer; |
lixianyu | 4:0670023d3f36 | 9 | #if 1 |
lixianyu | 3:e4ac7c1a14de | 10 | Motor MotorLeft(motor_pin0A, motor_pin0B); |
lixianyu | 3:e4ac7c1a14de | 11 | Motor MotorRight(motor_pin1A, motor_pin1B); |
lixianyu | 3:e4ac7c1a14de | 12 | /////////////////////////////////////////////////////////// |
lixianyu | 3:e4ac7c1a14de | 13 | #define CHANNEL_NUM 8 |
lixianyu | 3:e4ac7c1a14de | 14 | uint16_t channalData[CHANNEL_NUM]; //8通道数据 |
lixianyu | 3:e4ac7c1a14de | 15 | bool mode = 0; //nrf或者ble模式 |
lixianyu | 3:e4ac7c1a14de | 16 | int16_t throttle = 0; //油门 |
lixianyu | 3:e4ac7c1a14de | 17 | int16_t steering = 0; //转向 |
lixianyu | 4:0670023d3f36 | 18 | int safe_ms = 0; |
lixianyu | 0:403ac413a86d | 19 | |
lixianyu | 0:403ac413a86d | 20 | //Serial pc(P0_4,P0_0); |
lixianyu | 3:e4ac7c1a14de | 21 | |
lixianyu | 3:e4ac7c1a14de | 22 | static long map(long x, long in_min, long in_max, long out_min, long out_max) |
lixianyu | 3:e4ac7c1a14de | 23 | { |
lixianyu | 3:e4ac7c1a14de | 24 | return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; |
lixianyu | 3:e4ac7c1a14de | 25 | } |
lixianyu | 3:e4ac7c1a14de | 26 | |
lixianyu | 0:403ac413a86d | 27 | int main() |
lixianyu | 0:403ac413a86d | 28 | { |
lixianyu | 4:0670023d3f36 | 29 | bool freeFlag = true; |
lixianyu | 3:e4ac7c1a14de | 30 | g_cubeTimer.start(); |
lixianyu | 3:e4ac7c1a14de | 31 | mode = protocolSetup(); //遥控接收器初始化 |
lixianyu | 3:e4ac7c1a14de | 32 | |
lixianyu | 3:e4ac7c1a14de | 33 | MotorLeft.Fix(motor_fixL); |
lixianyu | 3:e4ac7c1a14de | 34 | MotorRight.Fix(motor_fixR); |
lixianyu | 0:403ac413a86d | 35 | //pc.baud(115200); |
lixianyu | 0:403ac413a86d | 36 | while(1) { |
lixianyu | 3:e4ac7c1a14de | 37 | if (protocolRead(channalData, mode)) { //判断是否接收到遥控信号 |
lixianyu | 3:e4ac7c1a14de | 38 | throttle = map(channalData[CHANNEL_THROTTLE], 1000, 2000, -MAX_THROTTLE, MAX_THROTTLE); |
lixianyu | 3:e4ac7c1a14de | 39 | steering = map(channalData[CHANNEL_STEERING], 1000, 2000, -MAX_STEERING, MAX_STEERING); |
lixianyu | 3:e4ac7c1a14de | 40 | |
lixianyu | 3:e4ac7c1a14de | 41 | MotorLeft.Driver(MotorLeft.GetData(throttle, steering, CHAN_LEFT)); |
lixianyu | 3:e4ac7c1a14de | 42 | MotorRight.Driver(MotorRight.GetData(throttle, steering, CHAN_RIGHT)); |
lixianyu | 3:e4ac7c1a14de | 43 | |
lixianyu | 3:e4ac7c1a14de | 44 | #ifdef _DEBUG |
lixianyu | 4:0670023d3f36 | 45 | asdf |
lixianyu | 3:e4ac7c1a14de | 46 | Serial.print("DATA OK :["); |
lixianyu | 3:e4ac7c1a14de | 47 | for (int a = 0; a < CHANNEL_NUM; a++) { |
lixianyu | 3:e4ac7c1a14de | 48 | Serial.print(channalData[a]); |
lixianyu | 3:e4ac7c1a14de | 49 | Serial.print(" "); |
lixianyu | 3:e4ac7c1a14de | 50 | } |
lixianyu | 3:e4ac7c1a14de | 51 | Serial.print("],throttle:"); |
lixianyu | 3:e4ac7c1a14de | 52 | Serial.print(throttle); |
lixianyu | 3:e4ac7c1a14de | 53 | Serial.print(",steering:"); |
lixianyu | 3:e4ac7c1a14de | 54 | Serial.println(steering); |
lixianyu | 3:e4ac7c1a14de | 55 | #endif |
lixianyu | 3:e4ac7c1a14de | 56 | safe_ms = g_cubeTimer.read_ms(); |
lixianyu | 3:e4ac7c1a14de | 57 | } |
lixianyu | 4:0670023d3f36 | 58 | #if 0 |
lixianyu | 3:e4ac7c1a14de | 59 | if (safe_ms > g_cubeTimer.read_ms()) { |
lixianyu | 3:e4ac7c1a14de | 60 | safe_ms = g_cubeTimer.read_ms(); |
lixianyu | 3:e4ac7c1a14de | 61 | } |
lixianyu | 3:e4ac7c1a14de | 62 | if (g_cubeTimer.read_ms() - safe_ms > SAFE_TIME_OUT) { |
lixianyu | 3:e4ac7c1a14de | 63 | MotorLeft.Free(); |
lixianyu | 3:e4ac7c1a14de | 64 | MotorRight.Free(); |
lixianyu | 3:e4ac7c1a14de | 65 | //MotorLeft.Brake(); |
lixianyu | 3:e4ac7c1a14de | 66 | //MotorRight.Brake(); |
lixianyu | 3:e4ac7c1a14de | 67 | } |
lixianyu | 4:0670023d3f36 | 68 | #endif |
lixianyu | 0:403ac413a86d | 69 | //pc.printf("Hello world\r\n"); |
lixianyu | 0:403ac413a86d | 70 | //sleep(); |
lixianyu | 0:403ac413a86d | 71 | } |
lixianyu | 0:403ac413a86d | 72 | } |
lixianyu | 4:0670023d3f36 | 73 | #else |
lixianyu | 4:0670023d3f36 | 74 | DigitalOut myled(D13); |
lixianyu | 4:0670023d3f36 | 75 | PwmOut PWM_A(motor_pin0A); |
lixianyu | 4:0670023d3f36 | 76 | PwmOut PWM_B(motor_pin0B); |
lixianyu | 4:0670023d3f36 | 77 | int pv = 0; |
lixianyu | 4:0670023d3f36 | 78 | int main() |
lixianyu | 4:0670023d3f36 | 79 | { |
lixianyu | 4:0670023d3f36 | 80 | myled.write(1); |
lixianyu | 4:0670023d3f36 | 81 | PWM_A.period_us(255); |
lixianyu | 4:0670023d3f36 | 82 | PWM_B.period_us(255); |
lixianyu | 4:0670023d3f36 | 83 | |
lixianyu | 4:0670023d3f36 | 84 | PWM_A.pulsewidth_us(200); |
lixianyu | 4:0670023d3f36 | 85 | PWM_B.pulsewidth_us(0); |
lixianyu | 4:0670023d3f36 | 86 | while (1) |
lixianyu | 4:0670023d3f36 | 87 | { |
lixianyu | 4:0670023d3f36 | 88 | #if 0 |
lixianyu | 4:0670023d3f36 | 89 | PWM_B.pulsewidth_us(pv); |
lixianyu | 4:0670023d3f36 | 90 | pv++; |
lixianyu | 4:0670023d3f36 | 91 | #endif |
lixianyu | 4:0670023d3f36 | 92 | wait(0.5); |
lixianyu | 4:0670023d3f36 | 93 | myled = !myled; |
lixianyu | 4:0670023d3f36 | 94 | if (pv >= 255) |
lixianyu | 4:0670023d3f36 | 95 | { |
lixianyu | 4:0670023d3f36 | 96 | pv = 0; |
lixianyu | 4:0670023d3f36 | 97 | } |
lixianyu | 4:0670023d3f36 | 98 | } |
lixianyu | 4:0670023d3f36 | 99 | } |
lixianyu | 4:0670023d3f36 | 100 | #endif |