liang brain / Mbed 2 deprecated Ex_econdercar_ROS

fdas

Dependencies:   QEI mbed

Files at this revision

API Documentation at this revision

Revision 0:13ba01a70b4a, committed 2018-05-06

Comitter:
brainliang
Date:
Sun May 06 06:01:45 2018 +0000
Commit message:
sdf

Changed in this revision

Motor3_L298N/Moter3_L298N.cpp Show annotated file Show diff for this revision Revisions of this file
Motor3_L298N/Moter3_L298N.h Show annotated file Show diff for this revision Revisions of this file
QEI.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file 
diff -r 000000000000 -r 13ba01a70b4a Motor3_L298N/Moter3_L298N.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motor3_L298N/Moter3_L298N.cpp	Sun May 06 06:01:45 2018 +0000
@@ -0,0 +1,48 @@
+#include "Moter3_L298N.h"
+#include "mbed.h"
+#define HIGH 1
+#define LOW 0
+
+Motor_3::Motor_3(PinName dia1,PinName dia2,PinName pwa):
+    _dia1(dia1),_dia2(dia2),_pwa(pwa),_paspeed(0)
+{
+    //init
+    DigitalOut pda1(_dia1,LOW);
+    DigitalOut pda2(_dia2,LOW);    
+}
+
+void Motor_3::mv(float speed)
+{
+    PwmOut mypwa(_pwa);
+    mypwa.period_ms(20);
+    speed=speed/100*20/1000;
+    if(speed>0)
+    {
+         DigitalOut mydia1(_dia1,HIGH);
+         DigitalOut mydia2(_dia2,LOW);
+        _paspeed=speed;
+        mypwa.pulsewidth(_paspeed);
+    }
+    else if (speed<0)
+    {
+        speed=abs(speed);
+        DigitalOut mydia1(_dia1,LOW);
+        DigitalOut mydia2(_dia2,HIGH);
+        _paspeed=speed;
+        mypwa.pulsewidth(_paspeed); 
+    }
+    else 
+    {
+        speed=abs(speed);
+        DigitalOut mydia1(_dia1,LOW);
+        DigitalOut mydia2(_dia2,LOW);
+        _paspeed=speed;
+        mypwa.pulsewidth(_paspeed); 
+    }
+        
+}
+void Motor_3::stop()
+{
+    DigitalOut pda1(_dia1,LOW);
+    DigitalOut pda2(_dia2,LOW);
+}
\ No newline at end of file

diff -r 000000000000 -r 13ba01a70b4a Motor3_L298N/Moter3_L298N.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motor3_L298N/Moter3_L298N.h	Sun May 06 06:01:45 2018 +0000
@@ -0,0 +1,21 @@
+#ifndef MOTOR3_L298N_H_
+#define MOTOR3_L298N_H_
+#include "mbed.h"
+
+DigitalOut mydia1(PinName a1,int b1);
+DigitalOut mydia2(PinName a2,int b2);
+PwmOut mypwa(PinName pa);
+
+
+class Motor_3{
+public:
+    Motor_3(PinName dia1,PinName dia2,PinName pwa);
+    void mv(float speed);               //speed取值范围0-20浮点数
+    void stop();
+protected:
+    float _paspeed;
+    PinName _dia1;
+    PinName _dia2;
+    PinName _pwa;
+};
+#endif

diff -r 000000000000 -r 13ba01a70b4a QEI.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI.lib	Sun May 06 06:01:45 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/brainliang/code/QEI/#a6bd3dee25b5

diff -r 000000000000 -r 13ba01a70b4a main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun May 06 06:01:45 2018 +0000
@@ -0,0 +1,162 @@
+#include "mbed.h"
+#include "Moter3_L298N.h"
+#include "QEI.h"
+#define LOW 0
+#define maxv 344.0    //电机最高转速
+
+float setspeed[2]={150,150}, error_current[2]={0,0}, error_last[2]={0,0}, error_sum[2]={0,0}, kp[2]={0.9,0.9}, ki[2]={0,0}, kd[2]={0,0};
+float time_encoder_pid=0.2;     //编码器读取周期,单位s,也是pid控制计算周期
+
+Motor_3 motor[] = {Motor_3(PB_12,PB_13,PA_11),Motor_3(PB_14,PB_15,PA_8)};
+//Motor_3 motor2(PB_14,PB_15,PA_8);
+
+//调试串口
+Serial pc(PB_10, PB_11);
+Serial bt(PA_2, PA_3);
+
+Ticker toggle_time_ticker;
+//编码器1-4
+QEI wheel[] = {QEI(PB_5, PB_6, NC, 11, QEI::X4_ENCODING),QEI(PB_7, PB_8, NC, 11, QEI::X4_ENCODING)};                //买来的电机自带编码器,说明书上有电机每转一圈编码器发生11个信号
+//QEI wheel2(PB_7, PB_8, NC, 11, QEI::X4_ENCODING);
+
+float a[2]= {150, 150};      //初始速度
+void time_ticker();
+void serialread();
+void pidcontroller();
+
+void pidcontroller(){
+    float target[2];
+    for (int i=0; i<=1;i++){
+        error_sum[i] += error_current[i];
+        target[i]=(a[i] + kp[i]*error_current[i] + ki[i]*(error_current[i] - error_last[i]) +kd[i]*(error_sum[i]));
+        motor[i].mv(target[i]/maxv*100);        //mv函数的参数取值范围是-100~100
+//    motor1.mv(a[0] + kp[0]*error_current[0] + ki*(error_current[0] - error_last[0]) +kd*(error_sum[0]));
+//    motor2.mv(a[1] + kp[1]*error_current[1] + ki*(error_current[1] - error_last[1]) +kd*(error_sum[1]));
+        error_last[i]=error_current[i];
+        i++;
+    }
+    
+    pc.printf("target x:%f y:%f\r\n",target[0],target[1]);
+}
+
+
+void time_ticker(){
+    for (int i=0; i<=1; i++){
+    //获取一个轮子的编码器读到的数和旋转方向
+    error_current[i] = setspeed[i] - wheel[i].getPulses();
+    //wheel[i].getState();   //获取旋转方向,返回值为1--顺时针, -1--逆时针
+    wheel[i].reset();
+    }
+    
+    pc.printf("error x:%f y:%f\r\n",error_current[0],error_current[1]);
+    pidcontroller();
+    
+    
+}
+
+void pcserialread(){
+    switch (pc.getc())
+  {
+    case '1':
+          setspeed[0]=100;
+          setspeed[1]=100;          
+        break;
+    case '2':
+          setspeed[0]=300;
+          setspeed[1]=300;          
+        break;
+    default:
+        break;
+  }
+}
+
+    typedef enum
+{
+    CheckS,CheckW,Checka,Checkb,Checkc,Checkd,Checke,Checkf,CheckSum
+}STATE;
+
+void btserialread(){
+    char u1Rec; 
+    static STATE State = CheckS;//初始状态待检查
+    static char PositionTemp[7]={0,0,0,0,0,0,0};//装载数据值a(左右转) b(前后退) c(机械手高度) d(机械手旋转角度) Sum(求和校验)
+    u1Rec = bt.getc(); 
+    switch(State)
+    {
+        case CheckS:
+            if(u1Rec==(char)'S')
+                State=CheckW;//准备跳转至数据读取
+            else
+                State=CheckS;
+            break;
+        case CheckW:
+            if(u1Rec==(char)'W')
+            State=Checka;
+            else if(u1Rec==(char)'S')
+                State=CheckW;
+            else 
+                State=CheckS;
+            break;
+        case Checka:
+            PositionTemp[0]=u1Rec;
+            State=Checkb;
+            break;
+        case Checkb:
+            PositionTemp[1]=u1Rec;
+            State=Checkc;
+            break;
+        case Checkc:
+            PositionTemp[2]=u1Rec;
+            State=Checkd;
+            break;
+        case Checkd:
+            PositionTemp[3]=u1Rec;
+            State=Checke;
+            break;
+        case Checke:
+            PositionTemp[4]=u1Rec;
+            State=Checkf;
+            break;
+        case Checkf:
+            PositionTemp[5]=u1Rec;
+            State=CheckSum;
+            break;
+        case CheckSum:
+            PositionTemp[6]=u1Rec;
+            if(PositionTemp[6]==(char)(PositionTemp[0]+PositionTemp[1]+PositionTemp[2]+PositionTemp[3]+PositionTemp[4]+PositionTemp[5]))
+            {
+                kp[0] = PositionTemp[0];
+                ki[0] = PositionTemp[1];
+                kd[0] = PositionTemp[2];
+                kp[1] = PositionTemp[3];
+                ki[1] = PositionTemp[4];
+                kd[1] = PositionTemp[5];
+            }
+            State=CheckS;
+            break;
+        default:
+            State=CheckS;
+            break;
+    }
+}
+
+
+
+int main()
+{
+    toggle_time_ticker.attach(&time_ticker, time_encoder_pid);       //使能ticker中断,0.2s产生一次.中断函数用于获取每个轮子的计数值
+    
+    pc.attach(&pcserialread);
+    bt.attach(&btserialread);
+    //for (int speed=10;speed<1000;speed+=10){        
+//        pc.printf("set to %d\r\n",speed);
+//        for (int i=0; i<=1; i++){
+//            motor[i].mv(speed/maxv*100);
+//        } 
+//        wait(2);
+//    }
+    for (int i=0; i<=1; i++){
+        motor[i].mv(a[i]/maxv*100);
+    } 
+    
+    while(1);
+}
\ No newline at end of file

diff -r 000000000000 -r 13ba01a70b4a mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Sun May 06 06:01:45 2018 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/5aab5a7997ee
\ No newline at end of file