main.cpp

00001 #include "QEI.h"
00002 
00003 Serial pc(USBTX, USBRX);
00004 RawSerial smc(PC_6, PA_12);
00005 //Use X2 encoding by default.
00006 QEI wheel (PC_8, PC_5, NC, 624);
00007 
00008 Ticker ctrl_tick;
00009 
00010 int tgt = 5000;
00011 
00012 void ctrl_event() {
00013     static int rot = 0;
00014     static int output = 0;
00015     rot = wheel.getPulses();
00016     pc.printf("Pulses is: %i\n\r", rot);
00017     int err = tgt - rot;
00018     if (err > 0) {
00019         err = err;
00020         output = (err * 0.1);
00021         if (output > 127) output = 127;
00022         smc.putc(0x8A);
00023         smc.putc((uint8_t)output);
00024     } else {
00025         err =  -err;
00026         output = (err * 0.1);
00027         if (output > 127) output = 127;
00028         smc.putc(0x89);
00029         smc.putc(output);
00030     }
00031 }
00032  
00033 int main() {
00034     pc.baud(115200);
00035     smc.baud(115200);
00036     ctrl_tick.attach(&ctrl_event, 0.1);
00037     while(1){
00038         wait(5);
00039         if (tgt != 0) tgt = 0;
00040         else tgt = 5000;
00041     }
00042 }