View unanswered posts | View active topics It is currently Wed Jul 30, 2014 7:16 am






Reply to topic  [ 4 posts ] 
NXT Compass (Motorized!) 
Author Message
Rookie

Joined: Mon Mar 23, 2009 9:22 pm
Posts: 5
Post 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

_________________
Artificial Intelligence is no match for Natural Stupidity !!!!


Mon Mar 23, 2009 9:40 pm
Profile
Moderator
Moderator
User avatar

Joined: Wed Mar 05, 2008 8:14 am
Posts: 3165
Location: Rotterdam, The Netherlands
Post 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

_________________
| 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]


Tue Mar 24, 2009 6:32 am
Profile WWW
Rookie

Joined: Mon Mar 23, 2009 9:22 pm
Posts: 5
Post 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

_________________
Artificial Intelligence is no match for Natural Stupidity !!!!


Tue Mar 24, 2009 3:55 pm
Profile
Moderator
Moderator
User avatar

Joined: Wed Mar 05, 2008 8:14 am
Posts: 3165
Location: Rotterdam, The Netherlands
Post 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

_________________
| 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]


Wed Mar 25, 2009 2:04 am
Profile WWW
Display posts from previous:  Sort by  
Reply to topic   [ 4 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.