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

NXT Compass (Motorized!)
http://www.robotc.net/forums/viewtopic.php?f=41&t=1381
Page 1 of 1

Author:  mohem [ Mon Mar 23, 2009 9:40 pm ]
Post subject:  NXT Compass (Motorized!)

Hi



This is my First attempt to program with RobotC. I used the HiTechnic compass sensor and make the robot head north.

This is a video to the robot in action.
NXT Compass (Motorized!)

here is the code (I modified the sample to draw the dial heading on the NXT the sample):

Code:
#pragma config(Sensor, S1,     ,                    sensorTouch)
#pragma config(Sensor, S2,     nCompass,            sensorI2CHiTechnicCompass)
#pragma config(Sensor, S3,     ,                    sensorSONAR)
#pragma config(Sensor, S4,     ,                    sensorLightInactive)
#pragma config(Motor,  motorA,          grabber,       tmotorNormal, PIDControl, )
#pragma config(Motor,  motorB,          rightMotor,    tmotorNormal, PIDControl, )
#pragma config(Motor,  motorC,          LeftMotor,     tmotorNormal, PIDControl, )
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "AdvancedSensors.c"    // include library files

void clear_motor_encoders();
void motorizedIt(int cdegree);
task drawDial();
task main();
void moveTo(int rot, int direction);

task drawDial() {
   int nCompassHeading;
   int nLastX     = 31; // out of range to force initial display
   int nLastY     = 31; // out of range to force initial display
  nCompassHeading = SensorValue[nCompass];
   //
   // Clear screen and setup variables
   //
   eraseDisplay();
   //
   // Repeat forever display angle and sine on the NXT LCD display
   //
  nxtDrawEllipse(0, 62, 62, 0);
   nxtDisplayStringAt(29, 58, "N");
   nxtDisplayStringAt(29,  8, "S");
   nxtDisplayStringAt( 3, 34, "W");
   nxtDisplayStringAt(54, 34, "E");
  nxtDrawLine(31, 31, nLastX, nLastY);
  while (true)
   {
      int X;
      int Y;
     //
     // Normalize
     //
     nCompassHeading = SensorValue[nCompass] % 360;
     if (nCompassHeading < 0)
       nCompassHeading += 360;
    X = 31 + (float) (sinDegrees(nCompassHeading) * 31);
    Y = 31 + (float) (cosDegrees(nCompassHeading) * 31);
     if ((nLastX != X)  || (nLastY != Y))
     {
        //
        // Position has changed. Redraw some screen elements
        // If unchanged, skip to avoid screen flicker
        //
        nxtInvertLine(31, 31, X,  Y);
        nxtInvertLine(31, 31, nLastX, nLastY);
        //
        // Numeric display of 'compass on right of screen
        //
        nxtDisplayStringAt(57, 60, "Heading");
        nxtDisplayStringAt(65, 52, "%+5d", nCompassHeading);
        nxtDisplayStringAt(75, 16, "Port");
        nxtDisplayStringAt(80,  8, "S%1d", (short) ((short) nCompass + 1));
        nLastX = X;
        nLastY = Y;
      }
   }
}


task main(){
  StartTask(drawDial);
   while (true){
    motorizedIt(SensorValue[nCompass]);
  }
}

void motorizedIt(int cdegree) {
  int rot =cdegree % 360;
  int mot = 1;
  //stop when the NXT facing North
  if (cdegree == 0){
     return;
  }
  //reset the encoders value to avoid overflaow
   clear_motor_encoders();

   if (cdegree > 180 && cdegree < 360){
      rot = 360 - rot;
      mot = 0;
   }

   rot *= 5;  // ratio between the circumference of the tire to the circumference of the rotation circle around itself
   switch (mot){
      case 1:
      moveTo(rot/2,1);
     break;
     case 0:
     moveTo(rot/2,-1);
     break;
     case -1:
     moveTo(rot,1);
     break;
   }
}


void clear_motor_encoders(){
   nMotorEncoder[motorA] = 0;
   nMotorEncoder[motorB] = 0;
}

void moveTo(int rot, int direction){
   nSyncedMotors = synchBC;
   nSyncedTurnRatio = -100;
   nMotorEncoderTarget[motorB] = rot;
   motor[motorB] = direction * 50;
   while (nMotorRunState[motorB] != runStateIdle) ;
   motor[motorB] = 0;

}


Let me know if you have any suggestion or improvment.

ThX

Author:  mightor [ Tue Mar 24, 2009 6:32 am ]
Post subject:  Re: NXT Compass (Motorized!)

Salam Mohem :)

I like the little dial you have on your screen to show where the robot is heading! I made quite a similar program a little while back that always tries to keep pointing in the original direction. I used what is known as a "Lazy Susan" to turn the robot without needing to pick it up. A Lazy Susan is a wooden or plastic disk that can rotate freely on a foot. You usually have it in the middle of a table with sauces and dips and things on it. They make this kind of programming and testing a lot easier :)

The only thing I can add to your program is to put in a few wait statements in your while loops. Updating the dial on the screen and motor speeds about 20 times a second is more than enough, so add something like wait1ms(50); there.

I saw you subscribed to my YouTube channel, thank you :)

Regards,
Xander

Author:  mohem [ Tue Mar 24, 2009 3:55 pm ]
Post subject:  Re: NXT Compass (Motorized!)

Salam ;)

I tried to add the wait (since 20ms is the fastest I can achieve in the compass, it makes sense ) but it gave me some lag in the responsiveness of the motor , then I decided to remove them at least for the motor movement .

I knew what you meant by the rotating table (I used my microwave's once on my table ;))

I'm researching now some algorithms for the line follower and some optimization. any suggestion for places ???


ThX

Author:  mightor [ Wed Mar 25, 2009 2:04 am ]
Post subject:  Re: NXT Compass (Motorized!)

Having no wait in your motor loop can make it hard for your robot to switch tasks. If 20ms is too much, try 10 or even 5. I think you should use a PID (or at least the P-part of that) to determine the speed of your motor. Say for example, if we're only 20 degrees out of alignment, you would power the motor at, say, 20%, at 40 degrees, you would power it 40%. Try different amounts of correction depending on the deflection. Remember, though, you need keep the speed of the motor above about 10, or it simply doesn't have enough power to do anything.

As for line following, I found this PDF to be very useful: http://www.fll-freak.com/misc/01-jgray_report.pdf, I recommend you check it out. It's for an RCX based robot, but the algorithms still apply.

Regards,
Xander

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