View unanswered posts | View active topics It is currently Thu Aug 28, 2014 9:50 pm






Reply to topic  [ 3 posts ] 
Compass Sensor question 
Author Message
Rookie

Joined: Mon Dec 01, 2008 11:44 pm
Posts: 3
Post Compass Sensor question
We have been working with the compass sensor and trying to understand how it works so that we can use it for navigation.

I've read the topics I found in the forums and have tried to use some of the examples to test the use of the compass sensor. No matter what we try we can't seem to get the robot to turn based on the compass. I've mounted the sensor high on the robot - away from the motors, etc. and we can see the values being updated for our variables. Here is the code we have been testing with. Could someone take a look and offer some suggestions? I'm sure we have to be doing something wrong.

#pragma config(Hubs, S1, HTMotor, HTServo, none, none)
#pragma config(Sensor, S2, Compass, sensorI2CHiTechnicCompass)
#pragma config(Sensor, S3, Sonar, sensorSONAR)
#pragma config(Sensor, S4, LeftLight, sensorLightInactive)
#pragma config(Motor, mtr_S1_C1_1, motorD, tmotorNormal, PIDControl, reversed)
#pragma config(Motor, mtr_S1_C1_2, motorE, tmotorNormal, PIDControl)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//

task main()
{
int heading =0;
heading = SensorValue[Compass];
wait1Msec(3000);
{
motor[motorD] = 50;
motor[motorE] = 50;
wait1Msec(3000);
}

{
int targetHeading = heading +90; // 90 degree turn we have also tried as -90

if (targetHeading < 0)
targetHeading += 360;
else if (targetHeading >= 360)
targetHeading -= 360;

motor[motorD] = 50;
motor[motorE] = -50; // we have also tried flopping the motors so that the other runs backwards
while (SensorValue[Compass] >= targetHeading );
}
}


Sun Mar 15, 2009 10:15 pm
Profile
Moderator
Moderator
User avatar

Joined: Wed Mar 05, 2008 8:14 am
Posts: 3196
Location: Rotterdam, The Netherlands
Post Re: Compass Sensor question
Hi there,

Very often the compass needs to be calibrated for the robot it is attached to the first time you use it. I've created a program that can do this but it makes a couple of assumptions:
* Your robot is tribot like robot, ie two wheels and a pivot
* It assumes the distance between the wheels is roughly that of a tribot

You can modify this program but you need to make sure that the robot turns no faster than 360 degrees per 20 seconds, or so. Also make sure you let the motors run long enough to turn the robot about 1.25-1.5 times.

This program will also make sure that after the calibration is done, whether or not the compass didn't experience an error during the calibration process.

Regards,
Xander

Code:
#pragma config(Sensor, S1,     HTCOMPASS_PORT,      sensorI2CCustomStd)
#pragma config(Motor,  motorB,          M_RIGHT,       tmotorNormal, PIDControl, reversed)
#pragma config(Motor,  motorC,          M_LEFT,        tmotorNormal, PIDControl, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

// These measurements are in milimeters.
#define WHEELDIST 115  // distance between the wheels
#define WHEELSIZE 56   // diameter of the wheels
#define MOTORSPEED 4   // speed at which motors should turn


byte cmdbuff[4];
byte reply[8];
byte _tmp;


task timeMe() {
  wait1Msec(20000);
  PlaySound(soundBeepBeep);
  while(bSoundActive);
}

int numRotations() {
  return ((WHEELDIST * 3142) / 1000) / ((WHEELSIZE * 3142) / 1000);
}

void startCalibration() {
  cmdbuff[0] = 3;                           // Number of bytes in I2C command
  cmdbuff[1] = 0x02;                        // I2C address of colour sensor
  cmdbuff[2] = 0x41;                        // Set write address to calibration register
  cmdbuff[3] = 0x43;                        // The data to be written

  eraseDisplay();
  nxtDisplayTextLine(2, "Calibr. %s", reply);
  sendI2CMsg(HTCOMPASS_PORT, cmdbuff[0], 0);    // Send command to protoboard
  PlaySound(soundUpwardTones);
  while(bSoundActive);

  if (nI2CStatus[HTCOMPASS_PORT] == ERR_COMM_BUS_ERR) {
    eraseDisplay();
    nxtDisplayTextLine(1, "ERROR: Couldn't");
    nxtDisplayTextLine(2, "calibrate sensor.");
    nxtDisplayTextLine(4, "Check connection");
    nxtDisplayTextLine(5, "and try again.");
    PlaySound(soundException);
    while(bSoundActive);
    wait1Msec(5000);
    StopAllTasks();
  }
}

void stopCalibration() {
  byte calibrationResult = 0;
  cmdbuff[0] = 3;                           // Number of bytes in I2C command
  cmdbuff[1] = 0x02;                        // I2C address of colour sensor
  cmdbuff[2] = 0x41;                        // Set write address to calibration register
  cmdbuff[3] = 0x00;                        // The data to be written

  sendI2CMsg(HTCOMPASS_PORT, cmdbuff[0], 0);    // Send command to protoboard

  if (nI2CStatus[HTCOMPASS_PORT] == ERR_COMM_BUS_ERR) {
    eraseDisplay();
    nxtDisplayTextLine(1, "ERROR: Couldn't");
    nxtDisplayTextLine(2, "stop calibration.");
    nxtDisplayTextLine(4, "Check connection");
    nxtDisplayTextLine(5, "and try again.");
    PlaySound(soundException);
    while(bSoundActive);
    wait1Msec(5000);
    StopAllTasks();
  }
  wait1Msec(50);

  cmdbuff[0] = 2;                           // Number of bytes in I2C command
  cmdbuff[1] = 0x02;                        // I2C address of colour sensor
  cmdbuff[2] = 0x41;                        // Set write address to calibration register

  sendI2CMsg(HTCOMPASS_PORT, cmdbuff[0], 1);    // Send command to protoboard

  if (nI2CStatus[HTCOMPASS_PORT] == ERR_COMM_BUS_ERR) {
    eraseDisplay();
    nxtDisplayTextLine(1, "ERROR: Couldn't");
    nxtDisplayTextLine(2, "get calibration");
    nxtDisplayTextLine(3, "result.");
    nxtDisplayTextLine(4, "Check connection");
    nxtDisplayTextLine(5, "and try again.");
    PlaySound(soundException);
    while(bSoundActive);
    wait1Msec(5000);
    StopAllTasks();
  }
  wait1Msec(50);

  readI2CReply(HTCOMPASS_PORT, calibrationResult, 1);
  if (_tmp == 2) {
    eraseDisplay();
    nxtDisplayTextLine(1, "ERROR: Calibration");
    nxtDisplayTextLine(2, "has failed.");
    nxtDisplayTextLine(4, "Check connection");
    nxtDisplayTextLine(5, "and try again.");
    PlaySound(soundException);
    while(bSoundActive);
    wait1Msec(5000);
    StopAllTasks();
  }
  nxtDisplayTextLine(1, "SUCCESS: ");
  nxtDisplayTextLine(2, "Calibr. done.");
   PlaySound(soundUpwardTones);
   while(bSoundActive);
   wait1Msec(5000);
}

task main () {
  int numDegrees = 0;

  while (nI2CBytesReady[HTCOMPASS_PORT] > 0)
    readI2CReply(HTCOMPASS_PORT, _tmp, 1);

  while(nI2CStatus[HTCOMPASS_PORT]== STAT_COMM_PENDING)
  wait1Msec(2);

  cmdbuff[0] = 2;                           // Number of bytes in I2C command
  cmdbuff[1] = 0x02;                        // I2C address of colour sensor
  cmdbuff[2] = 0x10;                        // Set write address to calibration register

  sendI2CMsg(HTCOMPASS_PORT, cmdbuff[0], 8);
  while(nI2CStatus[HTCOMPASS_PORT]== STAT_COMM_PENDING)
    wait1Msec(2);

  eraseDisplay();
  if (nI2CStatus[HTCOMPASS_PORT] != NO_ERR) {
    nxtDisplayTextLine(1, "ERROR: Couldn't");
    nxtDisplayTextLine(2, "talk to sensor.");
    nxtDisplayTextLine(4, "Check connection");
    nxtDisplayTextLine(5, "and try again.");
    PlaySound(soundException);
    while(bSoundActive);
    wait1Msec(5000);
    StopAllTasks();
  }

  readI2CReply(HTCOMPASS_PORT, reply[0], 8);

  nMotorEncoder[M_RIGHT] = 0;
  nMotorEncoder[M_LEFT] = 0;
  numDegrees = ((numRotations() * 3) / 2) * 360;
  //nSyncedMotors = synchBC;
  //nSyncedTurnRatio = -100;
  nMotorEncoderTarget[M_RIGHT] = numDegrees;
  StartTask(timeMe);
  startCalibration();

  motor[M_RIGHT] = MOTORSPEED;
  motor[M_LEFT] = -MOTORSPEED;

  while(nMotorRunState[M_RIGHT] != runStateIdle);
  motor[M_LEFT] = 0;
  stopCalibration();
  wait1Msec(5000);
}

_________________
| Professional Conduit of Reasonableness
| (Title bestowed upon on the 8th day of November, 2013)
| My Blog: I'd Rather Be Building Robots
| ROBOTC 3rd Party Driver Suite: [Project Page]


Mon Mar 16, 2009 3:01 am
Profile WWW
Expert
User avatar

Joined: Tue Oct 14, 2008 7:16 pm
Posts: 171
Location: Investigating an unidentified ship sighted in Sector 31428
Post Re: Compass Sensor question
Well, for starters, your syntax is all wonky, particularly your braces, which are all over the place and make it a little hard to figure out what it supposed to happen, however it appears, assuming I'm reading the code correctly, that your robot will never stop turning.

When working with the compass define a global constant that is set at the start of the program that holds the initial heading of your robot, like so:
Code:
int const StartHeading = SensorValue[compass];

Then declare a global variable that it your target heading. It could be an absolute value or angle turn away from the current heading;
Code:
int targetHeading = 270; //An absolute heading
int targetHeading = StartHeading + 90; //A 90 degree turn from the current heading


Now as for making the turn you were on the right track, you would set one motor forward and the other back and continue turning until the compass heading matched the target heading,
Code:
do
{
    motor[leftMotor] = -50;
    motor[rightMotor] = 50;
}while(SensorValue[compass] != targetHeading);
motor[leftMotor] = 0; //Remeber to stop the motors otherwise you'll turn right past your target
motor[rightMotor] = 0;


Now this code doesn't differentiate between a left and right turn, I experimented with a function that would turn the most efficient direction based on the angle provided, whether it was positive or negative, admittedly though it never worked right, but here it is, you can play with it.
Code:
void Turn(int angle)
{
    int initialHeading = SensorValue[compass];
    int targetHeading = initialHeading + angle;

    if(angle < 0)
    {
        do
        {
            motor[leftMotor] = 70;
            motor[rightMotor] = -70;
        }while(SensorValue[compass] != targetHeading);
        motor[leftMotor] = 0;
        motor[rightMotor] = 0;
    }
    else if(angle > 0)
    {
        do
        {
            motor[leftMotor] = -40;
            motor[rightMotor] = 40;
            nxtDisplayTextLine(2, "Compass = %d", SensorValue[compass]);
            if(SensorValue[compass] == targetHeading)
            {
                break;
            }
            else
            {
                continue;
            }
        }while(true);
        motor[leftMotor] = 0;
        motor[rightMotor] = 0;
    }
}

As you can see, I tried a couple of different ideas. I think part of the reason it didn't work is,
  1. The refresh rate on the compass was too slow to hit the desired value accurately.
  2. The robot was turning too fast, though even at the speeds used it turns pretty slow.
  3. Our robot vibrates a lot on our floor and this vibration contributes to inaccurate readings.

Try it out, but unless you turn at a snail's pace, it won't do you much good.

_________________
Captain, Head programmer, School of the Arts, Silverbots Robtics Team #2890
Code:
using namespace System;
using namespace Genius;
using namespace Personality;
public ref class Nerd : Geek, IAnserable
{
    Geek::Type brainMode = Geek::Type::Programmer;
}


Mon Mar 16, 2009 8:14 pm
Profile
Display posts from previous:  Sort by  
Reply to topic   [ 3 posts ] 

Who is online

Users browsing this forum: No registered users and 2 guests


You cannot post new topics in this forum
You cannot reply to topics in this forum
You cannot edit your posts in this forum
You cannot delete your posts in this forum
You cannot post attachments in this forum

Search for:
Jump to:  



Powered by phpBB © 2000, 2002, 2005, 2007 phpBB Group.
Designed by ST Software for PTF.