#6

/********************************************************
 ****************** CYberNeticS *************************
 *******************************************************/

#include <Wire.h>
#include <MMA8653.h>
#include <Arduino.h>
#include <Servo.h>
#include "Studuino.h"

boolean REACH_END = false;

int LEFT_SPEED = 255;
int RIGHT_SPEED = 210;
int TURNING_OFFSET = 100;

int MIDDLE_VALUE_1;
int MAX_VALUE_1 = 10000;
int MIN_VALUE_1 = 0;

int MIDDLE_VALUE_2;
int MAX_VALUE_2 = 10000;
int MIN_VALUE_2 = 0;

int SF2;
int SF3;
int SF4;
int SF5;

int SB1;
int SB2;

int TRAVELLING_STRAIGHT;

Studuino board;

/********************************************************
 ********************************************************
 ********* Setup ( Initialize all tha things ) **********
 ********************************************************
 *******************************************************/

void setup()
{
  Serial.begin(9600); // Start Serial Output
  board.InitDCMotorPort(PORT_M1); // Initialize Port M1 ( for motor )
  board.InitDCMotorPort(PORT_M2); // Initialize Port M2 ( for motor )

  board.InitSensorPort(PORT_A0, PIDPUSHSWITCH); // Initialize Port A0 ( for button )

  board.InitSensorPort(PORT_A6, PIDIRPHOTOREFLECTOR); // Initialize Port A6 ( for IR sensor )
  board.InitSensorPort(PORT_A7, PIDIRPHOTOREFLECTOR); // Initialize Port A7 ( for IR sensor )
}

/********************************************************
 ********************************************************
 ******** Functions ( By alphabetical Order ) ***********
 ********************************************************
 *******************************************************/

void calibrateSensors()
{
  for (int i = 0; i < 30; i++)
  {
    if (i >= 10 && i < 30)
    {
      spinRight();
      calibrateSensorValues();
    }
    else
    {
      spinLeft();
    }
    delay(100);
  }

  while ( SF4 == 0 )
  {
    spinLeft();
    getSensorValues();
  }

  moveForward(LEFT_SPEED,RIGHT_SPEED);
  delay(500);
}

//*****************************************************

void calibrateSensorValues()
{
  int a = board.GetIRPhotoreflectorValue(PORT_A7);
  int b = analogRead(A2);

  if ( a < MAX_VALUE_2 )
  {
    MAX_VALUE_1 = a;
  }
  if ( a > MIN_VALUE_2 )
  {
    MIN_VALUE_1 = a;
  }

  if ( b < MAX_VALUE_2 )
  {
    MAX_VALUE_2 = b;
  }
  if ( b > MIN_VALUE_2 )
  {
    MIN_VALUE_2 = b;
  }

  MIDDLE_VALUE_1 = ( MIN_VALUE_1 + MAX_VALUE_1 ) / 2;
  MIDDLE_VALUE_2 = ( MIN_VALUE_2 + MAX_VALUE_2 ) / 2;
  Serial.println(MIDDLE_VALUE_1);
  Serial.println(MIDDLE_VALUE_2);
}

//*****************************************************

void countdown()
{
  delay(3000); // Temporally solution until we get a LED
}

//*****************************************************

void moveForward(int leftSpeed, int rightSpeed)
{
  board.DCMotorPower(PORT_M1, leftSpeed);
  board.DCMotorControl(PORT_M1, REVERSE);

  board.DCMotorPower(PORT_M2, rightSpeed);
  board.DCMotorControl(PORT_M2, REVERSE);
}

//*****************************************************

void finishingMove()
{

}

//*****************************************************

void getSensorValues()
{

  if ( board.GetIRPhotoreflectorValue(PORT_A6) < MIDDLE_VALUE_1 )
  {
    SB1 = 1;
  }
  else
  {
    SB1 = 0;
  }

  if ( board.GetIRPhotoreflectorValue(PORT_A7) < MIDDLE_VALUE_1 )
  {
    SB2 = 1;
  }
  else
  {
    SB2 = 0;
  }

  if ( analogRead(A2) > MIDDLE_VALUE_2 )
  {
    SF2 = 1;
  }
  else
  {
    SF2 = 0;
  }

  if ( analogRead(A3) > MIDDLE_VALUE_2 )
  {
    SF3 = 1;
  }
  else
  {
    SF3 = 0;
  }

  if ( analogRead(A4) > MIDDLE_VALUE_2 )
  {
    SF4 = 1;
  }
  else
  {
    SF4 = 0;
  }

  if ( analogRead(A5) > MIDDLE_VALUE_2 )
  {
    SF5 = 1;
  }
  else
  {
    SF5 = 0;
  }
}

//*****************************************************

void solveMaze()
{
  while(REACH_END == false)
  {
    getSensorValues();

    /*** Stabalizer ***/
    if ((SF2 == 1 || SF5 == 1) && SF3 == 1 && SF4 == 1 && SB1 == 0 && SB2 == 0) // If robot is straight
    {
      moveForward(LEFT_SPEED,RIGHT_SPEED);
      TRAVELLING_STRAIGHT++;
    }

    if (SF5 == 0 && SF2 == 1 && SB1 == 0 && SB2 == 0) // If robot is slightly to the left
    {
      moveForward(LEFT_SPEED , RIGHT_SPEED - TURNING_OFFSET);
    }

    if (SF5 == 1 && SF2 == 0 && SB1 == 0 && SB2 == 0) // If robot is slightly to the left
    {
      moveForward(LEFT_SPEED - TURNING_OFFSET , RIGHT_SPEED);
    }

    if (TRAVELLING_STRAIGHT > 2000 && SF2 == 0 && SF3 == 0 && SF4 == 0 && SF5 == 0 && SB1 == 0 && SB2 == 0)
    {
      delay(100);
      getSensorValues();
      if (SF2 == 0 && SF3 == 0 && SF4 == 0 && SF5 == 0 && SB1 == 0 && SB2 == 0)
      {
        moveForward(0,0);
        delay(100000);
      }
    }


    if ( SB1 == 1 || SB2 == 1 )
    {
      delay(200);
      getSensorValues();
      TRAVELLING_STRAIGHT == 0;

      if (SB1 == 1 && SB2 == 1)
      {
        if (SF5 == 1 || SF4 == 1 || SF3 == 1 || SF2 == 1) // If Robot detects it as + junction
        {
          spinLeft();
          getSensorValues();
          while (SF2 == 0)
          {
            spinLeft();
            getSensorValues();
          }
        }

        if (SF5 == 0 || SF4 == 0 || SF3 == 0 || SF2 == 0) // If robot detects it as upright T junction
        {
          spinLeft();
          getSensorValues();
          delay(1000);
          while (SF2 == 0)
          {
            spinLeft();
            getSensorValues();
          }
        }
      }



      if (SB1 == 1 && SB2 == 0)
      {
        if (SF5 == 0 || SF4 == 0 || SF3 == 0 || SF2 == 0) // If robot detects it as left turn
        {
          spinLeft();
          getSensorValues();
          while (SF2 == 0)
          {
            spinLeft();
            getSensorValues();
          }
        }

        if (SF5 == 1 || SF4 == 1 || SF3 == 1 || SF2 == 1) // If robot detects it as T junction ( rotated clockwise )
        {
          spinLeft();
          getSensorValues();
          while (SF2 == 0)
          {
            spinLeft();
            getSensorValues();
          }
        }
      }

      if (SB1 == 0 && SB2 == 1)
      {
        if (SF5 == 1 || SF4 == 1 || SF3 == 1 || SF2 == 1) // If robot detects it as T junction ( rotated counterclockwise )
        {
          delay(500);
        }

        if (SF5 == 0 || SF4 == 0 || SF3 == 0 || SF2 == 0) // If robot detects it as right turn
        {
          spinRight();
          getSensorValues();
          while (SF5 == 0)
          {
            spinRight();
            getSensorValues();
          }
        }
      }
    }
  }
}



//*****************************************************

void showRawSensorValues()
{
  Serial.println(analogRead(A2));
  Serial.println(analogRead(A3));
  Serial.println(analogRead(A4));
  Serial.println(analogRead(A5));
  Serial.println("");
  Serial.println(board.GetIRPhotoreflectorValue(PORT_A6));
  Serial.println(board.GetIRPhotoreflectorValue(PORT_A7));
  Serial.println("");
  delay(500);
}


//*****************************************************

void spinLeft()
{
  board.DCMotorPower(PORT_M1, LEFT_SPEED);
  board.DCMotorControl(PORT_M1, NORMAL);

  board.DCMotorPower(PORT_M2, RIGHT_SPEED);
  board.DCMotorControl(PORT_M2, REVERSE);

}

//*****************************************************

void spinRight()
{
  board.DCMotorPower(PORT_M1, LEFT_SPEED);
  board.DCMotorControl(PORT_M1, REVERSE);

  board.DCMotorPower(PORT_M2, RIGHT_SPEED);
  board.DCMotorControl(PORT_M2, NORMAL);

}

/********************************************************
 ********************************************************
 ********** Loop ( Where the fun begins ) ***************
 ********************************************************
 *******************************************************/

void loop()
{
  int A0 = board.GetPushSwitchValue(PORT_A0);

  if ( A0 == 0 )
  {
    countdown();
    calibrateSensors();
    solveMaze();
    finishingMove();
  }
}