File content as of revision 38:fe05f93009a2:

#ifndef MAIN_H
#define MAIN_H

#include "mbed.h"
#include "ITG3200.h"
#include "motor.h"
#include "QEI.h"
#include <stack>          // std::stack
#include <utility>      // std::pair, std::make_pair

#define PULSES 3520
#define SAMPLE_NUM 40
#define WHEEL_SPEED 0.10

// Motors
/*
PwmOut left1(PB_7);
PwmOut left2(PB_8);
PwmOut right1(PA_10);
PwmOut right2(PA_11);

DigitalOut enableLeftMotor(PB_4);
DigitalOut enableRightMotor(PB_5);
*/

// RGB LED
DigitalOut redLed(PC_0);
DigitalOut blueLed(PC_1);
DigitalOut greenLed(PC_2);

// IRPairs
IRPair IRP_4( PB_13, PC_4 );         // swapped 4 and 3 here so that we do not have to flip it everywhere else
IRPair IRP_3( PB_1, PC_5);
IRPair IRP_2( PB_14, PA_7 );         // swapped 2 and 1 here so we do not have to flip it everywhere else!
IRPair IRP_1( PB_0, PA_6 );

Motor left_motor( PB_8, PB_7, PB_4 ); // forward, backwards, enable
Motor right_motor( PA_11, PA_10, PB_5 ); // forward, backwards, enable

/*
DigitalOut IR_1(PB_1);
DigitalOut IR_2(PB_13);
DigitalOut IR_3(PB_0);
DigitalOut IR_4(PB_14);
// Receivers
AnalogIn Rec_1(PC_5);
AnalogIn Rec_2(PC_4);
AnalogIn Rec_3(PA_6);
AnalogIn Rec_4(PA_7);
*/

// Doing DEBUGGING
#define DEBUGGING 0
Serial serial(PC_6, PC_7);

// Gyro
ITG3200 gyro(PC_9, PA_8);

volatile double reading = 0;

int gyroX = 0;
int gyroY = 0;
int gyroZ = 0;

InterruptIn dipButton1(PB_15);
InterruptIn dipButton2(PB_10);
InterruptIn dipButton3(PB_9);
InterruptIn dipButton4(PB_12);

void enableButton1();
void enableButton2();
void enableButton3();
void enableButton4();
void disableButton1();
void disableButton2();
void disableButton3();
void disableButton4();

bool isWallInFront(int x, int y);
bool isWallInBack(int x, int y);
bool isWallOnRight(int x, int y);
bool isWallOnLeft(int x, int y);

int chooseNextMovement();
void changeManhattanDistance(bool headCenter);
bool hasVisited(int x, int y);

volatile int dipFlags = 0;
#define BUTTON1_FLAG 0x1
#define BUTTON2_FLAG 0x2
#define BUTTON3_FLAG 0x4
#define BUTTON4_FLAG 0x8

int turnFlag = 0;
#define LEFT_FLAG 0x1
#define RIGHT_FLAG 0x2

QEI encoder0( PA_5, PB_3, NC, PULSES, QEI::X4_ENCODING );
QEI encoder1( PA_1, PA_0, NC, PULSES, QEI::X4_ENCODING );

#define F_WALL 0x1
#define L_WALL 0x2
#define R_WALL 0x4
#define B_WALL 0x8

#define MAZE_LEN 16

int mouseX = 0;
int mouseY = 0;
bool justTurned = false;
bool goingToCenter = true;

stack< pair<int, int> > cellsToVisit;

int currDir = 100;              // modulo this to keep track of the current direction of the mouse!
// 0 = forward, 1 = right, 2 = down, 3 = left
int wallArray[16][16] = {0};    // array to keep track of the walls
int visitedCells[16][16] = {0};     // array to keep track of the mouse's current location
int manhattanDistances[16][16] = {
                                  {14, 13, 12, 11, 10, 9, 8, 7, 7, 8, 9, 10, 11, 12, 13, 14},
                                  {13, 12, 11, 10, 9, 8, 7, 6, 6, 7, 8, 9, 10, 11, 12, 13},
                                  {12, 11, 10, 9, 8, 7, 6, 5, 5, 6, 7, 8, 9, 10, 11, 12},
                                  {11, 10, 9, 8, 7, 6, 5, 4, 4, 5, 6, 7, 8, 9, 10, 11},
                                  {10, 9, 8, 7, 6, 5, 4, 3, 3, 4, 5, 6, 7, 8, 9, 10},
                                  {9, 8, 7, 6, 5, 4, 3, 2, 2, 3, 4, 5, 6, 7, 8, 9},
                                  {8, 7, 6, 5, 4, 3, 2, 1, 1, 2, 3, 4, 5, 6, 7, 8},
                                  {7, 6, 5, 4, 3, 2, 1, 0, 0, 1, 2, 3, 4, 5, 6, 7},
                                  {7, 6, 5, 4, 3, 2, 1, 0, 0, 1, 2, 3, 4, 5, 6, 7},
                                  {8, 7, 6, 5, 4, 3, 2, 1, 1, 2, 3, 4, 5, 6, 7, 8},
                                  {9, 8, 7, 6, 5, 4, 3, 2, 2, 3, 4, 5, 6, 7, 8, 9},
                                  {10, 9, 8, 7, 6, 5, 4, 3, 3, 4, 5, 6, 7, 8, 9, 10},
                                  {11, 10, 9, 8, 7, 6, 5, 4, 4, 5, 6, 7, 8, 9, 10, 11},
                                  {12, 11, 10, 9, 8, 7, 6, 5, 5, 6, 7, 8, 9, 10, 11, 12},
                                  {13, 12, 11, 10, 9, 8, 7, 6, 6, 7, 8, 9, 10, 11, 12, 13},
                                  {14, 13, 12, 11, 10, 9, 8, 7, 7, 8, 9, 10, 11, 12, 13, 14},
                          };

int distanceToCenter[16][16] = {
                                  {14, 13, 12, 11, 10, 9, 8, 7, 7, 8, 9, 10, 11, 12, 13, 14},
                                  {13, 12, 11, 10, 9, 8, 7, 6, 6, 7, 8, 9, 10, 11, 12, 13},
                                  {12, 11, 10, 9, 8, 7, 6, 5, 5, 6, 7, 8, 9, 10, 11, 12},
                                  {11, 10, 9, 8, 7, 6, 5, 4, 4, 5, 6, 7, 8, 9, 10, 11},
                                  {10, 9, 8, 7, 6, 5, 4, 3, 3, 4, 5, 6, 7, 8, 9, 10},
                                  {9, 8, 7, 6, 5, 4, 3, 2, 2, 3, 4, 5, 6, 7, 8, 9},
                                  {8, 7, 6, 5, 4, 3, 2, 1, 1, 2, 3, 4, 5, 6, 7, 8},
                                  {7, 6, 5, 4, 3, 2, 1, 0, 0, 1, 2, 3, 4, 5, 6, 7},
                                  {7, 6, 5, 4, 3, 2, 1, 0, 0, 1, 2, 3, 4, 5, 6, 7},
                                  {8, 7, 6, 5, 4, 3, 2, 1, 1, 2, 3, 4, 5, 6, 7, 8},
                                  {9, 8, 7, 6, 5, 4, 3, 2, 2, 3, 4, 5, 6, 7, 8, 9},
                                  {10, 9, 8, 7, 6, 5, 4, 3, 3, 4, 5, 6, 7, 8, 9, 10},
                                  {11, 10, 9, 8, 7, 6, 5, 4, 4, 5, 6, 7, 8, 9, 10, 11},
                                  {12, 11, 10, 9, 8, 7, 6, 5, 5, 6, 7, 8, 9, 10, 11, 12},
                                  {13, 12, 11, 10, 9, 8, 7, 6, 6, 7, 8, 9, 10, 11, 12, 13},
                                  {14, 13, 12, 11, 10, 9, 8, 7, 7, 8, 9, 10, 11, 12, 13, 14},
                          };

int distanceToStart[16][16] = {0};
                          
    
                          
/* Constants for when HIGH_PWM_VOLTAGE = 0.2
#define IP_CONSTANT 6
#define II_CONSTANT 0
#define ID_CONSTANT 1
*/
 
// Constants for when HIGH_PWM_VOLTAGE = 0.1
// #define IP_CONSTANT 8.85
// #define II_CONSTANT 0.005
// #define ID_CONSTANT 3.15
//#define IP_CONSTANT 8.2
//#define II_CONSTANT 0.06
//#define ID_CONSTANT 7.55

const int desiredCount180 = 3380;   // change accordingly to the terrain
const int desiredCountR = 1600;
const int desiredCountL = 1600;
 
const int oneCellCount = 5400;
const int oneCellCountMomentum = 5070;//4570 (.15) speed;//4800;      // one cell count is actually approximately 5400, but this value is considering momentum!
 
double receiverOneReading = 0.0;
double receiverTwoReading = 0.0;
double receiverThreeReading = 0.0;
double receiverFourReading = 0.0;

float ir1base = 0.0;
float ir2base = 0.0;
float ir3base = 0.0;
float ir4base = 0.0;

float averageDivUpper = 0.5;
                          
inline void turnLeft()
{
    double speed0 = 0.11;
    double speed1 = -0.12;    // change back to 0.13 if turns stop working, testing something out!
 
    double kp = 0.000082;
 
    int counter = 0;
    int initial0 = encoder0.getPulses();
    int initial1 = encoder1.getPulses();
 
    int desiredCount0 = initial0 - desiredCountL; // left wheel
    int desiredCount1 = initial1 + desiredCountL; // right wheel
 
    int count0 = initial0;
    int count1 = initial1;
 
    double error0 = desiredCount0 - count0; // is negative
    double error1 = desiredCount1 - count1; // is positive
 
    while(1) {
 
        if(!(abs(error0) < 3) && !(abs(error1) < 3)) {
            count0 = encoder0.getPulses();
            count1 = encoder1.getPulses();
            
            error0 = desiredCount0 - count0; // is negative
            error1 = desiredCount1 - count1; // is positive
  
            right_motor.move(error1*kp);
            left_motor.move(error0*kp);
            counter = 0;
        } else {
            counter++;
            right_motor.brake();
            left_motor.brake();
        }
        if (counter > 60) {
            break;
        }
    }
 
    right_motor.brake();
    left_motor.brake();
    turnFlag = 0;           // zeroing out the flags!
    currDir -= 1;
}


inline void turnRight()
{
    double speed0 = -0.11;
    double speed1 = 0.12;     // change back to 0.13 if turns stop working, testing something out!
 
    double kp = 0.00009;
 
    int counter = 0;
    int initial0 = encoder0.getPulses();
    int initial1 = encoder1.getPulses();
 
    int desiredCount0 = initial0 + desiredCountR; // left wheel
    int desiredCount1 = initial1 - desiredCountR; // right wheel
 
    int count0 = initial0;
    int count1 = initial1;
 
    double error0 = desiredCount0 - count0; // is positive
    double error1 = desiredCount1 - count1; // is negative
 
    while(1) {
 
        if(!(abs(error0) < 3) && !(abs(error1) < 3)) {
            count0 = encoder0.getPulses();
            count1 = encoder1.getPulses();
 
            error0 = desiredCount0 - count0; // is positive
            error1 = desiredCount1 - count1; // is negative
 
            right_motor.move(error1*kp);
            left_motor.move(error0*kp);
            counter = 0;
        } else {
            counter++;
            right_motor.brake();
            left_motor.brake();
        }
        if (counter > 60) {
            break;
        }
    }
 
    right_motor.brake();
    left_motor.brake();
    turnFlag = 0;
    currDir += 1;
}

inline void turn180()
{
    double speed0 = -0.10;
    double speed1 = 0.11;
    
    double kp = 0.000055;
 
    int counter = 0;
    int initial0 = encoder0.getPulses();
    int initial1 = encoder1.getPulses();
 
    int desiredCount0 = initial0 + desiredCount180;
    int desiredCount1 = initial1 - desiredCount180;
 
    int count0 = initial0;
    int count1 = initial1;
 
    double error0 = count0 - desiredCount0;
    double error1 = count1 - desiredCount1;
 
    while(1) {
 
        if(!(abs(error0) < 3) && !(abs(error1) < 3)) {
            count0 = encoder0.getPulses();
            count1 = encoder1.getPulses();
 
            error0 = desiredCount0 - count0;
            error1 = desiredCount1 - count1;
 
            right_motor.move(error1*kp);
            left_motor.move(error0*kp);
            counter = 0;
        } else {
            counter++;
            right_motor.brake();
            left_motor.brake();
        }
        if (counter > 60) {
            break;
        }
    }
    right_motor.brake();
    left_motor.brake();
    currDir += 2;
}

#endif