Qian Yuyang / Mbed 2 deprecated EX_velocity_control_QianYuyang

velocity_control

Dependencies:   mbed QEI-1 nRF24L01P

main.cpp@4:b74c57dc0ea6, 2019-11-14 (annotated)

Committer:
AlexQian
Date:
Thu Nov 14 06:38:08 2019 +0000
Revision:
4:b74c57dc0ea6
Parent:
3:ee5e434e047e 
velocity control

Who changed what in which revision?

UserRevisionLine numberNew contents of line
AlexQian 3:ee5e434e047e 1 #define HIGH 1
AlexQian 3:ee5e434e047e 2 #define LOW 0
glintligo 0:e63858fec119 3 #include "mbed.h"
AlexQian 3:ee5e434e047e 4 #include <string>
AlexQian 3:ee5e434e047e 5 typedef bool boolean;
AlexQian 3:ee5e434e047e 6 typedef std::string String;
glintligo 0:e63858fec119 7 #include "QEI.h"
brainliang 2:af377291f3ae 8
AlexQian 3:ee5e434e047e 9 int Position;
AlexQian 4:b74c57dc0ea6 10 float velocity;
AlexQian 4:b74c57dc0ea6 11 float target_velocity;
AlexQian 3:ee5e434e047e 12 float Error;
AlexQian 3:ee5e434e047e 13 int Output;
AlexQian 3:ee5e434e047e 14 float Error_Last;
AlexQian 3:ee5e434e047e 15 float P;
AlexQian 3:ee5e434e047e 16 float Error_int;
AlexQian 3:ee5e434e047e 17 float I;
AlexQian 3:ee5e434e047e 18 float Error_diff;
AlexQian 3:ee5e434e047e 19 float D;
AlexQian 4:b74c57dc0ea6 20 float T=0.05; //控制周期
brainliang 2:af377291f3ae 21
AlexQian 3:ee5e434e047e 22 QEI qei_PA_0(PA_0,PA_1,NC,13,QEI::X4_ENCODING);
AlexQian 3:ee5e434e047e 23 Serial Serial_2(PA_2,PA_3);
AlexQian 3:ee5e434e047e 24 Ticker tick622103;
AlexQian 3:ee5e434e047e 25 PwmOut myServoPB_0(PB_0);
AlexQian 3:ee5e434e047e 26 PwmOut myServoPB_1(PB_1);
brainliang 2:af377291f3ae 27
AlexQian 3:ee5e434e047e 28
brainliang 2:af377291f3ae 29
AlexQian 3:ee5e434e047e 30 void PID_Caculation() {
AlexQian 4:b74c57dc0ea6 31 Error = target_velocity - velocity;
AlexQian 3:ee5e434e047e 32 Error_diff = Error - Error_Last;
AlexQian 3:ee5e434e047e 33 Error_Last = Error;
AlexQian 3:ee5e434e047e 34 Error_int = Error_int + Error;
AlexQian 3:ee5e434e047e 35 if (Error_int > 100) {
AlexQian 3:ee5e434e047e 36 Error_int = 100;
AlexQian 3:ee5e434e047e 37 } else if (Error_int < -100) {
AlexQian 3:ee5e434e047e 38 Error_int = -100;
AlexQian 3:ee5e434e047e 39 }
AlexQian 3:ee5e434e047e 40 if (Error > -10 && Error < 10) {
AlexQian 3:ee5e434e047e 41 Output = 0;
AlexQian 3:ee5e434e047e 42 } else {
AlexQian 3:ee5e434e047e 43 Output = P * Error + (I * Error_int + D * Error_diff);
AlexQian 3:ee5e434e047e 44 }
AlexQian 3:ee5e434e047e 45 if (Output > 100) {
AlexQian 3:ee5e434e047e 46 Output = 100;
AlexQian 3:ee5e434e047e 47 } else if (Output < -100) {
AlexQian 3:ee5e434e047e 48 Output = -100;
AlexQian 3:ee5e434e047e 49 }
glintligo 0:e63858fec119 50 }
glintligo 1:1e3eb2d1496b 51
AlexQian 3:ee5e434e047e 52 void Set_speed() {
AlexQian 3:ee5e434e047e 53 if (Output >= 0) {
AlexQian 3:ee5e434e047e 54 myServoPB_0.period_ms(20);
AlexQian 3:ee5e434e047e 55 myServoPB_0.pulsewidth_us((200 * Output));
AlexQian 3:ee5e434e047e 56 myServoPB_1.period_ms(20);
AlexQian 3:ee5e434e047e 57 myServoPB_1.pulsewidth_us(0);
AlexQian 3:ee5e434e047e 58 } else if (Output < 0) {
AlexQian 3:ee5e434e047e 59 myServoPB_0.period_ms(20);
AlexQian 3:ee5e434e047e 60 myServoPB_0.pulsewidth_us(0);
AlexQian 3:ee5e434e047e 61 myServoPB_1.period_ms(20);
AlexQian 3:ee5e434e047e 62 myServoPB_1.pulsewidth_us((-200 * Output));
AlexQian 3:ee5e434e047e 63 }
glintligo 1:1e3eb2d1496b 64 }
glintligo 1:1e3eb2d1496b 65
AlexQian 3:ee5e434e047e 66 void tick622103_handle() {
AlexQian 4:b74c57dc0ea6 67 velocity=qei_PA_0.getPulses()/1300/T;
AlexQian 3:ee5e434e047e 68 Position = Position + qei_PA_0.getPulses();
AlexQian 3:ee5e434e047e 69 qei_PA_0.reset();
AlexQian 3:ee5e434e047e 70 Serial_2.printf("%d\n",Position);
AlexQian 3:ee5e434e047e 71 PID_Caculation();
AlexQian 3:ee5e434e047e 72 Set_speed();
AlexQian 3:ee5e434e047e 73 }
AlexQian 3:ee5e434e047e 74
AlexQian 3:ee5e434e047e 75
AlexQian 3:ee5e434e047e 76 int main() {
AlexQian 3:ee5e434e047e 77 Serial_2.baud(9600);
AlexQian 3:ee5e434e047e 78
AlexQian 4:b74c57dc0ea6 79 tick622103.attach(&tick622103_handle,T);
AlexQian 3:ee5e434e047e 80
AlexQian 3:ee5e434e047e 81 Position = 0;
AlexQian 3:ee5e434e047e 82 qei_PA_0.reset();
AlexQian 3:ee5e434e047e 83 while (true) {
AlexQian 4:b74c57dc0ea6 84 target_velocity=1; //单位r/s
AlexQian 3:ee5e434e047e 85 }
AlexQian 3:ee5e434e047e 86
glintligo 1:1e3eb2d1496b 87 }