liang brain
fdas
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
--- /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
--- /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
--- /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
--- /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
--- /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