ROBOTC.net forums
http://robotc.net/forums/

Need help with Hitechnic Compass Sensor
http://robotc.net/forums/viewtopic.php?f=41&t=2799
Page 1 of 1

Author:  bestgr [ Tue Nov 30, 2010 2:11 pm ]
Post subject:  Need help with Hitechnic Compass Sensor

Hello guys,

I am a student of engineering and I am developping a project about a self-navigating car which detects obstacles and avoids them. At the phase where I am now, I am creating each function and module separately. I have created all my modules but I have a problem with the one that uses the compass sensor to navigate the car towards a certain direction.

Let me give you some details about my design, before describing my problem.
I have built a car. It uses one motor on the back wheels to give movement, one motor on a front steering axis, to turn the wheels when needed (the third motor is needed elsewhere). In order to handle the steering, I am using the functions below:

Code:
void alignSteer(){
  wait1Msec(50);
  if (steerDir==1){ //if the wheels are turned left
    nMotorEncoder[motorC] = 0;

     while (nMotorEncoder[motorC] < 25) //turn them 25 degrees to the right
     {
       motor[motorC] = 60;
     }

     motor[motorC] = 0;
  }
  else if (steerDir==3){//if they are right
    nMotorEncoder[motorC] = 0;

     while (nMotorEncoder[motorC] > -25)//turn 25 degrees left
     {
       motor[motorC] = -60;
     }

     motor[motorC] = 0;
  }
  else if (steerDir==2){ //if we are already on the straight position, do nothing
    wait1Msec(5); //do nothing
  }
  else{
    alignSteerStart(); //in any other case reset the steering. Practically it should never end up in this else, but
                        //I have used it in there, just in case.
  }
  steerDir=2;
}


void turnLeft(){
  wait1Msec(50);
  if (steerDir==2){
    nMotorEncoder[motorC] = 0;

     while (nMotorEncoder[motorC] > -25)
     {
       motor[motorC] = -60;
     }

     motor[motorC] = 0;
  }
  else if (steerDir==3){
    nMotorEncoder[motorC] = 0;

     while (nMotorEncoder[motorC] > -50)
     {
       motor[motorC] = -60;
     }

     motor[motorC] = 0;
  }
  else if (steerDir==1){
    wait1Msec(5); //do nothing
  }
  else{
    alignSteerStart();
    nMotorEncoder[motorC] = 0;
     while (nMotorEncoder[motorC] > -25)
     {
       motor[motorC] = -60;
     }
     motor[motorC] = 0;
  }
  steerDir=1;
}


TurnRight() is the same as TurnLeft() with reverse conditions.

When the program starts, I am setting the target for my compass sensor and starting a while loop until my light sensor finds black on the floor. I am using the relative attribute of the compass sensor as defined in the driver provided by Xander. The thing is that if I hold the robot in my hands and turn it left or right, everything works well, but if I put it on the floor, after it moves some distance, it loses the focus and does irrational things. I have modified and run the provided calibration program, so that it works with my car which is a four wheel car and not a tribot. The code that checks and adjucts the steering is below:

Code:
void steer(){
  if(relativeHeading > 10){
      turnLeft();
      motor[motorB] = 25;
    }
    else if(relativeHeading < -10){
      turnRight();
      motor[motorB] = 25;
    }
    else{
      alignSteer();
      motor[motorB] = 50;
    }
}

task main()
{
  wait1Msec(50);
  StartTask(checkCompass);

  alignSteerStart();
  while(SensorValue[lightSensor] > 30){
    steer();
    wait1Msec(100);
  }
  wait1Msec(1000);
  StopTask(checkCompass);
}


I know about things such as sub-woofers,etc. that might influence the compass, but I don't think this is the case. Any advice is highly appreciated.

PS: I have talked with Hitechnic on the phone and they said I should post here and wait for Xander to reply, so here I am :)

Thanks,
Nick

Author:  mightor [ Tue Nov 30, 2010 2:32 pm ]
Post subject:  Re: Need help with Hitechnic Compass Sensor

Hello Nick,

It's a bit hard for me to see where the problem is when you're not showing the parts that read the actual sensor. Do you think you could attach the complete program to this thread?

Regards,
Xander

Author:  bestgr [ Tue Nov 30, 2010 2:34 pm ]
Post subject:  Re: Need help with Hitechnic Compass Sensor

Sorry forgot to add the task code:

Code:
task checkCompass(){

 _target = HTMCsetTarget(HTMC);

 while(true){
   heading = HTMCreadHeading(HTMC);
   relativeHeading = HTMCreadRelativeHeading(HTMC);

   nxtDisplayTextLine(4, "Abs:   %4d", heading);
   nxtDisplayTextLine(5, "Rel:   %4d", relativeHeading);
   wait1Msec(200);
 }
}


heading and relativeHeading are global variables.
thanks

Author:  mightor [ Tue Nov 30, 2010 2:36 pm ]
Post subject:  Re: Need help with Hitechnic Compass Sensor

I'd really rather see the whole program, can you attach it please?

- Xander

Author:  bestgr [ Tue Nov 30, 2010 2:38 pm ]
Post subject:  Re: Need help with Hitechnic Compass Sensor

Code:
#pragma config(Sensor, S1,     HTMC,                sensorLowSpeed)
#pragma config(Sensor, S2,     lightSensor,         sensorLightActive)

//compass driver
#include "drivers/HTMC-driver.h"

//compass variables
int _target = 0;
int heading = 0;
int relativeHeading=0;
//end compass variables


int steerDir = 0;  //1 = left, 2=center, 3=right

task checkCompass(){

 _target = HTMCsetTarget(HTMC);

 while(true){
   heading = HTMCreadHeading(HTMC);
   relativeHeading = HTMCreadRelativeHeading(HTMC);

   nxtDisplayTextLine(4, "Abs:   %4d", heading);
   nxtDisplayTextLine(5, "Rel:   %4d", relativeHeading);
   wait1Msec(200);
 }
}

void alignSteerStart(){

    nMotorEncoder[motorC] = 0;
    motor[motorC] = 60;
   wait1Msec(300);

    nMotorEncoder[motorC] = 0;

    while (nMotorEncoder[motorC] > -40)
    {
      motor[motorC] = -60;
    }

    motor[motorC] = 0;
    steerDir=2;
}

void alignSteer(){
  wait1Msec(50);
  if (steerDir==1){ //if the wheels are turned left
    nMotorEncoder[motorC] = 0;

     while (nMotorEncoder[motorC] < 25) //turn them 25 degrees to the right
     {
       motor[motorC] = 60;
     }

     motor[motorC] = 0;
  }
  else if (steerDir==3){//if they are right
    nMotorEncoder[motorC] = 0;

     while (nMotorEncoder[motorC] > -25)//turn 25 degrees left
     {
       motor[motorC] = -60;
     }

     motor[motorC] = 0;
  }
  else if (steerDir==2){ //if we are already on the straight position, do nothing
    wait1Msec(5); //do nothing
  }
  else{
    alignSteerStart(); //in any other case reset the steering. Practically it should never end up in this else, but
                        //I have used it in there, just in case.
  }
  steerDir=2;
}


void turnLeft(){
  wait1Msec(50);
  if (steerDir==2){
    nMotorEncoder[motorC] = 0;

     while (nMotorEncoder[motorC] > -25)
     {
       motor[motorC] = -60;
     }

     motor[motorC] = 0;
  }
  else if (steerDir==3){
    nMotorEncoder[motorC] = 0;

     while (nMotorEncoder[motorC] > -50)
     {
       motor[motorC] = -60;
     }

     motor[motorC] = 0;
  }
  else if (steerDir==1){
    wait1Msec(5); //do nothing
  }
  else{
    alignSteerStart();
    nMotorEncoder[motorC] = 0;
     while (nMotorEncoder[motorC] > -25)
     {
       motor[motorC] = -60;
     }
     motor[motorC] = 0;
  }
  steerDir=1;
}

void turnRight(){
  wait1Msec(50);
  if (steerDir==2){
    nMotorEncoder[motorC] = 0;

     while (nMotorEncoder[motorC] < 25)
     {
       motor[motorC] = 60;
     }

     motor[motorC] = 0;
  }
  else if (steerDir==1){
    nMotorEncoder[motorC] = 0;

     while (nMotorEncoder[motorC] < 50)
     {
       motor[motorC] = 60;
     }

     motor[motorC] = 0;
  }
  else if (steerDir==3){
    wait1Msec(5); //do nothing
  }
  else{
    alignSteerStart();
    nMotorEncoder[motorC] = 0;
     while (nMotorEncoder[motorC] < 25)
     {
       motor[motorC] = 60;
     }
     motor[motorC] = 0;
  }
  steerDir=3;
}

void steer(){
  if(relativeHeading > 10){
      turnLeft();
      motor[motorB] = 25;
    }
    else if(relativeHeading < -10){
      turnRight();
      motor[motorB] = 25;
    }
    else{
      alignSteer();
      motor[motorB] = 50;
    }
}

task main()
{
  wait1Msec(50);
  StartTask(checkCompass);

  alignSteerStart();
  while(SensorValue[lightSensor] > 30){
    steer();
    wait1Msec(100);
  }
  wait1Msec(1000);
  StopTask(checkCompass);
}

Author:  mightor [ Tue Nov 30, 2010 2:54 pm ]
Post subject:  Re: Need help with Hitechnic Compass Sensor

I don't think the problem is with the compass. Your motor control could use a bit of an improvement. Consider replacing code like this:
Code:
nMotorEncoder[motorC] = 0;
while (nMotorEncoder[motorC] < 25)
{
   motor[motorC] = 60;
}

motor[motorC] = 0;


With something like this:
Code:
nMotorEncoderTarget[motorC] = 25;
motor[motorC] = 60;
while(nMotorRunState[motorC] != runStateIdle) EndTimeSlice();


The problem with the continuous encoder resets is that it's generally a bad thing to do. The reason is that sometimes they take a bit longer to take effect than expected (up to a few ms).

Also, I would advise you to use the motor and sensor configuration to also configure your motors. It will make it a LOT easier for you to read your code in a few weeks or for someone like me to figure it out. Use sensible names like "STEERING" and "DRIVING" and "CLAW" for example.

- Xander

Author:  bestgr [ Tue Nov 30, 2010 3:02 pm ]
Post subject:  Re: Need help with Hitechnic Compass Sensor

Thanks for your reply. Yeah, sorry for the motor name. MotorC is the motor used for steering as you probably figured out.
The thing is that I was using the nMotorEncoderTarget method, but it did not work back then, this is why I changed it to the one I posted. Anyway, I will try it right now and tell you in a few minutes the results.

Thanks again,
Nick

Author:  bestgr [ Tue Nov 30, 2010 3:13 pm ]
Post subject:  Re: Need help with Hitechnic Compass Sensor

Yes, as I thought, it happens just like last time did. It turns once, but it never gets out of the while loop of the target encoder.

the new code:

Code:
#pragma config(Sensor, S1,     HTMC,                sensorLowSpeed)
#pragma config(Sensor, S2,     lightSensor,         sensorLightActive)
#pragma config(Motor,  motorC,          SteeringMotor, tmotorNormal, PIDControl, reversed, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

//compass driver
#include "drivers/HTMC-driver.h"

//compass variables
int _target = 0;
int heading = 0;
int relativeHeading=0;
//end compass variables


int steerDir = 0;  //1 = left, 2=center, 3=right

task checkCompass(){

 _target = HTMCsetTarget(HTMC);

 while(true){
   heading = HTMCreadHeading(HTMC);
   relativeHeading = HTMCreadRelativeHeading(HTMC);

   nxtDisplayTextLine(4, "Abs:   %4d", heading);
   nxtDisplayTextLine(5, "Rel:   %4d", relativeHeading);
   wait1Msec(200);
 }
}

void alignSteerStart(){

    nMotorEncoder[SteeringMotor] = 0;
    motor[SteeringMotor] = 60;
   wait1Msec(300);

    nMotorEncoderTarget[SteeringMotor] = -40;
   motor[SteeringMotor] = -60;
   while(nMotorRunState[SteeringMotor] != runStateIdle) EndTimeSlice();

    motor[SteeringMotor] = 0;
    steerDir=2;
}

void alignSteer(){
  wait1Msec(50);
  if (steerDir==1){ //if the wheels are turned left
     nMotorEncoderTarget[SteeringMotor] = 25;
     motor[SteeringMotor] = 60;
     while(nMotorRunState[SteeringMotor] != runStateIdle) EndTimeSlice();

     motor[SteeringMotor] = 0;
  }
  else if (steerDir==3){//if they are right
    nMotorEncoderTarget[SteeringMotor] = -25;
    motor[SteeringMotor] = -60;
    while(nMotorRunState[SteeringMotor] != runStateIdle) EndTimeSlice();

     motor[SteeringMotor] = 0;
  }
  else if (steerDir==2){ //if we are already on the straight position, do nothing
    wait1Msec(5); //do nothing
  }
  else{
    alignSteerStart(); //in any other case reset the steering. Practically it should never end up in this else, but
                        //I have used it in there, just in case.
  }
  steerDir=2;
}


void turnLeft(){
  wait1Msec(50);
  if (steerDir==2){
    nMotorEncoderTarget[SteeringMotor] = -25;
    motor[SteeringMotor] = -60;
    while(nMotorRunState[SteeringMotor] != runStateIdle) EndTimeSlice();

     motor[SteeringMotor] = 0;
  }
  else if (steerDir==3){
    nMotorEncoderTarget[SteeringMotor] = -50;
    motor[SteeringMotor] = -60;
    while(nMotorRunState[SteeringMotor] != runStateIdle) EndTimeSlice();

     motor[SteeringMotor] = 0;
  }
  else if (steerDir==1){
    wait1Msec(5); //do nothing
  }
  else{
    alignSteerStart();
    nMotorEncoderTarget[SteeringMotor] = -25;
    motor[SteeringMotor] = -60;
    while(nMotorRunState[SteeringMotor] != runStateIdle) EndTimeSlice();
     motor[SteeringMotor] = 0;
  }
  steerDir=1;
}

void turnRight(){
  wait1Msec(50);
  if (steerDir==2){
    nMotorEncoderTarget[SteeringMotor] = 25;
    motor[SteeringMotor] = 60;
    while(nMotorRunState[SteeringMotor] != runStateIdle) EndTimeSlice();

     motor[SteeringMotor] = 0;
  }
  else if (steerDir==1){
    nMotorEncoderTarget[SteeringMotor] = 50;
    motor[SteeringMotor] = 60;
    while(nMotorRunState[SteeringMotor] != runStateIdle) EndTimeSlice();

     motor[SteeringMotor] = 0;
  }
  else if (steerDir==3){
    wait1Msec(5); //do nothing
  }
  else{
    alignSteerStart();
    nMotorEncoderTarget[SteeringMotor] = 25;
    motor[SteeringMotor] = 60;
    while(nMotorRunState[SteeringMotor] != runStateIdle) EndTimeSlice();
     motor[SteeringMotor] = 0;
  }
  steerDir=3;
}

void steer(){
  if(relativeHeading > 10){
      turnLeft();
      motor[motorB] = 25;
    }
    else if(relativeHeading < -10){
      turnRight();
      motor[motorB] = 25;
    }
    else{
      alignSteer();
      motor[motorB] = 50;
    }
}

task main()
{
  wait1Msec(50);
  StartTask(checkCompass);

  alignSteerStart();
  while(SensorValue[lightSensor] > 30){
    steer();
    wait1Msec(100);
  }
  wait1Msec(1000);
  StopTask(checkCompass);
}

Author:  mightor [ Tue Nov 30, 2010 4:01 pm ]
Post subject:  Re: Need help with Hitechnic Compass Sensor

Check for both "runStateHoldPosition" and "runStateIdle", both are valid.

- Xander

Author:  bestgr [ Tue Nov 30, 2010 4:08 pm ]
Post subject:  Re: Need help with Hitechnic Compass Sensor

mightor wrote:
Check for both "runStateHoldPosition" and "runStateIdle", both are valid.

- Xander


both not working. they take me in an infinite loop. never reaches the target.

Page 1 of 1 All times are UTC - 5 hours [ DST ]
Powered by phpBB © 2000, 2002, 2005, 2007 phpBB Group
http://www.phpbb.com/