SERA.ino

/************************************************************************************
                          Library file includes
 ***********************************************************************************/
#include <Wire.h>
#include <Servo.h>
#include "HMC5883L.h"

HMC5883L compass;
/************************************************************************************
                            Pin Definitions
 ***********************************************************************************/
#define trigPin 33  
#define echoPin 32  

/************************************************************************************
                           State Definitions
 ***********************************************************************************/
#define HALT    0
#define FORWARD 1
#define LEFT    2
#define RIGHT   3

#define GRAB    9
#define ROTATE  10

/************************************************************************************
                          Global Variables
 ***********************************************************************************/
Servo grip_grab;
Servo grip_rotate;
Servo camera;
Servo cameraPan;

int leftSpeed = 65;
int rightSpeed = 65;
int driveStatus = HALT;

/************************************************************************************
                    Setup
 ***********************************************************************************/
void setup(){
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  
  pinMode(4, OUTPUT);
  pinMode(5, OUTPUT);
  pinMode(6, OUTPUT);
  pinMode(7, OUTPUT);
  
  halt();

  attachInterrupt(4, line, HIGH);
  attachInterrupt(5, line, HIGH);
  
  Serial.begin(9600);
  compass.begin();
  compass.setOffset(-180, -160);
}

void loop(){
  if(Serial.available()){
    char input  = Serial.read();
      switch (input){
        //Drive
        case 'w': forward();  break;
        case 'a': turn_L();   break;
        case 'd': turn_R();   break;
        case 's': halt();     break;
        case 'x': backward(); break;
  
        case 'r': turn_90l(); break;
        case 't': turn_90r(); break;
        
        case 'n': turn_ll(); break;
        case 'm': turn_lr(); break;
        
        case 'z': speedChange(); break;

        //Camera
        case 'c': pan(); break;
        case 'v': tilt(); break;

        //Gripper
        case 'p': grip(); break;
        case 'l': leave(); break;
        case 'i': InitGripper(); break;

        
        case 'g': compassCal(); break;

      }
   }
}

/************************************************************************************
                                  Gripper
 ***********************************************************************************/
void grip()
{
  float dis;
  
  while(1)
  {
    dis = IR();
    if (dis < 17)
    {
      Pick();
      break;
    }else{
      forward();
      delay(50);
      halt();
    }
  }
  Serial.print("1");
  return;
}

void leave()
{
  float dis;
  
  while(1)
  {
    dis = IR();
    if (dis < 17.5)
    {
      Drop();
      break;
    }else{
      forward();
      delay(50);
      halt();
    }
  }
  
  Serial.print("1");
  return;
}

void Pick()
{
  grip_grab.attach(GRAB);
  grip_rotate.attach(ROTATE);

  delay(2000);
  grip_grab.write(0);
  delay(2000);
  grip_rotate.write(100);

  delay(1000);
  grip_rotate.detach();
}

void Drop()
{
  grip_grab.attach(GRAB);
  grip_rotate.attach(ROTATE);
  
  grip_grab.write(150);
  delay(2000);
  
  grip_grab.detach();
  grip_rotate.detach();
}

void InitGripper()
{
  grip_grab.attach(GRAB);
  grip_rotate.attach(ROTATE);

  delay(1000);
  
  grip_grab.write(150);
  delay(1000);
  grip_rotate.write(0);
  delay(1000);
  
  grip_grab.detach();
  grip_rotate.detach();
  
  Serial.print("1");
}


/************************************************************************************
                                    Drive
 ***********************************************************************************/
void forward()
{
  driveStatus = FORWARD;
  digitalWrite(4,LOW);
  digitalWrite(5,HIGH);
  analogWrite(6,leftSpeed);
  analogWrite(7,rightSpeed);
}

void backward()
{
  driveStatus = FORWARD;
  digitalWrite(4,HIGH);
  digitalWrite(5,LOW);
  analogWrite(6,rightSpeed);
  analogWrite(7,leftSpeed);
}

void halt()
{
  driveStatus = HALT;
  digitalWrite(4,LOW);
  digitalWrite(5,HIGH);
  analogWrite(6,0);
  analogWrite(7,0);
}

void turn_L() 
{
  driveStatus = LEFT;
  digitalWrite(4,HIGH);
  digitalWrite(5,HIGH);
  analogWrite (6,90);
  analogWrite (7,90);
}

void turn_R()
{
  driveStatus = RIGHT;
  digitalWrite(4,LOW);
  digitalWrite(5,LOW);
  analogWrite (6,90);
  analogWrite (7,90);
}

void turn_ll()
{
  turn_L();
  delay(30);
  halt();
}

void turn_lr()
{
  turn_R();
  delay(30);
  halt();
}

void turn_90l(){
  int degree;

  //90 Degree Turn
  //int destl = compassValue() + 89;
  //int desth = compassValue() + 91;
  
  //45 Degree Turn
  int destl = compassValue() + 43;
  int desth = compassValue() + 47;
  
  if(destl > 360) destl -= 360;
  if(desth > 360) desth -= 360;
  
  do{
    turn_L();
    delay(20);
    halt();
    delay(10);
    degree = compassValue();
    degree = compassValue();
    degree = compassValue();
    //Serial.println("*");
  }while((degree < destl) | (degree > desth));
  
  halt();  
  Serial.print("1");
}

void turn_90r(){
  int degree;
  
  //90 Degree Turn
  //int destl = compassValue() - 91;
  //int desth = compassValue() - 89;
  
  //45 Degree Turn
  int destl = compassValue() - 47;
  int desth = compassValue() - 43;

  if(destl < 0) destl += 360;
  if(desth < 0) desth += 360;
  
  do{
    turn_R();
    delay(20);
    halt();
    delay(10);
    degree = compassValue();
    degree = compassValue();
    degree = compassValue();
    //Serial.println("*");
  }while((degree < destl) | (degree > desth));
  
  halt();  
  Serial.print("1");
}


/************************************************************************************
                                    Camera
 ***********************************************************************************/
void tilt(){
  int value;
  cameraPan.attach(12);
  value = Serial.parseInt();
  cameraPan.write(value);  
  delay(1000);
  cameraPan.detach();
}

void pan(){
  int value;
  camera.attach(11);
  value = Serial.parseInt();
  camera.write(value);
  delay(1000);
  camera.detach();
}

/************************************************************************************
                                    Line Sensor
 ***********************************************************************************/
void line(){
  if(driveStatus == FORWARD){
    halt();
    Serial.print(1);
  }
}

/************************************************************************************
                                    Compass
 ***********************************************************************************/
int compassValue(){
  int angle;
  
  Vector norm = compass.readNormalize();

  float heading = atan2(norm.YAxis, norm.XAxis);
  float declinationAngle = (-0 - (8 / 60.0)) / (180 / M_PI);
  heading += declinationAngle;

  if (heading < 0)
  {
    heading += 2 * PI;
  }

  if (heading > 2 * PI)
  {
    heading -= 2 * PI;
  }

  angle = heading * 180 / M_PI;

  return angle;
}

void compassCal()
{
  int minX = 0;
  int maxX = 0;
  int minY = 0;
  int maxY = 0;
  int offX = 0;
  int offY = 0;

  compass.setOffset(0, 0);

  turn_R();
  
  for(int i = 0; i <3000; i++)
  {
    Vector mag = compass.readRaw();

    if (mag.XAxis < minX) minX = mag.XAxis;
    if (mag.XAxis > maxX) maxX = mag.XAxis;
    if (mag.YAxis < minY) minY = mag.YAxis;
    if (mag.YAxis > maxY) maxY = mag.YAxis;
  
    offX = (maxX + minX)/2;
    offY = (maxY + minY)/2;
  
   }

  halt();

  Serial.println(offX);
  Serial.println(offY);
  compass.setOffset(offX, offY);

  
}

void speedChange(){
  int value;
  
  value = Serial.parseInt();
  if(value < 250 & value > 50)
    leftSpeed = value;

  value = Serial.parseInt();
  if(value < 250 & value > 50)
    rightSpeed = value;

  if(driveStatus == FORWARD)
    forward();
}

float IR(){
  float value = 0;
  float p_value = 0;
  
  do
  {
    p_value = get_IR();
    value = get_IR(); 
  }while((value > p_value + 0.5) | (value < p_value - 0.5));
  
  return value;
}


float get_IR()
{
  float SumIRMid = 0.0;
  float IRvalueM[10];
  
  for (int i = 0; i < 20; i++) {
    float voltsM = analogRead(0) * 0.0048828125; 
    IRvalueM[i] = 50 * pow(voltsM, -1.10); 
    SumIRMid = SumIRMid + IRvalueM[i];
  }

  return SumIRMid/20;
}