Project_SENILA firmware

/* ========================
  TriniX Development
  Project: Mr Senila
  Author: coldzero
=========================== */

#include <SoftwareSerial.h>
#include <Servo.h>

#define btRxPin 10 //recv pin
#define btTxPin 11 //transmit pin

#define MOTOR1_PIN1 3
#define MOTOR1_PIN2 5
#define MOTOR2_PIN1 6
#define MOTOR2_PIN2 9

#define SERVO_PIN 7

#define TRIGER_PIN 12
#define ECHO_PIN 13

//RX pin = receive pin, TX pin = transmit pin
SoftwareSerial BlueTooth(btRxPin, btTxPin);

//Sensor servo stuff
Servo SensorServo;
int SensorServoPos = 0;
int iDirection = 1; //1 - Right, 2 - Left
int MaxAngle = 150;
int MinAngle = 0;
int ServoStep = 10;
int iCurAngle = 0;

//Sensor stuff
int iTime = 0;
int iDistance = 0;
int iDistanceMap[3];
int iCriticalDistance = 20;

boolean gotMission = false;

void setup()
{
  Serial.begin(9600);

  BlueTooth.begin(9600);

  pinMode(MOTOR1_PIN1, OUTPUT);
  pinMode(MOTOR1_PIN2, OUTPUT);
  pinMode(MOTOR2_PIN1, OUTPUT);
  pinMode(MOTOR2_PIN2, OUTPUT);

  SensorServo.attach(SERVO_PIN);
  SensorServo.write(80);

  pinMode(TRIGER_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);

  gotMission = true;
}

void loop()
{
  if (gotMission)
  {
    /*iCurAngle = 0;
    for (int i=0; i<3; i++)
    {
      SensorServo.write(iCurAngle);
      if (iCurAngle == 0) //wait for the servo to turn
        delay(1200);
      else
        delay(800);
      //Sensor stuff first!!!
      digitalWrite(TRIGER_PIN, LOW); //to ensure a clean TRIGER
      delayMicroseconds(2);
      digitalWrite(TRIGER_PIN, HIGH);
      delayMicroseconds(5);
      digitalWrite(TRIGER_PIN, LOW);

      iTime = pulseIn(ECHO_PIN, HIGH);
      iDistance = (iTime/2) / 29.1;
      iDistanceMap[i] = iDistance;

      //For DEBUGING
      Serial.print(iDistance);
      Serial.println(" cm");
      iCurAngle += 90;
    }*/

    iDistanceMap[1] = GetDistanceFrom(80);

    if (iDistanceMap[1] <= iCriticalDistance && iDistanceMap[1] >= 0) //critical distance
    {
      StopTank(); //STOP IT OR YOU WILL DIE!
      iDistanceMap[0] = GetDistanceFrom(0);
      if (iDistanceMap[0] < 0)
        iDistanceMap[0] = 200;
      iDistanceMap[2] = GetDistanceFrom(180);
      if (iDistanceMap[2] < 0)
        iDistanceMap[2] = 200;

      if (iDistanceMap[0] > iDistanceMap[2])
      {
        TurnForLeft(80);
        delay(2000);
        StopTank();
      }
      else
      {
        TurnForRight(80);
        delay(2000);
        StopTank();
      }
    }
    else
    {
      MoveForward(80);
    }

    //MoveForward(100);

    //BlueTooth.listen();
    if (BlueTooth.available() > 0)
    {
      char inBtByte = BlueTooth.read();
      if (inBtByte == 1) //UP
      {
        BlueTooth.write("UP");
        Move(255, 255);
      }
      else if (inBtByte == 2) //DOWN
      {
        BlueTooth.write("DOWN");
        Move(-255, -255);
      }
      else if (inBtByte == 3) //LEFT
      {
        BlueTooth.write("LEFT");
        Move(-255,255);
      }
      else if (inBtByte == 4) //RIGHT
      {
        BlueTooth.write("RIGHT");
        Move(255,-255);
      }
      else if (inBtByte == 5)
      {
        BlueTooth.write("STOP");
        Move(0,0);
      }
    }
    delay(100);
  }
}

int GetDistanceFrom(int iAngle)
{
  int itempDistance, itempTime;

  SensorServo.write(iAngle);
  delay(800);
  //Sensor stuff first!!!
  digitalWrite(TRIGER_PIN, LOW); //to ensure a clean TRIGER
  delayMicroseconds(2);
  digitalWrite(TRIGER_PIN, HIGH);
  delayMicroseconds(5);
  digitalWrite(TRIGER_PIN, LOW);

  itempTime = pulseIn(ECHO_PIN, HIGH);
  itempDistance = (itempTime/2) / 29.1;

  //For DEBUGING
  Serial.print(itempDistance);
  Serial.println(" cm");

  return itempDistance;
}

void StopTank()
{
  analogWrite(MOTOR1_PIN1, 0);
  analogWrite(MOTOR1_PIN2, 0);

  analogWrite(MOTOR2_PIN1, 0);
  analogWrite(MOTOR2_PIN2, 0);
}

void MoveForward(int MotorSpeed)
{
  analogWrite(MOTOR1_PIN1, MotorSpeed);
  analogWrite(MOTOR1_PIN2, 0);

  analogWrite(MOTOR2_PIN1, MotorSpeed);
  analogWrite(MOTOR2_PIN2, 0);
}

void MoveBackward(int MotorSpeed)
{
  analogWrite(MOTOR1_PIN1, 0);
  analogWrite(MOTOR1_PIN2, -MotorSpeed);

  analogWrite(MOTOR2_PIN1, 0);
  analogWrite(MOTOR2_PIN2, -MotorSpeed);
}

void TurnForLeft(int MotorSpeed)
{
  analogWrite(MOTOR1_PIN1, MotorSpeed);
  analogWrite(MOTOR1_PIN2, 0);

  analogWrite(MOTOR2_PIN1, 0);
  analogWrite(MOTOR2_PIN2, 0);
}

void TurnForRight(int MotorSpeed)
{
  analogWrite(MOTOR1_PIN1, 0);
  analogWrite(MOTOR1_PIN2, 0);

  analogWrite(MOTOR2_PIN1, MotorSpeed);
  analogWrite(MOTOR2_PIN2, 0);
}

void TurnBackLeft(int MotorSpeed)
{
  analogWrite(MOTOR1_PIN1, 0);
  analogWrite(MOTOR1_PIN2, -MotorSpeed);

  analogWrite(MOTOR2_PIN1, 0);
  analogWrite(MOTOR2_PIN2, 0);
}

void TurnBackRight(int MotorSpeed)
{
  analogWrite(MOTOR1_PIN1, 0);
  analogWrite(MOTOR1_PIN2, 0);

  analogWrite(MOTOR2_PIN1, 0);
  analogWrite(MOTOR2_PIN2, -MotorSpeed);
}

void Move(int speedLeft, int speedRight)
{
  if (speedLeft >= 0)
  {
    analogWrite(MOTOR1_PIN1, speedLeft);
    analogWrite(MOTOR1_PIN2, 0);
  }
  else
  {
    analogWrite(MOTOR1_PIN1, 0);
    analogWrite(MOTOR1_PIN2, -speedLeft);
  }

  if (speedRight >= 0)
  {
    analogWrite(MOTOR2_PIN1, speedRight);
    analogWrite(MOTOR2_PIN2, 0);
  }
  else
  {
    analogWrite(MOTOR2_PIN1, 0);
    analogWrite(MOTOR2_PIN2, -speedRight);
  }
}