TomYumBoys / Mbed 2 deprecated MM2017

Dependencies:   mbed

Control/drivecontrol.cpp

Committer:
kolanery
Date:
2017-05-06
Revision:
0:cb667de3a336
Child:
2:619b02232144

File content as of revision 0:cb667de3a336:

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

// Constants for PID control
#define POS_TURN_CONST  0.5f
#define NEG_TURN_CONST -0.5f;
// 1024 * (40/8) * (18 / (3.1415*1.3))   
//encoder_units_per_revolution * gear_ratio * (cell_size / wheel_circumference)
#define CELL_DIST 10600;

#define PID_SAMPLE_PERIOD 0.005 // 5 ms
#define CELL_DISTANCE   10600
#define DISTANCE_CONST_P    22
#define DISTANCE_CONST_D    43
#define DISTANCE_CONST_I    10000


// Define states for debugging the mouse hardware
const int DRIVE = 1, TURN = 2, DEBUG = 3, STOP = 4;
const int SENSOR_THRESHOLD = 12;

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);
}

// 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() {
    leftMotor = NEG_TURN_CONST;
    rightMotor = POS_TURN_CONST;
    
    // 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) {
    // 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() {
    leftMotor = POS_TURN_CONST;
    rightMotor = NEG_TURN_CONST;
    
    // TODO: Add PID Control
}

void DriveControl::stop() {
    leftMotor = 0;
    rightMotor = 0;    
}

void DriveControl::drive_one_forward() {
    // TODO: Add PID Control
    //boolean stopLoop = false;
    //while (!stopLoop) {
    //}
}

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;
}