File content as of revision 3:4230b82fde43:

#include "drivecontrol.h"
#include "Cell.h"
#include "ir_sensor.h"
#include "left_motor.h"
#include "right_motor.h"
//#include "pidconstants.h"



// Define states for debugging the mouse hardware
const int DRIVE = 1, TURN = 2, DEBUG = 3, STOP = 4;
const int SENSOR_THRESHOLD = 12;
LeftMotor * leftMotor;
RightMotor * rightMotor;
Cell * curr_cell;

// Currently only have the x, y position fields for
// each cell.
DriveControl::DriveControl (int start_x, int start_y) {
    curr_cell = new Cell (start_x, start_y);
    leftMotor= new LeftMotor();
    rightMotor = new RightMotor();
 //   leftMotor->speed(0.9f);
//    rightMotor->speed(0.9f);
}

// Defines the next cell to traverse.
Cell * next_cell() {
    // cell should get the reference from the Algorithm class.
    // Cell * cell;
    return curr_cell;
}

void DriveControl::turn_left() {
    
    // TODO: Add PID Control
}

int DriveControl::get_next_direction() {
    // TODO: Define the direction based on heuristic eval. 
    return 1;   
}

int DriveControl::get_next_state(int state) {
    // Simply drives the mouse for testing
    return DRIVE;
    
    /*
    // Front wall threshold is set to 12
    if (this->DriveControl::has_front_wall()) {
        return DRIVE;
    }
    
    if (!has_right_wall() || !has_left_wall()) {
     //   return TURN;
    }
    // Add Another Check for abnormal state
    return DEBUG;   */ 
}

void DriveControl::turn_right() {
    
    // TODO: Add PID Control
}

void DriveControl::stop() {
    leftMotor->stop();
    rightMotor->stop(); 
       
}

void DriveControl::drive_one_forward() {
    // TODO: Add PID Control
    //boolean stopLoop = false;
    //while (!stopLoop) {
    //}
    //int curr_dist = 0;
    //while (curr_dist != 5) {
        leftMotor->speed(0.15f);
        rightMotor->speed(0.85f);
    //    curr_dist ++;
    //}
    //curr_dist = 0;
}

bool DriveControl::has_front_wall() {
    return rightFrontIR < SENSOR_THRESHOLD && leftFrontIR < SENSOR_THRESHOLD;
}

bool DriveControl::has_left_wall() {
    return leftIR < SENSOR_THRESHOLD;
}

bool DriveControl::has_right_wall() {
    return rightIR < SENSOR_THRESHOLD;
}