OK guys I have had lots of hits but no response. My problem is simple except well I am still searching for the solution.
I am using a digital compass CMPS03 to record the orientation and a Sharp IR sensor to record distance. This code rotates the robot thro 360 degrees. As it rotates, it records the compass readings at it's start point, the current compass reading and an additional compass reading whenever the IR Sensor distance reading is greater than a previous IR Sensor distance reading. The code works well to this point.
When the 360* rotation is complete, I would like a brief delay and the the robot to rotate to the point recorded with the greater IR Sensor distance recording.
I am able to rotate exactly 360* because the robot stops when the value of the compass start-bearing == the compass current bearing. The problem is when I am trying to rotate the robot back to the point where the greater IR Sensor distance was made. I thought it should be :- 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();
}
[code]