drivecontrol.cpp

00001 #include "drivecontrol.h"
00002 #include "Cell.h"
00003 #include "ir_sensor.h"
00004 #include "left_motor.h"
00005 #include "right_motor.h"
00006 #include "pin_assignment.h"
00007 
00008 IRSensor leftIR(PA_8, PC_5);
00009 IRSensor rightIR(PB_0, PA_4);
00010 //Encoder rightEncoder(PA_1, PA_0);
00011 //Encoder leftEncoder(PC_9, PC_8);
00012 LeftMotor * leftMotor;
00013 RightMotor * rightMotor;
00014 Cell * curr_cell;
00015 Serial pc(PA_9, PA_10);
00016 
00017 // Sensor offsets
00018 //float FRONT_SENSOR_THRES = 7.0f, SENSOR_ERROR_OFFSET = 0.0f;
00019 float FRONT_SENSOR_THRES = 2.2f, SENSOR_ERROR_OFFSET = 0.0f;
00020 //float LEFT_WALL_THRES = 0.336f, RIGHT_WALL_THRES = 0.163f;
00021 float LEFT_WALL_THRES = 0.336f, RIGHT_WALL_THRES = 0.163f;
00022 float RIGHT_SIDE_WALL_THRES = 0.20f, LEFT_SIDE_WALL_THRES = 0.20f;
00023 //float RIGHT_SIDE_WALL_THRES = 0.28f, LEFT_SIDE_WALL_THRES = 0.38f;
00024 
00025 // Motor speed offsets
00026 float left_speed, right_speed, motor_speed;
00027 float MOTOR_BASE_SPEED = 20.0f; //12.0f;
00028 float cool_down_offset = 0.0f;
00029 //
00030 //    bool right_front_wall = (rightFrontIR.readIR() * 1000) > 355.0f;//350.39f;
00031 //    bool left_front_wall = (leftFrontIR.readIR() * 1000) > 315.0f;//310.25f;
00032 
00033 float RIGHT_FRONT_THRES = 355.0f;
00034 float LEFT_FRONT_THRES = 315.0f;
00035 
00036 bool enter_right_wall_pid_debug = false;
00037 
00038 // Encoder offsets
00039 int ENCODER_TURN_UNIT = 16500;
00040 
00041 namespace pid_controller { 
00042     // PID Constants
00043     float error_p = 0.0f, old_error_p = 0.0f, old_error_d = 0.0f, error_d = 0.0f;
00044     float total_error = 0.0f;
00045     float P = 18.0f, D = 20.0f;
00046     
00047     void navigate() {
00048         bool has_left_wall = leftDiagonalIR.readIR() > LEFT_WALL_THRES;
00049         bool has_right_wall = rightDiagonalIR.readIR() > RIGHT_WALL_THRES;
00050 
00051         if (has_left_wall && has_right_wall) {
00052             enter_right_wall_pid_debug = false;
00053             if (rightDiagonalIR - RIGHT_WALL_THRES < 0) {
00054                 error_p = rightDiagonalIR - RIGHT_WALL_THRES;
00055             }
00056             else if (leftDiagonalIR - LEFT_WALL_THRES < 0) {
00057                 error_p = LEFT_WALL_THRES - leftDiagonalIR;
00058             }
00059             else{
00060                 error_p = rightDiagonalIR - leftDiagonalIR;
00061                 enter_right_wall_pid_debug = true; 
00062             }
00063             error_d = error_p - old_error_p;  
00064              
00065         }
00066 //        float RIGHT_SIDE_WALL_THRES = 0.59f, LEFT_SIDE_WALL_THRES = 0.38f;
00067         else if (has_left_wall) {
00068            // error_p = 4 * (LEFT_WALL_THRES - 0.50 * leftDiagonalIR.readIR());
00069            error_p = LEFT_WALL_THRES - leftDiagonalIR.readIR();
00070             //error_p = 2 * (LEFT_SIDE_WALL_THRES - leftIR.readIR());
00071             error_d = error_p - old_error_p;
00072             enter_right_wall_pid_debug = false;
00073         }
00074         else if (has_right_wall) {
00075             error_p = rightDiagonalIR.readIR() - RIGHT_WALL_THRES;
00076             //error_p = 2 * (RIGHT_SIDE_WALL_THRES - rightIR.readIR();
00077             error_d = error_p - old_error_p;
00078             enter_right_wall_pid_debug = false;
00079         }
00080         else if (!has_left_wall && !has_right_wall) {
00081             error_p = 0;
00082             error_d = 0;
00083             enter_right_wall_pid_debug = false;
00084         }
00085         total_error = P * error_p + D * (error_d - old_error_d);
00086         old_error_p = error_p;
00087         old_error_d = error_d;
00088         
00089         //MOTOR_BASE_SPEED -= cool_down_offset;
00090         if(total_error < 7.5f){
00091             leftMotor -> speed(MOTOR_BASE_SPEED - total_error);
00092             rightMotor -> speed(MOTOR_BASE_SPEED + total_error);
00093         }
00094         else{
00095             leftMotor->speed(MOTOR_BASE_SPEED);
00096             rightMotor->speed(MOTOR_BASE_SPEED);
00097         }
00098         
00099         
00100     }
00101     
00102     void clear_pid(){
00103         error_p = 0.0f, old_error_p = 0.0f, old_error_d = 0.0f, error_d = 0.0f;
00104         total_error = 0.0f;
00105     }
00106     
00107     void cool_down_speed() {
00108         if (cool_down_offset < 5.0f) {
00109             cool_down_offset += 2.0f;
00110         }
00111     }
00112     
00113     // should use this method as the exit condition
00114     // in the pid_controller::navigate() method
00115     // resets the pid, encoders, etc.
00116     void one_cell_traversed() {
00117         leftEncoder.reset();
00118         rightEncoder.reset();
00119         old_error_p = old_error_d = total_error = 0.0f;
00120     }
00121     
00122     // TODO
00123     void turn (bool turn_right) {
00124         float MOTOR_TURN_SPEED = 14.0f;
00125         
00126         if (turn_right) {
00127             leftMotor -> speed(MOTOR_TURN_SPEED);
00128             rightMotor -> speed(-MOTOR_TURN_SPEED);
00129         }
00130         else {
00131             leftMotor -> speed(-MOTOR_TURN_SPEED);
00132             rightMotor -> speed(MOTOR_TURN_SPEED);
00133         }
00134     }
00135 }
00136 
00137 bool DriveControl::right_wall_pid_debug() {
00138     return enter_right_wall_pid_debug;
00139 }
00140 // Currently only have the x, y position fields for
00141 // each cell.
00142 DriveControl::DriveControl (int start_x, int start_y) {
00143     curr_cell = new Cell (start_x, start_y);
00144     leftMotor= new LeftMotor();
00145     rightMotor = new RightMotor();
00146 }
00147 
00148 // Defines the next cell to traverse.
00149 Cell * next_cell() {
00150     return curr_cell;
00151 }
00152 
00153 int DriveControl::get_next_direction() {
00154     // TODO: Define the direction based on heuristic eval. 
00155     return 1;   
00156 }
00157 
00158 void DriveControl::print_serial_ports(){
00159     pc.printf("LEFT Encoder Reading %d\n\r", leftEncoder.getEncoderDistance(1));
00160     pc.printf("RIGHT Encoder Reading %d\n\r", rightEncoder.getEncoderDistance(0));
00161     pc.printf("FRONT TOP RIGHT IRSensor %f\n\r", rightFrontIR.readIR() * 1000);
00162     pc.printf("FRONT TOP LEFT IRSensor %f\n\r", leftFrontIR.readIR() * 1000);
00163     pc.printf("LEFT Diagonal Sensor %f\n\r", leftDiagonalIR.readIR());
00164     pc.printf("RIGHT Diagonal Sensor %f\n\r", rightDiagonalIR.readIR());
00165     pc.printf("LEFT SIDE Sensor %f\n\r", leftIR.readIR());
00166     pc.printf("RIGHT SIDE Sensor %f\n\r", rightIR.readIR());
00167 }
00168 
00169 void DriveControl::turn_left() {
00170     // TODO: Add PID Control
00171     pid_controller::turn(false);
00172 }
00173 
00174 void DriveControl::set_wall_follower_sensor_thres(){
00175     //TODO
00176     RIGHT_FRONT_THRES = 300.0f;
00177     LEFT_FRONT_THRES = 250.0f;
00178 }
00179 
00180 void DriveControl::turn_right() {
00181     // TODO: Add PID Control
00182     pid_controller::turn(true);
00183 }
00184 
00185 void DriveControl::set_wall_follower_speed() {
00186     MOTOR_BASE_SPEED  = 25;
00187 }
00188 
00189 void DriveControl::turn_around() {
00190     // TODO: Add PID Control
00191     //pid_controller::turn(TURN_AROUND);
00192     pid_controller::turn(true);
00193 }
00194 
00195 void DriveControl::stop() {
00196     leftMotor->stop();
00197     rightMotor->stop(); 
00198 }
00199 
00200 void DriveControl::resetEncoders() {
00201     leftEncoder.resetEncoders();
00202     rightEncoder.resetEncoders();
00203 }
00204 
00205 void DriveControl::drive_forward() {
00206     pid_controller::navigate();
00207     
00208 }
00209 
00210 bool DriveControl::should_stop_drive_forward() {
00211     float SHOULD_STOP = 7.0f;
00212     return (rightFrontIR.readIR() * 1000) > SHOULD_STOP;
00213 }
00214 
00215 bool DriveControl::should_finish_turn_right() {
00216     int max = 
00217         (leftEncoder.getEncoderDistance(1) < rightEncoder.getEncoderDistance(0))
00218         ? rightEncoder.getEncoderDistance(0):leftEncoder.getEncoderDistance(1);
00219     return max < - ENCODER_TURN_UNIT;
00220 }
00221 
00222 bool DriveControl::should_finish_turn_left() {
00223     int min_two = 
00224         (leftEncoder.getEncoderDistance(1) > rightEncoder.getEncoderDistance(0))
00225         ? rightEncoder.getEncoderDistance(0):leftEncoder.getEncoderDistance(1);
00226     return min_two > ENCODER_TURN_UNIT;
00227 }
00228 
00229 bool DriveControl::should_finish_drive_forward() {
00230     int max_two = 
00231         (- leftEncoder.getEncoderDistance(1) < rightEncoder.getEncoderDistance(0))
00232         ? rightEncoder.getEncoderDistance(0):- leftEncoder.getEncoderDistance(1);
00233     return max_two > 36000;
00234 }
00235 
00236 bool DriveControl::has_front_wall() {
00237 //    float RIGHT_FRONT_THRES = 355.0f;
00238 //float LEFT_FRONT_THRES = 315.0f;
00239     bool right_front_wall = (rightFrontIR.readIR() * 1000) > RIGHT_FRONT_THRES;//350.39f;
00240     bool left_front_wall = (leftFrontIR.readIR() * 1000) > LEFT_FRONT_THRES;//310.25f;
00241     return right_front_wall || left_front_wall;
00242 }
00243 
00244 bool DriveControl::has_left_wall() {
00245     return leftIR.readIR() > LEFT_SIDE_WALL_THRES;
00246 }
00247 
00248 bool DriveControl::has_right_wall() {
00249     return rightIR.readIR() > RIGHT_SIDE_WALL_THRES;
00250 }
00251 
00252 void DriveControl::clear_pid() {
00253     pid_controller::clear_pid();
00254 }