main.cpp

00001 #include "mbed.h"
00002 #include "QEI.h"
00003 #include "header.h"
00004 
00005 inline void pulse_ir(int in){
00006     for(int i = 0; i < 4; i++){
00007         LeftIR = in;
00008         
00009         FrontLeftIR = in;
00010         FrontRightIR = in;
00011         RightIR = in;
00012     }
00013 }
00014 
00015 int main() {
00016     pid lman, rman;
00017     lman.kp = .009f;
00018     lman.ki = .0f;
00019     lman.kd = .0f;
00020     rman.kp = .5f;
00021     rman.ki = .1f;
00022     rman.kd = .4f;
00023     
00024     lpwmf.period(0.01f);
00025     lpwmb.period(0.01f);
00026     lpwmf = 0; //Previously started on, replace with lpwmf = lbase to make it start on (not a good idea)
00027     rpwmb=0;
00028     rpwmf.period(0.01f);
00029     rpwmb.period(0.01f);
00030     rpwmb=0;
00031     rpwmf = 0;
00032     
00033     pid ir_lman, ir_rman;
00034     ir_lman.kp = .0001f;
00035     ir_lman.ki = .0f;
00036     ir_lman.kd = .0f;
00037     ir_rman.kp = .5f;
00038     ir_rman.ki = .1f;
00039     ir_rman.kd = .4f;    
00040     t_time.start();
00041     while(1){       
00042        float dt = t_time.read();
00043        float lspeed = 0; float rspeed = 0;
00044             LeftIR = 1;
00045             wait(.01f);
00046             FrontLeftIR=1;
00047             wait(.01f);
00048             FrontRightIR=1;
00049             wait(.01f);
00050             RightIR=1;
00051        ProcessIR(dt, ir_lman, lspeed, rspeed);
00052        lpwmb = 0; rpwmb = 0;
00053        lpwmf = lspeed; rpwmf = rspeed;
00054             LeftIR = 0;
00055             wait(.01f);
00056             FrontLeftIR=0;
00057             wait(.01f);
00058             FrontRightIR=0;
00059             wait(.01f);
00060             RightIR=0;
00061        t_time.reset();
00062     }
00063 }