Go Down

Topic: problem in navigate robot by using compass HMC6352.plz help. urgent (Read 798 times) previous topic - next topic

chokey

hai...i trying to navigate the robot by using compass HMC6352...it suppose to be move in square pattern when moving...i trying to program it by moving straight for few second and then turn 90 deg to the left or the right...the process suppose to keep repeating until it move in square pattern...Here i also put the code that i had write..hope to get some guideline :)
Code: [Select]
#include <Wire.h>
int HMC6352Address = 0x42;
// This is calculated in the setup() function
int slaveAddress;
int ledPin = 13;
boolean ledState = false;
byte headingData[2];
int i, direct;
int state = 1;

int E1 = 4;  // shield motor type PWM
int M1 = 5;
int E2 = 7;  // shield motor type PWM                   
int M2 = 6;

void setup()
{
// Shift the device's documented slave address (0x42) 1 bit right
// This compensates for how the TWI library only wants the
// 7 most significant bits (with the high bit padded with 0)
slaveAddress = HMC6352Address >> 1;   // This results in 0x21 as the address to pass to TWI
Serial.begin(9600);
pinMode(M1, OUTPUT);   
pinMode(M2, OUTPUT);
pinMode(ledPin, OUTPUT);      // Set the LED pin as output
Wire.begin();

}

void motor_forward()
{
  digitalWrite(M1,LOW);
  digitalWrite(M2,LOW);
  analogWrite(E2,100);
  analogWrite(E1,100);
}

void motor_left() // turn left
{
  digitalWrite(M1,LOW);   // need to check whether LOW forward or vice versa
  digitalWrite(M2,HIGH);
  analogWrite(E2,0);
  analogWrite(E1,100);
}
void motor_right() //turn right
{
  digitalWrite(M1,HIGH);
  digitalWrite(M2,HIGH);
  analogWrite(E1,0);
  analogWrite(E2,100);
}

void motor_stop() // stop
{
  digitalWrite(M1,LOW);
  digitalWrite(M2,LOW);
  analogWrite(E2,0);
  analogWrite(E1,0);
}
void loop()
{
  // Flash the LED on pin 13 just to show that something is happening
  // Also serves as an indication that we're not "stuck" waiting for TWI data
  ledState = !ledState;
  if (ledState) {
    digitalWrite(ledPin,HIGH);
  }
  else
  {
    digitalWrite(ledPin,LOW);
  }
  // Send a "A" command to the HMC6352
  // This requests the current heading data
  Wire.beginTransmission(slaveAddress);
  Wire.write("A");              // The "Get Data" command
  Wire.endTransmission();
  delay(10);                   // The HMC6352 needs at least a 70us (microsecond) delay
  // after this command.  Using 10ms just makes it safe
  // Read the 2 heading bytes, MSB first
  // The resulting 16bit word is the compass heading in 10th's of a degree
  // For example: a heading of 1345 would be 134.5 degrees
  Wire.requestFrom(slaveAddress, 2);        // Request the 2 byte heading (MSB comes first)
  i = 0;
  while(Wire.available() && i < 2)
  {
    headingData[i] = Wire.read();
    i++;
  }
  direct = headingData[0]*256 + headingData[1];  // Put the MSB and LSB together

  ///////Serial PRINT////////
  //Serial.print("RAW data: ");
  //Serial.print(" ");
  //Serial.print(direct);
  //Serial.print(" ");
  //Serial.print("Current heading: ");
  //Serial.print(int (headingValue / 10));     // The whole number part of the heading
  //Serial.print(".");
  //Serial.print(int (headingValue % 10));     // The fractional part of the heading
  //Serial.println(" degrees");
  //delay(100);
 
  if (state==1)
  {
    motor_forward();
    delay(1000);
    motor_stop();
    state==2;
  }
  if (state==2)
  { if (direct < 90+5 && direct >90-5)
    {
      motor_stop();
      state==3;
    }
  else if (direct < 90-6)
    {
      motor_right();
    }
    else if (direct > 90+6)
    {
     motor_left();
    }
  }
  if (state==3)
  {
    motor_forward();
    delay(1000);
    motor_stop();
    state==4;
  }
  if (state==4)
  { if (direct <= 180 + 5 && direct >= 180 - 5)
      {
        motor_stop();
        state==5;
      }
      else if ( direct < 180 - 6)
      {
       motor_right();
      }
       else if (direct > 180 + 6)
      {
       motor_left();
      }
  if (state==5)
  {
    motor_forward();
    delay(1000);
    motor_stop();
    state==6;
  }
  if (state==6)
  { if (direct <= 270 + 5 && direct >= 270 - 5)
      {
        motor_stop();
        state==7;
      }
      else if ( direct < 270 - 6)
      {
       motor_right();
      }
       else if (direct > 270 + 6)
      {
       motor_left();
      }
     
  if (state==7)
  {
    motor_forward();
    delay(1000);
    motor_stop();
    state==8;
  }
  if (state==8)
  { if (direct <= 270 + 5 && direct <= 270 - 5)
      {
        motor_stop();
        state==1;
      }
      else if ( direct < 270 - 6)
      {
       motor_right();
      }
       else if (direct > 270 + 6)
      {
       motor_left();
      }
   
  }
}
  }
}


strykeroz

Hi,

What does it do running the code as it is here?

Geoff
"There is no problem so bad you can't make it worse"
- retired astronaut Chris Hadfield

PeterH

I only provide help via the forum - please do not contact me for private consultancy.

AWOL

What do your debug prints tell you is happening?

Code: [Select]
state==8; What's that?
"Pete, it's a fool looks for logic in the chambers of the human heart." Ulysses Everett McGill.
Do not send technical questions via personal messaging - they will be ignored.

chokey

I am develope an autonomous robot that can move in a maze. This is to try the compass sensor. Suppose it works,but it didn't. I doubt there is problem with the coding....

AWOL

"Pete, it's a fool looks for logic in the chambers of the human heart." Ulysses Everett McGill.
Do not send technical questions via personal messaging - they will be ignored.

Go Up