main.cpp

00001 #define HIGH 1
00002 #define LOW 0
00003 #include "mbed.h"
00004 #include <string>
00005 typedef bool boolean;
00006 typedef std::string String;
00007 #include "QEI.h"
00008 
00009 int Position;
00010 float Error;
00011 int Output;
00012 float Error_Last;
00013 float P;
00014 float Error_int;
00015 float I;
00016 float Error_diff;
00017 float D;
00018 
00019 QEI qei_PA_0(PA_0,PA_1,NC,13,QEI::X4_ENCODING);
00020 Serial Serial_2(PA_2,PA_3);
00021 PwmOut myServoPB_0(PB_0);
00022 PwmOut myServoPB_1(PB_1);
00023 Ticker tick561436;
00024 
00025 void PID_Caculation() {
00026 Error = 0 - Position;
00027 Error_diff = Error - Error_Last;
00028 Error_Last = Error;
00029 Error_int = Error_int + Error;
00030 if (Error_int > 100) {
00031 Error_int = 100;
00032 } else if (Error_int < -100) {
00033 Error_int = -100;
00034 }
00035 if (Error > -10 && Error < 10) {
00036 Output = 0;
00037 } else {
00038 Output = P * Error + (I * Error_int + D * Error_diff);
00039 }
00040 if (Output > 100) {
00041 Output = 100;
00042 } else if (Output < -100) {
00043 Output = -100;
00044 }
00045 }
00046 
00047 void Set_speed() {
00048 if (Output >= 0) {
00049 myServoPB_0.period_ms(20);
00050 myServoPB_0.pulsewidth_us((200 * Output));
00051 myServoPB_1.period_ms(20);
00052 myServoPB_1.pulsewidth_us(0);
00053 } else if (Output < 0) {
00054 myServoPB_0.period_ms(20);
00055 myServoPB_0.pulsewidth_us(0);
00056 myServoPB_1.period_ms(20);
00057 myServoPB_1.pulsewidth_us((-200 * Output));
00058 }
00059 }
00060 
00061 void tick561436_handle() {
00062 Position = Position + qei_PA_0.getPulses();
00063 qei_PA_0.reset();
00064 Serial_2.printf("%d\n",Position);
00065 PID_Caculation();
00066 Set_speed();
00067 }
00068 
00069 
00070 int main() {
00071 Serial_2.baud(9600);
00072 
00073 tick561436.attach(&tick561436_handle,0.05);
00074 Position = 0;
00075 qei_PA_0.reset();
00076 P = 1;
00077 I = 0;
00078 D = 10;
00079 while (true) {
00080 }
00081 
00082 }