Revision 0:4ca3e247b86a, committed 2018-06-08

Comitter:

Charlie_He

Date:

Fri Jun 08 12:29:16 2018 +0000

Child:

1:60c79e942c98

Commit message:

TDPS;

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Distance.h	Fri Jun 08 12:29:16 2018 +0000
@@ -0,0 +1,46 @@
+#include "mbed.h"
+#include "HCSR04.h"
+#include "SRF05.h"
+#include "gyro.h"
+SRF05 Usound1(p26,p15);
+HCSR04 Usound2(p25,p16);
+Ticker t1;
+int n=0;
+void distancedetectL()
+{
+    float u= Usound1.read();
+    if(7<=u<=20)  {
+        n+=1;
+        if (n>=3)
+        {
+            //turnL();
+            change(90);
+            //driveit();
+            n=0;
+            }
+        }
+    }
+void distancedetectR()
+{
+    float u= Usound2.read_cm();
+    if(7<=u<=20)  {
+        n+=1;
+        if (n>=3)
+        {
+            //turnL();
+            change(90);
+            //driveit();
+            n=0;
+            }
+        }
+    }
+void enable1()
+{
+   t1.detach();
+   t1.attach(&distancedetectR,0.4);
+    }  
+void enable2()
+{
+   t1.detach();
+   t1.attach(&distancedetectL,0.4);
+    }    
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HCSR04.lib	Fri Jun 08 12:29:16 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/grantphillips/code/HCSR04/#8286d0de19ce

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SRF05.lib	Fri Jun 08 12:29:16 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/simon/code/SRF05/#e758665e072c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/gyro.cpp	Fri Jun 08 12:29:16 2018 +0000
@@ -0,0 +1,108 @@
+#include "gyro.h"
+//#include "motor.h"
+#define IMU_TX_PINMAP p28
+#define IMU_RX_PINMAP p27
+Serial device(IMU_TX_PINMAP, IMU_RX_PINMAP);  // tx, rx
+Ticker ddrive;
+Timer gotime;
+int started = false;
+Serial pc(USBTX,USBRX);
+Motor M1(p19,p20,p21);
+Motor M2(p17,p18,p22);
+unsigned char Re_buf[11],counter=0;
+unsigned char sign=0;
+int nnn=0;
+float Standang; //standary angle
+float angles=0;
+float yaw = 0;
+void output();
+void deviceEvent() {
+    Re_buf[counter]=(unsigned char)device.getc();
+    if(counter==0&&Re_buf[0]!=0x55) return;      //第0号数据不是帧头              
+    counter++;       
+    if(counter==11)             //接收到11个数据
+    {    
+       counter=0;               //重新赋值，准备下一帧数据的接收 
+       sign=1;
+    }
+    output();
+}
+void drive()
+{
+    int tem;
+    tem = (unsigned int)Standang-yaw;
+     pc.printf("DisAngle %d\n",tem);
+    if (tem>1)
+    {
+        M1=0.05;M2=0.3;    
+        }
+    else if ( tem<-1)
+    {
+        M1=0.3,M2=0.05;}
+    else 
+    {
+        M1=0.2;M2=0.2;}
+}
+void output()
+{
+    if(sign)
+  {  
+     sign=0;
+     if(Re_buf[0]==0x55)      //检查帧头
+    {  
+    for (int i=0;i<10;i++)
+    switch(Re_buf [1])
+    {
+    static float lastYaw = 0;
+    static float turnNum = 0;
+    case 0x53:
+        double yaw_;
+        yaw_ = Re_buf[7] << 8 | Re_buf[6];
+        yaw_ = yaw_ / 32768.0 * 180;
+        if(lastYaw < 90 && yaw_ > 270){turnNum -= 1;}
+        else if(lastYaw > 270 && yaw_ < 90){turnNum += 1;}
+        lastYaw = yaw_;
+        yaw = yaw_ + 360*turnNum;
+        angles += yaw;
+        nnn +=1;
+        if(nnn == 100)
+        {
+            Standang = angles/nnn;
+            started = true;
+            //pc.printf("standart setted\n");
+            //pc.printf("standartAngle %8.1f\n",Standang);
+        }
+        break;
+        } 
+    }
+  } 
+}
+void change(int up)
+{
+    Standang += up;
+    }
+void turnL()
+{
+    M1=-0.5;M2=0.5;
+    wait(1);
+    }
+void turnR()
+{
+    M1=0.5;M2=-0.5;
+    wait(1);
+    }
+
+void driveit(float t)
+{
+        ddrive.attach(&drive,0.5);
+        if (gotime.read()>t)
+        {
+            ddrive.detach();
+            gotime.reset();
+            }
+    }
+    
+void init_gyro(){
+   device.baud(115200);
+   device.attach(&deviceEvent, device.RxIrq);
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/gyro.h	Fri Jun 08 12:29:16 2018 +0000
@@ -0,0 +1,19 @@
+#ifndef GYRO_H
+#define GYRO_H
+#include "mbed.h"
+#include "motor.h"
+
+extern int started;
+extern float yaw;
+extern Serial pc;
+extern Timer gotime;
+//extern Motor M1;
+//extern Motor M2;
+
+void init_gyro();
+void driveit(float t);
+void change(int up);
+void turnL();
+void turnR();
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Jun 08 12:29:16 2018 +0000
@@ -0,0 +1,39 @@
+#include "mbed.h"
+#include "gyro.h"
+#include "Distance.h"
+Ticker tim;
+void data(int i)
+{
+    change(i);
+    //driveit();
+    }
+void ddd()
+{
+   
+    if(started == true)
+    {
+        tim.detach();
+        gotime.start();
+        driveit(7.5);
+        turnL();
+        //M1=0.3;M2=0.3;
+        //wait(6.75);
+        //直走识别色块
+        
+        
+        
+        //得到返回值 转角(定好三个函数) 
+        // change(测量)+ driveit()
+        // 到达的判断 + change + driveit() 超声波
+
+        //driveit();   
+        
+        }
+    }
+
+int main()
+{
+    init_gyro();
+    tim.attach(&ddd,0.1);
+    
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Fri Jun 08 12:29:16 2018 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/5aab5a7997ee
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/motor.h	Fri Jun 08 12:29:16 2018 +0000
@@ -0,0 +1,48 @@
+#ifndef MOTOR_H
+#define MOTOR_H
+#include "mbed.h"
+class Motor
+{
+public:
+    Motor(DigitalOut ma,DigitalOut mb,PwmOut ms)
+        : motorA(ma),motorB(mb),motorS(ms),motorSpeed(0),direction(0) {}
+    Motor(PinName pa,PinName pb,PinName ps)
+        : motorA(DigitalOut(pa)),motorB(DigitalOut(pb)),motorS(PwmOut(ps)),motorSpeed(0),direction(0) {}
+    
+    void setSpeed(float speed);
+    void setDefaultDirection(int dir) {
+        direction = dir;
+    }
+    
+    void operator = (float f){
+        setSpeed(f);
+    }
+    
+    operator float(){
+        return motorSpeed;
+    }
+
+private:
+    DigitalOut motorA,motorB;
+    PwmOut motorS;
+    float motorSpeed;
+    int direction;
+
+};
+
+void Motor::setSpeed(float speed)
+{
+    if(speed > 0) {
+        motorA = direction?1:0;
+        motorB = direction?0:1;
+    } else {
+        motorA = direction?0:1;
+        motorB = direction?1:0;
+    }
+
+    motorSpeed = speed;
+    
+    motorS = abs(speed);
+}
+
+#endif
\ No newline at end of file