Using a digital compass CMPS03 and a Sharp IR sensor, I wrote this code to rotate my mobile platform 360 degrees and simultaneously record the furtherest distance of obstacles as well as the compass bearing. During it's 360* rotation, as soon as a longer distance is recorded by the IR sensor, the current reading of the compass (ie bearing) is stored as the newBearing in my codes. The code does this very well. However I am having problems writing the bit of the code to enable the mobile platform rotate back to the point with the furtherest distance measured. for example:- do {slowRotate}while (bearing != newBearing);
/*
CMPS03 with arduino I2C example
This will display a value of 0 - 359 for a full rotation of the compass.
The SDA line is on analog pin 4 of the arduino and is connected to pin 3 of the CMPS03.
The SCL line is on analog pin 5 of the arduino and is conected to pin 2 of the CMPS03.
Both SDA and SCL are also connected to the +5v via a couple of 1k8 resistors.
A switch to callibrate the CMPS03 can be connected between pin 6 of the CMPS03 and the ground.
*/
// include Wire library to read and write I2C commands:
#include <Wire.h>
#include <LiquidCrystal.h>
#define address 0x60 //defines address of compass
#define resultRegister 0x02 // this is the memory register in the sensor that contains the result:
//#define hideCsr 0x04 // Byte to hide LCD03 cursor
#define md25Address 0x58 // Address of the MD25
#define md25Address 0x59 // Address of the MD25
#define softwareReg 0x0D // Byte to read the software version
#define speed1 0x00 // Byte to send speed to first motor
#define speed2 0x01 // Byte to send speed to second motor
#define cmdByte 0x10 // Command byte
byte highByte;
byte lowByte;
LiquidCrystal lcd(12, 11, 5, 4, 3, 2);
//unsigned long nextPrint = 0;
int sensorPin = 0;
int distance;
int maxReading = 59;
int minReading = 8;
int bearing;
int firstBearing;
int startBearing;
int newBearing;
int startDistance;
int newDistance;
void setup()
{
Wire.begin(); // start the I2C bus
delay(100);
// printIT();
firstCompassRead(); // function to call first compass reading
startBearing = firstBearing; // capture and store initial compass reading when platform is powered on
slowRotate(); //Forces Robot to do rotate out of the captured initial compass reading.
delay(1000); // forced rotation to last for 2 seconds
readDistance();
startDistance = distance; // capture and store initial IR obstacle distance reading when platform is powered on
delay(100);
}
void loop()
{
printIT();
rotate360();
readDistance();
readCompass();
do {
if (startDistance > distance)
{
break;
}
else
{
getValues();
startDistance = newDistance;
}
}while (bearing == startBearing);
exit;
do{
delay(5000); // I AM HAVING PROBLEMS HERE
slowRotate();
} while (bearing != newBearing);
}
void firstCompassRead()
{
byte highByte;
byte lowByte;
Wire.beginTransmission(address); //starts communication with cmps03
Wire.send(2); //Sends the register we wish to read
Wire.endTransmission();
Wire.requestFrom(address, 2); //requests high byte, 2 byte is for an integer
while(Wire.available() < 2); //while there is a byte to receive
highByte = Wire.receive(); //reads the byte as an integer
lowByte = Wire.receive();
firstBearing = ((highByte<<8)+lowByte)/10;
}
void rotate360()
{
do
{
if ((bearing) >= (startBearing + 5))
{ slowRotate();}
else if ((bearing) <= (startBearing - 5))
{slowRotate();}
else
{stopRotate();}
exit;
stopRotate;
} while (bearing == startBearing);
}
void getValues()
{
newBearing = bearing;
int sensorValue = analogRead(sensorPin); // read the pot value
// the sensor actually gives results that aren't linear.
// this formula converts the results to a linear range.
int distNew = (6787 / (sensorValue - 3)) - 4;
if(distNew < minReading) {
distNew = minReading; // keep the minimum range at 20cm
}
if(distNew > maxReading) {
distNew = maxReading;
}
newDistance = distNew;
}
void printIT()
{
// print it:
lcd.begin(16, 2); // set up the LCD's number of rows and columns.
lcd.print("CP: ");
lcd.print(startBearing); //Print Compass Reading on LCD
lcd.print(" ");
lcd.print(bearing); //Print Compass Reading on LCD
lcd.print(" ");
lcd.print(newBearing); //Print Compass Reading on LCD
// delay(50);
lcd.setCursor(0, 1); // move to LCD's 2nd rows and 1st columns.
lcd.print("Dist: ");
lcd.print(startDistance);
lcd.print(" ");
lcd.print(distance);
lcd.print(" ");
lcd.print(newDistance);
// wait before next reading:
delay(200);
}
void readCompass()
{
byte highByte;
byte lowByte;
Wire.beginTransmission(address); //starts communication with cmps03
Wire.send(2); //Sends the register we wish to read
Wire.endTransmission();
Wire.requestFrom(address, 2); //requests high byte, 2 byte is for an integer
while(Wire.available() < 2); //while there is a byte to receive
highByte = Wire.receive(); //reads the byte as an integer
lowByte = Wire.receive();
bearing = ((highByte<<8)+lowByte)/10;
}
void readDistance()
{
int sensorValue = analogRead(sensorPin); // read the pot value
// the sensor actually gives results that aren't linear.
// this formula converts the results to a linear range.
distance = (6787 / (sensorValue - 3)) - 4;
if(distance < minReading) {
distance = minReading; // keep the minimum range at 20cm
}
if(distance > maxReading) {
distance = maxReading;
}
}
void slowRotate()
{
MoveA();
MoveB();
MoveC();
}
void stopRotate()
{
StopA();
StopB();
StopC();
}
int getSoft()
{ // Function that gets the software version
Wire.beginTransmission(0x58); // Send byte to read software version as a single byte
Wire.send(softwareReg);
Wire.endTransmission();
Wire.requestFrom(0x58, 1); // request 1 byte form MD25
while(Wire.available() < 1); // Wait for it to arrive
int softwareA = Wire.receive(); // Read it in
//return(software);
Wire.beginTransmission(0x59); // Send byte to read software version as a single byte
Wire.send(softwareReg);
Wire.endTransmission();
Wire.requestFrom(0x59, 1); // request 1 byte form MD25
while(Wire.available() < 1); // Wait for it to arrive
int softwareB = Wire.receive(); // Read it in
//return(software);
}
int MoveA()
{
Wire.beginTransmission(0x58); // 1st MD25_Set first motorA from max speed of 255 to lower speed of 155
Wire.send(speed1);
//Wire.send(155);
Wire.send(125);
Wire.endTransmission();
}
int MoveB()
{
Wire.beginTransmission(0x58); // 1st MD25_Set first motorB from max reverse speed of 0 to lower reverse speed of 100
Wire.send(speed2);
//Wire.send(100);
Wire.send(130);
Wire.endTransmission();
}
int MoveC()
{
Wire.beginTransmission(0x59); // 2nd MD25_Set first motorB from max reverse speed of 0 to lower reverse speed of 100
Wire.send(speed2);
//Wire.send(100);
Wire.send(130);
Wire.endTransmission();
}
int StopA()
{
Wire.beginTransmission(0x58); // 1st MD25_Set first motorA from max speed of 255 to lower speed of 155
Wire.send(speed1);
Wire.send(128);
Wire.endTransmission();
}
int StopB()
{
Wire.beginTransmission(0x58); // 1st MD25_Set first motorB from max reverse speed of 0 to lower reverse speed of 100
Wire.send(speed2);
Wire.send(128);
Wire.endTransmission();
}
int StopC()
{
Wire.beginTransmission(0x59); // 2nd MD25_Set first motorB from max reverse speed of 0 to lower reverse speed of 100
Wire.send(speed2);
Wire.send(128);
Wire.endTransmission();
}