header.h

00001 #ifndef Header_H
00002 #define Header_H
00003 //IR:
00004 //PB 7 Left Side
00005 //PB 0 Front Left
00006 //PC 11 Front Right 
00007 //PC 10 Side Right
00008 //Receiver:
00009 // Left Side PC_0
00010 // Front Left PC_1
00011 // Front Right PA_4
00012 // Side Left PA_0
00013 DigitalOut LeftIR(PB_7);
00014 DigitalOut FrontLeftIR(PB_0);
00015 DigitalOut FrontRightIR(PC_11);
00016 DigitalOut RightIR(PC_10);
00017 AnalogIn LeftReceiver(PC_0);
00018 AnalogIn FrontLeftReceiver(PC_1);
00019 AnalogIn FrontRightReceiver(PA_4);
00020 AnalogIn RightReceiver(PA_0);
00021 PwmOut lpwmf(PA_7);
00022 PwmOut lpwmb(PB_6);
00023 PwmOut rpwmf(PB_10);
00024 PwmOut rpwmb(PC_7);
00025 PinName rfront(PB_3);
00026 PinName rback(PA_15);
00027 PinName lfront(PA_1);
00028 PinName lback(PC_4);
00029 Serial pc(SERIAL_TX, SERIAL_RX); 
00030 QEI LeftEncoder(lfront, lback, NC, 4096, QEI::X4_ENCODING);
00031 QEI RightEncoder(rfront, rback, NC, 4096, QEI::X4_ENCODING);       
00032 const float rbase = 0.12f;
00033 const float lbase = 0.185f;
00034 const float WALL_IR_L = 0.74f;
00035 const float WALL_IR_R = 0.74f;
00036 const float WALL_IR_FL = 0.74f;
00037 const float WALL_IR_FR = 0.74f;    
00038 Timer t_time;
00039 struct pid { //Note that because we have two different types of distance sensors (Andrew's works a little differently than Jeffrey's we should have two different errors. To stay straight though we can just use one side right?)
00040   pid(){
00041         integral = 0.0f;
00042         prev = 0.0f;
00043         kp = .01f; //the ks should be negative to counteract error
00044         ki = 0.0f;
00045         kd = 0.8f;
00046       }
00047   float integral;
00048   int prev;
00049   float kp; //the ks should be negative to counteract error
00050   float ki;
00051   float kd;
00052 };
00053 
00054 void resetpid(struct pid *e) {
00055   e->integral = 0.0f;
00056   e->prev = 0.0f;
00057 }
00058 float getFix(struct pid *e, float error, float time) {
00059   float d = (error - e->prev)/time;
00060   e->integral += error * time;
00061   e->prev = error;
00062   float temp = (e->kp * error + e->ki * e->integral + e->kd * d);
00063   return temp;
00064 }
00065 
00066 float constrain(float l_limit, float u_limit, float val){
00067     if (val < l_limit){
00068         return l_limit;
00069         }
00070     else if(val > u_limit){
00071         return u_limit;
00072         }    
00073     return val;    
00074 }
00075 
00076 struct IRstruct{
00077     IRstruct(){
00078         statuscode = 0;
00079     }
00080     float leftIR, leftfrontIR, rightfrontIR, rightIR;
00081     unsigned int statuscode;
00082 };
00083 
00084 unsigned int readIR(IRstruct& in_ir){
00085     in_ir.leftIR = LeftReceiver.read();
00086     in_ir.rightIR = RightReceiver.read();
00087     in_ir.leftfrontIR = FrontLeftReceiver.read();
00088     in_ir.rightfrontIR = FrontRightReceiver.read();
00089     //pc.printf("LeftIR: %f RightIR: %f leftfronIR: %f rightfrontIR: %f \n", in_ir.leftIR, in_ir.rightIR, in_ir.leftfrontIR, in_ir.rightfrontIR); 
00090     
00091     //We then set the status codes to be analyzed later for what we should do
00092     in_ir.statuscode = (in_ir.statuscode|0x0);
00093     
00094     //0000...FL,FR,L,R
00095     //Bottom is more of an abstraction, we probably don't even need it
00096     if(in_ir.leftIR > WALL_IR_L)
00097         in_ir.statuscode = (in_ir.statuscode|2); // 0...0 | 1 -> 0...1
00098     if(in_ir.rightIR > WALL_IR_R)
00099         in_ir.statuscode = in_ir.statuscode|1;
00100     if(in_ir.leftfrontIR > WALL_IR_FL)
00101         in_ir.statuscode = in_ir.statuscode|8;
00102     if(in_ir.rightfrontIR > WALL_IR_FR)
00103         in_ir.statuscode = in_ir.statuscode|4;
00104     return in_ir.statuscode;
00105 }
00106 
00107 void turn_right(){
00108     int l_init = LeftEncoder.getPulses();
00109     pc.printf("l_init %d \n", l_init);
00110     while((LeftEncoder.getPulses() - l_init) < 3000){
00111         lpwmb = 0.0f;
00112         rpwmf = 0.0f;
00113         lpwmf = lbase;
00114         rpwmb = rbase;
00115         pc.printf("LeftEncoder: %d \n", LeftEncoder.getPulses());
00116         pc.printf("lEncoderDifference %f \n", LeftEncoder.getPulses() - l_init);
00117     }
00118         //lpwmb = 0.0f;
00119         //rpwmf = 0.0f;
00120         //lpwmf = 0.0f;
00121         //rpwmb = 0.0f;
00122 }
00123 
00124 void turn_left(){
00125     int r_init = RightEncoder.getPulses();
00126     pc.printf("r_init %d \n", r_init);
00127     while((RightEncoder.getPulses() - r_init) < 2000){
00128         lpwmf = 0.0f;
00129         rpwmb = 0.0f;
00130         lpwmb = lbase;
00131         rpwmf = rbase;
00132         pc.printf("RightEncoder: %d \n", RightEncoder.getPulses());
00133         pc.printf("rEncoderDifference %f: \n", RightEncoder.getPulses()-r_init);
00134     }
00135 }
00136 
00137 void ProcessIR(float dt, pid& IR_lman, float& lspeed, float& rspeed){
00138     IRstruct ir_read;
00139     int status = readIR(ir_read); 
00140     if(ir_read.leftIR > WALL_IR_L && ir_read.leftfrontIR > WALL_IR_FL && ir_read.rightfrontIR > WALL_IR_FR && ir_read.rightIR < WALL_IR_R){ //High in front and left -> Turn right
00141         turn_right();
00142     }
00143     else if(ir_read.rightIR > WALL_IR_R && ir_read.leftfrontIR > WALL_IR_FL && ir_read.rightfrontIR > WALL_IR_FR){ //High in front and right -> Turn left
00144         turn_left();
00145     }
00146     float error_ir = (ir_read.rightIR - ir_read.leftIR);
00147     /*if(error_ir < 0.0001f){    
00148         resetpid(&IR_lman);
00149     }*/
00150     float adjust_l = getFix(&IR_lman, error_ir, dt);
00151     lspeed = constrain(0.0f, 0.3f, lbase - adjust_l);
00152     rspeed = constrain(0.0f, 0.3f, rbase);
00153     pc.printf("adjust_l: %f lspeed: %f rspeed: %f error_ir: %f dt: %f \n", adjust_l, lspeed, rspeed, error_ir, dt);
00154 }
00155     
00156 #endif