Li Weiyi / Mbed 2 deprecated BalanceCar

虽然移植完毕，但是不work。需要细调……

Dependencies:   mbed

SomeTest.cpp@2:99785a1007a4, 2016-06-07 (annotated)

Committer:
lixianyu
Date:
Tue Jun 07 05:26:03 2016 +0000
Revision:
2:99785a1007a4
Still not work......commit first.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lixianyu 2:99785a1007a4 1 #include "SomeTest.h"
lixianyu 2:99785a1007a4 2 #include "mbed.h"
lixianyu 2:99785a1007a4 3
lixianyu 2:99785a1007a4 4 extern DigitalOut myled;
lixianyu 2:99785a1007a4 5 Ticker myTicker;
lixianyu 2:99785a1007a4 6 Ticker myTicker1;
lixianyu 2:99785a1007a4 7 Serial mpc(P0_4,P0_0);
lixianyu 2:99785a1007a4 8 #if 0
lixianyu 2:99785a1007a4 9 #include "RollPitch.h"
lixianyu 2:99785a1007a4 10
lixianyu 2:99785a1007a4 11 extern MPU6050 mpu;
lixianyu 2:99785a1007a4 12 extern Timer g_Timer;
lixianyu 2:99785a1007a4 13
lixianyu 2:99785a1007a4 14 float ypr[3]; //Yaw,Pitch,Roll三个轴的角度值
lixianyu 2:99785a1007a4 15 void testMPU6050(void)
lixianyu 2:99785a1007a4 16 {
lixianyu 2:99785a1007a4 17 mpc.baud(115200);
lixianyu 2:99785a1007a4 18 bool ret = dmpSetup();
lixianyu 2:99785a1007a4 19 if (ret == true) {
lixianyu 2:99785a1007a4 20 mpc.printf("dmp ready\r\n");
lixianyu 2:99785a1007a4 21 } else {
lixianyu 2:99785a1007a4 22 mpc.printf("dmp not ready\r\n");
lixianyu 2:99785a1007a4 23 }
lixianyu 2:99785a1007a4 24 dmpGetYPR(ypr);
lixianyu 2:99785a1007a4 25 mpc.printf("ypr[0] = %f, ypr[1] = %f, ypr[2] = %f\r\n", ypr[0], ypr[1], ypr[2]);
lixianyu 2:99785a1007a4 26 while (1) {
lixianyu 2:99785a1007a4 27 wait(0.5);
lixianyu 2:99785a1007a4 28 dmpGetYPR(ypr);
lixianyu 2:99785a1007a4 29 mpc.printf("ypr[0] = %f, ypr[1] = %f, ypr[2] = %f\r\n", ypr[0], ypr[1], ypr[2]);
lixianyu 2:99785a1007a4 30 #if 0
lixianyu 2:99785a1007a4 31 if (mpu.testConnection() == true)
lixianyu 2:99785a1007a4 32 mpc.printf("connect ok\r\n");
lixianyu 2:99785a1007a4 33 #endif
lixianyu 2:99785a1007a4 34 }
lixianyu 2:99785a1007a4 35 }
lixianyu 2:99785a1007a4 36 #endif
lixianyu 2:99785a1007a4 37
lixianyu 2:99785a1007a4 38 static void flip()
lixianyu 2:99785a1007a4 39 {
lixianyu 2:99785a1007a4 40 static uint32_t c = 0;
lixianyu 2:99785a1007a4 41 c++;
lixianyu 2:99785a1007a4 42 if ((c % 2000) == 0) {
lixianyu 2:99785a1007a4 43 myled = !myled;
lixianyu 2:99785a1007a4 44 }
lixianyu 2:99785a1007a4 45 }
lixianyu 2:99785a1007a4 46 void testTicker(void)
lixianyu 2:99785a1007a4 47 {
lixianyu 2:99785a1007a4 48 myTicker.attach_us(&flip, 25);
lixianyu 2:99785a1007a4 49 while (1) {}
lixianyu 2:99785a1007a4 50 }
lixianyu 2:99785a1007a4 51 #if 0
lixianyu 2:99785a1007a4 52 #define PIN_EN 4 //PORTB,0
lixianyu 2:99785a1007a4 53 #define PIN_DIRA A0 //PORTA,7
lixianyu 2:99785a1007a4 54 #define PIN_STEPA 5 //PORTB,1
lixianyu 2:99785a1007a4 55 #define PIN_DIRB A1 //PORTA,6
lixianyu 2:99785a1007a4 56 #define PIN_STEPB 6 //PORTB,2
lixianyu 2:99785a1007a4 57 #define PIN_DIRC A2 //PORTA,5
lixianyu 2:99785a1007a4 58 #define PIN_STEPC 7 //PORTB,3
lixianyu 2:99785a1007a4 59 #define PIN_DIRD A3 //PORTA,4
lixianyu 2:99785a1007a4 60 #define PIN_STEPD 8 //PORTD,6
lixianyu 2:99785a1007a4 61 #else
lixianyu 2:99785a1007a4 62 #define PIN_EN P0_25
lixianyu 2:99785a1007a4 63 #define PIN_DIRA P0_7
lixianyu 2:99785a1007a4 64 #define PIN_STEPA P0_24
lixianyu 2:99785a1007a4 65
lixianyu 2:99785a1007a4 66 #define PIN_DIRB P0_6
lixianyu 2:99785a1007a4 67 #define PIN_STEPB P0_1
lixianyu 2:99785a1007a4 68
lixianyu 2:99785a1007a4 69 #define PIN_DIRC P0_14
lixianyu 2:99785a1007a4 70 #define PIN_STEPC P0_20
lixianyu 2:99785a1007a4 71 #define PIN_DIRD P0_23
lixianyu 2:99785a1007a4 72 #define PIN_STEPD P0_19
lixianyu 2:99785a1007a4 73 #endif
lixianyu 2:99785a1007a4 74 #if 0
lixianyu 2:99785a1007a4 75 //Stepper stepperL(PIN_DIRB, PIN_STEPB); //左电机，使用stepper底板A接口
lixianyu 2:99785a1007a4 76 //Stepper stepperR(PIN_DIRA, PIN_STEPA); //右电机，使用stepper底板D接口
lixianyu 2:99785a1007a4 77 DigitalOut LdirB(PIN_DIRB);
lixianyu 2:99785a1007a4 78 DigitalOut LstepB(PIN_STEPB);
lixianyu 2:99785a1007a4 79 DigitalOut RdirB(PIN_DIRA);
lixianyu 2:99785a1007a4 80 DigitalOut RstepB(PIN_STEPA);
lixianyu 2:99785a1007a4 81 DigitalOut SteperE(PIN_EN);
lixianyu 2:99785a1007a4 82 static void timeHandle_left(void)
lixianyu 2:99785a1007a4 83 {
lixianyu 2:99785a1007a4 84 //LstepB = !LstepB;
lixianyu 2:99785a1007a4 85 LstepB = 1;
lixianyu 2:99785a1007a4 86 wait_us(1);
lixianyu 2:99785a1007a4 87 LstepB = 0;
lixianyu 2:99785a1007a4 88 }
lixianyu 2:99785a1007a4 89
lixianyu 2:99785a1007a4 90 static void timeHandle_right(void)
lixianyu 2:99785a1007a4 91 {
lixianyu 2:99785a1007a4 92 //LstepB = !LstepB;
lixianyu 2:99785a1007a4 93 RstepB = 1;
lixianyu 2:99785a1007a4 94 wait_us(1);
lixianyu 2:99785a1007a4 95 RstepB = 0;
lixianyu 2:99785a1007a4 96 }
lixianyu 2:99785a1007a4 97
lixianyu 2:99785a1007a4 98 void testStepper(void)
lixianyu 2:99785a1007a4 99 {
lixianyu 2:99785a1007a4 100 SteperE = 1;
lixianyu 2:99785a1007a4 101 LdirB = 0;
lixianyu 2:99785a1007a4 102 RdirB = 0;
lixianyu 2:99785a1007a4 103 LstepB = 0;
lixianyu 2:99785a1007a4 104 RstepB = 0;
lixianyu 2:99785a1007a4 105 myTicker.attach_us(&timeHandle_left, 510);
lixianyu 2:99785a1007a4 106 myTicker1.attach_us(&timeHandle_right, 2500);
lixianyu 2:99785a1007a4 107 SteperE = 0;
lixianyu 2:99785a1007a4 108 while (1) {}
lixianyu 2:99785a1007a4 109 }
lixianyu 2:99785a1007a4 110 #endif
lixianyu 2:99785a1007a4 111
lixianyu 2:99785a1007a4 112 #if 0
lixianyu 2:99785a1007a4 113 DigitalOut SteperE(PIN_EN);
lixianyu 2:99785a1007a4 114 DigitalOut Rdir(PIN_DIRA);
lixianyu 2:99785a1007a4 115 PwmOut rstep(PIN_STEPA);//P0_24, D5
lixianyu 2:99785a1007a4 116 #if 1
lixianyu 2:99785a1007a4 117 DigitalOut Ldir(PIN_DIRB);
lixianyu 2:99785a1007a4 118 PwmOut lstep(PIN_STEPB);//P0_1, D6
lixianyu 2:99785a1007a4 119 #endif
lixianyu 2:99785a1007a4 120
lixianyu 2:99785a1007a4 121 void testStepper1(void)
lixianyu 2:99785a1007a4 122 {
lixianyu 2:99785a1007a4 123 int widthL = 1;
lixianyu 2:99785a1007a4 124 int widthR = 1;
lixianyu 2:99785a1007a4 125 int period = 410; // 2500Hz
lixianyu 2:99785a1007a4 126 myled = 0;
lixianyu 2:99785a1007a4 127 SteperE = 0;
lixianyu 2:99785a1007a4 128
lixianyu 2:99785a1007a4 129 Rdir = 1;
lixianyu 2:99785a1007a4 130 rstep.period_us(period);
lixianyu 2:99785a1007a4 131 rstep.pulsewidth_us(widthR);
lixianyu 2:99785a1007a4 132
lixianyu 2:99785a1007a4 133 #if 1
lixianyu 2:99785a1007a4 134 Ldir = 1;
lixianyu 2:99785a1007a4 135 lstep.period_us(period);
lixianyu 2:99785a1007a4 136 lstep.pulsewidth_us(widthL);
lixianyu 2:99785a1007a4 137 #endif
lixianyu 2:99785a1007a4 138
lixianyu 2:99785a1007a4 139 //wait(2.0);
lixianyu 2:99785a1007a4 140 //SteperE = 0;
lixianyu 2:99785a1007a4 141 while (1) {
lixianyu 2:99785a1007a4 142 #if 0
lixianyu 2:99785a1007a4 143 wait(0.1);
lixianyu 2:99785a1007a4 144 widthR += 50;
lixianyu 2:99785a1007a4 145 if (widthR >= 1500) {
lixianyu 2:99785a1007a4 146 widthR = 0;
lixianyu 2:99785a1007a4 147 }
lixianyu 2:99785a1007a4 148 rstep.pulsewidth_us(widthR);
lixianyu 2:99785a1007a4 149
lixianyu 2:99785a1007a4 150 widthL += 50;
lixianyu 2:99785a1007a4 151 if (widthL >= 500) {
lixianyu 2:99785a1007a4 152 widthL = 0;
lixianyu 2:99785a1007a4 153 }
lixianyu 2:99785a1007a4 154 lstep.pulsewidth_us(widthL);
lixianyu 2:99785a1007a4 155 #else
lixianyu 2:99785a1007a4 156 period += 20;
lixianyu 2:99785a1007a4 157 lstep.period_us(period);
lixianyu 2:99785a1007a4 158 rstep.period_us(period);
lixianyu 2:99785a1007a4 159 //myled = !myled;
lixianyu 2:99785a1007a4 160 wait(0.5);
lixianyu 2:99785a1007a4 161 #endif
lixianyu 2:99785a1007a4 162 }
lixianyu 2:99785a1007a4 163 }
lixianyu 2:99785a1007a4 164 #endif