File content as of revision 3:ee5e434e047e:

#define HIGH 1
#define LOW 0
#include "mbed.h"
#include <string>
typedef bool boolean;
typedef std::string String;
#include "QEI.h"

int Position;
float Error;
int Output;
float Error_Last;
float P;
float Error_int;
float I;
float Error_diff;
float D;

QEI qei_PA_0(PA_0,PA_1,NC,13,QEI::X4_ENCODING);
Serial Serial_2(PA_2,PA_3);
Ticker tick622103;
PwmOut myServoPB_0(PB_0);
PwmOut myServoPB_1(PB_1);



void PID_Caculation() {
Error = 0 - Position;
Error_diff = Error - Error_Last;
Error_Last = Error;
Error_int = Error_int + Error;
if (Error_int > 100) {
Error_int = 100;
} else if (Error_int < -100) {
Error_int = -100;
}
if (Error > -10 && Error < 10) {
Output = 0;
} else {
Output = P * Error + (I * Error_int + D * Error_diff);
}
if (Output > 100) {
Output = 100;
} else if (Output < -100) {
Output = -100;
}
}

void Set_speed() {
if (Output >= 0) {
myServoPB_0.period_ms(20);
myServoPB_0.pulsewidth_us((200 * Output));
myServoPB_1.period_ms(20);
myServoPB_1.pulsewidth_us(0);
} else if (Output < 0) {
myServoPB_0.period_ms(20);
myServoPB_0.pulsewidth_us(0);
myServoPB_1.period_ms(20);
myServoPB_1.pulsewidth_us((-200 * Output));
}
}

void tick622103_handle() {
Position = Position + qei_PA_0.getPulses();
qei_PA_0.reset();
Serial_2.printf("%d\n",Position);
PID_Caculation();
Set_speed();
}


int main() {
Serial_2.baud(9600);

tick622103.attach(&tick622103_handle,0.05);

Position = 0;
qei_PA_0.reset();
while (true) {
}

}