Revision 4:b74c57dc0ea6, committed 2019-11-14

Comitter:

AlexQian

Date:

Thu Nov 14 06:38:08 2019 +0000

Parent:

3:ee5e434e047e

Commit message:

velocity control

Changed in this revision

--- a/main.cpp	Wed Oct 16 13:59:33 2019 +0000
+++ b/main.cpp	Thu Nov 14 06:38:08 2019 +0000
@@ -7,6 +7,8 @@
 #include "QEI.h"
 
 int Position;
+float velocity;
+float target_velocity;
 float Error;
 int Output;
 float Error_Last;
@@ -15,6 +17,7 @@
 float I;
 float Error_diff;
 float D;
+float T=0.05; //控制周期
 
 QEI qei_PA_0(PA_0,PA_1,NC,13,QEI::X4_ENCODING);
 Serial Serial_2(PA_2,PA_3);
@@ -25,7 +28,7 @@
 
 
 void PID_Caculation() {
-Error = 0 - Position;
+Error =  target_velocity - velocity;
 Error_diff = Error - Error_Last;
 Error_Last = Error;
 Error_int = Error_int + Error;
@@ -61,6 +64,7 @@
 }
 
 void tick622103_handle() {
+velocity=qei_PA_0.getPulses()/1300/T;
 Position = Position + qei_PA_0.getPulses();
 qei_PA_0.reset();
 Serial_2.printf("%d\n",Position);
@@ -72,11 +76,12 @@
 int main() {
 Serial_2.baud(9600);
 
-tick622103.attach(&tick622103_handle,0.05);
+tick622103.attach(&tick622103_handle,T);
 
 Position = 0;
 qei_PA_0.reset();
 while (true) {
+    target_velocity=1; //单位r/s
 }
 
 }
\ No newline at end of file