HMC5883_example_code_2_0

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

* Copyright(C) : National Central University ROD Lab Jen Hao Chen (Howard)                                                   
* Create Date  : 2014/07/28 by Howard Chen                                                           
* Modified Date: 2014/09/27 by Howard Chen                                                          
* Abstract     : This program will let the robot adjust the orientation automatically. 
                    HMC5883 is an IC for detecting the orientation. We use the I2C protocol
                    to extract the data form HMC5883 for detection.                                                          
* Reference    : None       
                                                                                             
/*****************************************************************************/
#include <Wire.h> //I2C Arduino Library

#define address 0x1E //0011110b, I2C 7bit address of HMC5883

int x[10];
int y[10];
int z[10];
int xout;
int yout;
int zout;
int ii;
const int motorIn1 = 5;
const int motorIn2 = 6;
const int motorIn3 = 9;      
const int motorIn4 = 10; 


void setup()
{
    Serial.begin(9600);
    pinMode(motorIn1, OUTPUT);
    pinMode(motorIn2, OUTPUT);
    pinMode(motorIn3, OUTPUT);
    pinMode(motorIn4, OUTPUT);

    Wire.begin();
    //Put the HMC5883 IC into the correct operating mode
    Wire.beginTransmission(address); //open communication with HMC5883
    Wire.write(0x02); //select mode register
    Wire.write(0x00); //continuous measurement mode
    Wire.endTransmission();
}

void loop()
{
    GetSensorData_HMC5883();
}


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

 * Function Name: GetSensorData_HMC5883
 * Discription: This function will catch the data for the sensor via I2C protocol
                    and adjust the orientation of robot if it got some orient slide. 
 * Output: 0
 * Communication protocol: I2C
 * Arduino input port: SCK, SCL
 * Unit: degress

 *********************************************************************************/
void GetSensorData_HMC5883()
{  
    int x = 0;       //triple x-axis data
    int y = 0;       //triple y-axis data
    int z = 0;       //triple z-axis data                          

    for(ii=0; ii<=9; ii++)
    {
        Wire.beginTransmission(address);     //Tell the HMC5883 where to begin reading data
        Wire.write(0x03);                    //select register 3, X MSB register
        Wire.endTransmission();
        Wire.requestFrom(address, 6);        //Read data from each axis, 2 registers per axis
        if(6<=Wire.available())
        {
            x = Wire.read()<<8; //X msb
            x |= Wire.read(); //X lsb
            z = Wire.read()<<8; //Z msb
            z |= Wire.read(); //Z lsb
            y = Wire.read()<<8; //Y msb
            y |= Wire.read(); //Y lsb

            xout=x+xout;
            yout=y+yout;
            zout=z+zout;
        } 
    } 

    Algorithm_Degress(xout, yout, zout);
} 
  
    

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

 * Function Name: Algorithm_Degress
 * Discription: This function is the algorithm of computing the slide degree form the data
                    that we caught form the sensor.
 * Output: 0
 * Communication protocol: none
 * Arduino input port: none
 * Unit: degress

 *********************************************************************************/
int Algorithm_Degress(int xout, int yout, int zout)
{
    int Average_X = xout/10;
    int Average_Y = yout/10;
    int Average_Z = zout/10;
    int x0 = 450;
    int y0 = -122;
    float cen_x=161;
    float cen_y=42.47;
    float x1=x0-16;	                // enter refernce point
    float y1=y0-206.64;             // enter refernce point
    float x2=Average_X - 16;
    float y2=Average_Y - 206.64;    // center (161, 42.47)
    float line = sqrt((x2 - x1)*(x2 - x1)+(y2-y1)*(y2-y1));
    int deg = acos((245000-line*line)/245000 )*180/PI;
    
    AdjustOrient(Average_X, Average_Y, deg);
}


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

 * Function Name: AdjustOrient
 * Discription: This function will adjust the orient of the robot depends on the data we caught
                    form the sensor.
 * Output: 0
 * Communication protocol: none
 * Arduino input port: none
 * Unit: degress

 *********************************************************************************/
int AdjustOrient(int Average_X, int Average_Y, int SlideDegrees)
{

    if(Average_X > 450){
    SlideDegrees = SlideDegrees;
    }
    else{
    SlideDegrees = -SlideDegrees;
    }

    Serial.print(Average_X);
    Serial.print("\t");
    Serial.print(Average_Y);
    Serial.print("\t");
    Serial.println(SlideDegrees);

    if(SlideDegrees > 30)
    {                   //    Right slide
        left();
    }
    else if(SlideDegrees < -30)
    {   
        right();        //     left slide
    }
    else if(SlideDegrees < 20)
    {   
        motorstop();
    }     
    else if(SlideDegrees> -20)
    {
        motorstop();
    }  
} 



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

 * Function Name: motorstop, right, left
 * Discription: This function will send the commend to the motor driver and make action.
 * Communication protocol: digital write

 *********************************************************************************/
void right(){
  digitalWrite(motorIn1, LOW);
  digitalWrite(motorIn2, HIGH);
  digitalWrite(motorIn3, HIGH);
  digitalWrite(motorIn4, LOW);
}

void left(){
  digitalWrite(motorIn1, HIGH);
  digitalWrite(motorIn2, LOW);
  digitalWrite(motorIn3, LOW);
  digitalWrite(motorIn4, HIGH);
}

void motorstop(){
  digitalWrite(motorIn1, LOW);
  digitalWrite(motorIn2, LOW);
  digitalWrite(motorIn3, LOW);
  digitalWrite(motorIn4, LOW);
}