So good news and bad news:
Good news is… THIS ONE WORKS!!! YAY!!!
Bad news is… This one overshoots the 90 degree turns quite a bit, particularly the 3rd turn.
I don’t know why this is, but when I try testing it by holding it in my hand, putting something in front of it, and then turning around so that it goes through the code, it overshoots on the 1st, 2nd, and 4th turns by about 5-10 degrees, and on the 3rd up to roughly 30 degrees. The “turning” variables are respective to which turn it is presently on.
#include <Wire.h>
float range;
float range2;
float range3;
int irdistance = 1;
int irsensed = 1;
byte turning = 0;
byte turning2 = 0;
byte turning3 = 0;
byte turning4 = 0;
int HMC6352Address = 0x42;
int slaveAddress;
byte headingData[2];
int i, headingValue;
float headingStart;
float headingTurn;
byte done = 0;
/////////////////////////////////////////////////////////////
void setup() {
pinMode(13, OUTPUT);
Wire.begin();
Serial.begin(9600);
Serial.print("Beginning setup.");
digitalWrite(13, HIGH);
Serial.print(".");
delay(10);
digitalWrite(13, LOW);
Serial.println(".");
//Note: the following four lines of code are not mine.
// 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
delay (50); //(my own code continues here)Just for good measure.
pinMode(3, OUTPUT); //These four
pinMode(5, OUTPUT); //lines of code
pinMode(6, OUTPUT); //are to set
pinMode(9, OUTPUT); //the motors up.
digitalWrite(13,HIGH);
headingStart = getHeading() / 10; //This sets up a variable called headingStart that gives a real heading and saves it as its initial heading.
if (headingStart >= 270) {
headingStart = headingStart - 360;
}
digitalWrite(13,LOW);
Serial.println("Setup completed.");
digitalWrite(3, HIGH); //And these
digitalWrite(5, LOW); //four are to
digitalWrite(6, LOW); //get them to
digitalWrite(9, HIGH); //go forward.
}
/////////////////////////////////////////////////////////////
void loop() {
irsensed = analogRead(0); //Gets distance of object in front of the robot.
irdistance = irsensed; //Doublechecks. I added this in because the sensor sometimes would just return 0.
if (done == 0) { //fixed this. Its purpose is to make sure it doesn't keep going and going.
if (irdistance >= 488 && irdistance <= 536 && turning == 0) { // if an object is about 20 cm away and turning is not true
digitalWrite(13,HIGH);
turning = 1; //this tells itaself that it needs to start turning, hence the name of the variable.
Serial.println("Beginning turning sequence...");
range = headingStart + 85; //this sets up the threshold for the turn later in the code (see loop).
digitalWrite(13,LOW);
}
if (turning == 1) { //If it is beginning its turn
digitalWrite(3, LOW); //stop both motors
digitalWrite(5, LOW);
digitalWrite(6, LOW);
digitalWrite(9, LOW);
delay(10);
digitalWrite(3, HIGH); //turns right
digitalWrite(5, LOW);
digitalWrite(6, HIGH);
digitalWrite(9, LOW);
headingTurn = getHeading() / 10;
if (headingTurn >= range) {
digitalWrite(13,HIGH);
turning++;
turning2 = 1;
digitalWrite(3, LOW);
digitalWrite(5, HIGH);
digitalWrite(6, LOW);
digitalWrite(9, HIGH);
digitalWrite(3, LOW);
digitalWrite(5, LOW);
digitalWrite(6, LOW);
digitalWrite(9, LOW);
Serial.println("First turn completed.");
digitalWrite(13,LOW);
digitalWrite(3, HIGH);
digitalWrite(5, LOW);
digitalWrite(6, LOW);
digitalWrite(9, HIGH);
delay(1500);
digitalWrite(3, LOW);
digitalWrite(5, LOW);
digitalWrite(6, LOW);
digitalWrite(9, LOW);
digitalWrite(13,HIGH);
}
}
if (turning2 == 1) {
delay(10);
digitalWrite(3, LOW);
digitalWrite(5, HIGH);
digitalWrite(6, LOW);
digitalWrite(9, HIGH);
headingTurn = getHeading() / 10;
if (headingTurn <= headingStart || headingTurn - 360 >= headingStart) {
turning3 = 1;
turning2 = 0;
digitalWrite(3, HIGH);
digitalWrite(5, LOW);
digitalWrite(6, HIGH);
digitalWrite(9, LOW);
digitalWrite(3, LOW);
digitalWrite(5, LOW);
digitalWrite(6, LOW);
digitalWrite(9, LOW);
digitalWrite(13,LOW);
digitalWrite(3, HIGH);
digitalWrite(5, LOW);
digitalWrite(6, LOW);
digitalWrite(9, HIGH);
delay(1500);
digitalWrite(3, LOW);
digitalWrite(5, LOW);
digitalWrite(6, LOW);
digitalWrite(9, LOW);
digitalWrite(13,HIGH);
}
}
if (turning3 == 1) {
delay(10);
digitalWrite(3, LOW);
digitalWrite(5, HIGH);
digitalWrite(6, LOW);
digitalWrite(9, HIGH);
headingTurn = getHeading() / 10;
if (headingTurn <= range - 180 || headingTurn >= range + 180) {
turning4 = 1;
turning3 = 0;
digitalWrite(3, HIGH);
digitalWrite(5, LOW);
digitalWrite(6, HIGH);
digitalWrite(9, LOW);
digitalWrite(3, LOW);
digitalWrite(5, LOW);
digitalWrite(6, LOW);
digitalWrite(9, LOW);
digitalWrite(13,LOW);
digitalWrite(3, HIGH);
digitalWrite(5, LOW);
digitalWrite(6, LOW);
digitalWrite(9, HIGH);
delay(1500);
digitalWrite(3, LOW);
digitalWrite(5, LOW);
digitalWrite(6, LOW);
digitalWrite(9, LOW);
digitalWrite(13,HIGH);
}
}
if (turning4 == 1) {
delay(10);
digitalWrite(3, HIGH);
digitalWrite(5, LOW);
digitalWrite(6, HIGH);
digitalWrite(9, LOW);
headingTurn = getHeading() / 10;
if (headingTurn >= headingStart || headingTurn - 360 >= headingStart) {
done = 1;
turning4 = 0;
digitalWrite(3, LOW);
digitalWrite(5, HIGH);
digitalWrite(6, LOW);
digitalWrite(9, HIGH);
digitalWrite(3, LOW);
digitalWrite(5, LOW);
digitalWrite(6, LOW);
digitalWrite(9, LOW);
digitalWrite(13,LOW);
digitalWrite(3, HIGH);
digitalWrite(5, LOW);
digitalWrite(6, LOW);
digitalWrite(9, HIGH);
delay(1500);
digitalWrite(3, LOW); //stop both motors
digitalWrite(5, LOW);
digitalWrite(6, LOW);
digitalWrite(9, LOW);
digitalWrite(13,HIGH);
delay(1000);
digitalWrite(13,LOW);
}
}
}
}
/////////////////////////////////////////////////////////////
int getHeading() {
Serial.print("getting heading.");
Wire.beginTransmission(slaveAddress);
Serial.print(".");
Wire.send("A"); // The "Get Data" command
Serial.print(".");
Wire.endTransmission();
Serial.print(".");
delay(10); // The HMC6352 needs at least a 70us (microsecond) delay
// after this command. Using 1ms 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
Serial.print(".");
Wire.requestFrom(slaveAddress, 2); // Request the 2 byte heading (MSB comes first)
Serial.print(".");
i = 0;
Serial.print(".");
delay(5);
while (Wire.available() && i < 2)
{
Serial.print(".");
headingData[i] = Wire.receive();
Serial.print(".");
i++;
Serial.println(".");
Serial.println("Heading found.");
}
headingValue = headingData[0]*256 + headingData[1]; // Put the MSB and LSB together
Serial.println(float (headingValue / 10));
return headingValue;
}
/////////////////////////////////////////////////////////////
Any ideas?
).
.)