Evan Brown
/
MM_rat_Assignment4-newwest
p
Fork of MM_rat_Assignment4 by
header.h
- Committer:
- evenbrownie
- Date:
- 2017-11-28
- Revision:
- 7:7cdb0381e1b8
- Parent:
- 6:ce498700a28c
File content as of revision 7:7cdb0381e1b8:
#ifndef Header_H #define Header_H //IR: //PB 7 Left Side //PB 0 Front Left //PC 11 Front Right //PC 10 Side Right //Receiver: // Left Side PC_0 // Front Left PC_1 // Front Right PA_4 // Side Left PA_0 DigitalOut LeftIR(PB_7); DigitalOut FrontLeftIR(PB_0); DigitalOut FrontRightIR(PC_11); DigitalOut RightIR(PC_10); AnalogIn LeftReceiver(PC_0); AnalogIn FrontLeftReceiver(PC_1); AnalogIn FrontRightReceiver(PA_4); AnalogIn RightReceiver(PA_0); PwmOut lpwmf(PB_6); PwmOut lpwmb(PA_7); PwmOut rpwmf(PB_10); PwmOut rpwmb(PC_7); PinName rfront(PB_3); PinName rback(PA_15); PinName lfront(PA_1); PinName lback(PC_4); Serial pc(SERIAL_TX, SERIAL_RX); QEI LeftEncoder(lfront, lback, NC, 4096, QEI::X4_ENCODING); QEI RightEncoder(rfront, rback, NC, 4096, QEI::X4_ENCODING); const float rbase = 0.15f; //.09 const float lbase = 0.163f; //.1 const float WALL_IR_L = 0.74f; const float WALL_IR_R = 0.74f; const float WALL_IR_FL = 0.74f; const float WALL_IR_FR = 0.74f; Timer t_time; 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?) pid(){ integral = 0.0f; prev = 0.0f; kp = .01f; //the ks should be negative to counteract error ki = 0.0f; kd = 0.8f; } float integral; int prev; float kp; //the ks should be negative to counteract error float ki; float kd; }; void resetpid(struct pid *e) { e->integral = 0.0f; e->prev = 0.0f; } float getFix(struct pid *e, float error, float time) { float d = (error - e->prev)/time; e->integral += error * time; e->prev = error; float temp = (e->kp * error + e->ki * e->integral + e->kd * d); return temp; } float constrain(float l_limit, float u_limit, float val){ if (val < l_limit){ return l_limit; } else if(val > u_limit){ return u_limit; } return val; } struct IRstruct{ IRstruct(){ statuscode = 0; } float leftIR, leftfrontIR, rightfrontIR, rightIR; unsigned int statuscode; }; unsigned int readIR(IRstruct& in_ir){ in_ir.leftIR = LeftReceiver.read(); in_ir.rightIR = RightReceiver.read(); in_ir.leftfrontIR = FrontLeftReceiver.read(); in_ir.rightfrontIR = FrontRightReceiver.read(); //pc.printf("LeftIR: %f RightIR: %f leftfronIR: %f rightfrontIR: %f \n", in_ir.leftIR, in_ir.rightIR, in_ir.leftfrontIR, in_ir.rightfrontIR); //We then set the status codes to be analyzed later for what we should do in_ir.statuscode = (in_ir.statuscode|0x0); //0000...FL,FR,L,R //Bottom is more of an abstraction, we probably don't even need it if(in_ir.leftIR > WALL_IR_L) in_ir.statuscode = (in_ir.statuscode|2); // 0...0 | 1 -> 0...1 if(in_ir.rightIR > WALL_IR_R) in_ir.statuscode = in_ir.statuscode|1; if(in_ir.leftfrontIR > WALL_IR_FL) in_ir.statuscode = in_ir.statuscode|8; if(in_ir.rightfrontIR > WALL_IR_FR) in_ir.statuscode = in_ir.statuscode|4; return in_ir.statuscode; } void turn_right(){ int l_init = LeftEncoder.getPulses(); pc.printf("l_init %d \n", l_init); while((LeftEncoder.getPulses() - l_init) < 166){ lpwmb = 0.0f; rpwmf = 0.0f; lpwmf = lbase; rpwmb = rbase; //pc.printf("LeftEncoder: %d \n", LeftEncoder.getPulses()); // pc.printf("lEncoderDifference %f \n", LeftEncoder.getPulses() - l_init); } //lpwmb = 0.0f; //rpwmf = 0.0f; lpwmf = 0.0f; rpwmb = 0.0f; wait(1.0); } void turn_left(){ int r_init = RightEncoder.getPulses(); pc.printf("r_init %d \n", r_init); while((RightEncoder.getPulses() - r_init) < 150){ lpwmf = 0.0f; rpwmb = 0.0f; lpwmb = lbase; rpwmf = rbase; // pc.printf("RightEncoder: %d \n", RightEncoder.getPulses()); //pc.printf("rEncoderDifference %f: \n", RightEncoder.getPulses()-r_init); } lpwmb = 0.0f; rpwmf = 0.0f; wait( 1.0); } void ProcessIR(float dt, pid& IR_lman, float& lspeed, float& rspeed){ IRstruct ir_read; int status = readIR(ir_read); 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 turn_right(); } 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 turn_left(); } float error_ir = (ir_read.rightIR - ir_read.leftIR); /*if(error_ir < 0.0001f){ resetpid(&IR_lman); }*/ float adjust_l = getFix(&IR_lman, error_ir, dt); lspeed = constrain(0.0f, 0.3f, lbase - adjust_l); rspeed = constrain(0.0f, 0.3f, rbase); pc.printf("adjust_l: %f lspeed: %f rspeed: %f error_ir: %f dt: %f \n", adjust_l, lspeed, rspeed, error_ir, dt); } #endif