SomeTest.cpp

00001 #include "SomeTest.h"
00002 #include "mbed.h"
00003 
00004 extern DigitalOut myled;
00005 Ticker myTicker;
00006 Ticker myTicker1;
00007 Serial mpc(P0_4,P0_0);
00008 #if 0
00009 #include "RollPitch.h"
00010 
00011 extern MPU6050 mpu;
00012 extern Timer g_Timer;
00013 
00014 float ypr[3];             //Yaw,Pitch,Roll三个轴的角度值
00015 void testMPU6050(void)
00016 {
00017     mpc.baud(115200);
00018     bool ret = dmpSetup();
00019     if (ret == true) {
00020         mpc.printf("dmp ready\r\n");
00021     } else {
00022         mpc.printf("dmp not ready\r\n");
00023     }
00024     dmpGetYPR(ypr);
00025     mpc.printf("ypr[0] = %f, ypr[1] = %f, ypr[2] = %f\r\n", ypr[0], ypr[1], ypr[2]);
00026     while (1) {
00027         wait(0.5);
00028         dmpGetYPR(ypr);
00029         mpc.printf("ypr[0] = %f, ypr[1] = %f, ypr[2] = %f\r\n", ypr[0], ypr[1], ypr[2]);
00030 #if 0
00031         if (mpu.testConnection() == true)
00032             mpc.printf("connect ok\r\n");
00033 #endif
00034     }
00035 }
00036 #endif
00037 
00038 static void flip()
00039 {
00040     static uint32_t c = 0;
00041     c++;
00042     if ((c % 2000) == 0) {
00043         myled = !myled;
00044     }
00045 }
00046 void testTicker(void)
00047 {
00048     myTicker.attach_us(&flip, 25);
00049     while (1) {}
00050 }
00051 #if 0
00052 #define PIN_EN      4      //PORTB,0
00053 #define PIN_DIRA    A0    //PORTA,7
00054 #define PIN_STEPA   5   //PORTB,1
00055 #define PIN_DIRB    A1    //PORTA,6
00056 #define PIN_STEPB   6   //PORTB,2
00057 #define PIN_DIRC    A2    //PORTA,5
00058 #define PIN_STEPC   7   //PORTB,3
00059 #define PIN_DIRD    A3    //PORTA,4
00060 #define PIN_STEPD   8   //PORTD,6
00061 #else
00062 #define PIN_EN      P0_25
00063 #define PIN_DIRA    P0_7
00064 #define PIN_STEPA   P0_24
00065 
00066 #define PIN_DIRB    P0_6
00067 #define PIN_STEPB   P0_1
00068 
00069 #define PIN_DIRC    P0_14
00070 #define PIN_STEPC   P0_20
00071 #define PIN_DIRD    P0_23
00072 #define PIN_STEPD   P0_19
00073 #endif
00074 #if 0
00075 //Stepper stepperL(PIN_DIRB, PIN_STEPB);              //左电机，使用stepper底板A接口
00076 //Stepper stepperR(PIN_DIRA, PIN_STEPA);              //右电机，使用stepper底板D接口
00077 DigitalOut LdirB(PIN_DIRB);
00078 DigitalOut LstepB(PIN_STEPB);
00079 DigitalOut RdirB(PIN_DIRA);
00080 DigitalOut RstepB(PIN_STEPA);
00081 DigitalOut SteperE(PIN_EN);
00082 static void timeHandle_left(void)
00083 {
00084     //LstepB = !LstepB;
00085     LstepB = 1;
00086     wait_us(1);
00087     LstepB = 0;
00088 }
00089 
00090 static void timeHandle_right(void)
00091 {
00092     //LstepB = !LstepB;
00093     RstepB = 1;
00094     wait_us(1);
00095     RstepB = 0;
00096 }
00097 
00098 void testStepper(void)
00099 {
00100     SteperE = 1;
00101     LdirB = 0;
00102     RdirB = 0;
00103     LstepB = 0;
00104     RstepB = 0;
00105     myTicker.attach_us(&timeHandle_left, 510);
00106     myTicker1.attach_us(&timeHandle_right, 2500);
00107     SteperE = 0;
00108     while (1) {}
00109 }
00110 #endif
00111 
00112 #if 0
00113 DigitalOut SteperE(PIN_EN);
00114 DigitalOut Rdir(PIN_DIRA);
00115 PwmOut rstep(PIN_STEPA);//P0_24, D5
00116 #if 1
00117 DigitalOut Ldir(PIN_DIRB);
00118 PwmOut lstep(PIN_STEPB);//P0_1, D6
00119 #endif
00120 
00121 void testStepper1(void)
00122 {
00123     int widthL = 1;
00124     int widthR = 1;
00125     int period = 410; // 2500Hz
00126     myled = 0;
00127     SteperE = 0;
00128     
00129     Rdir = 1;
00130     rstep.period_us(period);
00131     rstep.pulsewidth_us(widthR);
00132     
00133     #if 1
00134     Ldir = 1;
00135     lstep.period_us(period);
00136     lstep.pulsewidth_us(widthL);
00137     #endif
00138     
00139     //wait(2.0);
00140     //SteperE = 0;
00141     while (1) {
00142         #if 0
00143         wait(0.1);
00144         widthR += 50;
00145         if (widthR >= 1500) {
00146             widthR = 0;
00147         }
00148         rstep.pulsewidth_us(widthR);
00149         
00150         widthL += 50;
00151         if (widthL >= 500) {
00152             widthL = 0;
00153         }
00154         lstep.pulsewidth_us(widthL);
00155         #else
00156         period += 20;
00157         lstep.period_us(period);
00158         rstep.period_us(period);
00159         //myled = !myled;
00160         wait(0.5);
00161         #endif
00162     }
00163 }
00164 #endif