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






Reply to topic  [ 26 posts ]  Go to page 1, 2  Next
Troubles with encoders 
Author Message
Rookie

Joined: Mon Jun 11, 2012 9:28 pm
Posts: 37
Post Troubles with encoders
Hello again,

I have just written some code for a motor controller that makes the motor move at one rotation every two seconds:

Code:

int beltSpeed = 5;

// Code here, blah blah

task beltMove()

//makes the belt go at precisely 1 revolution every 2 seconds.

{

  ClearTimer(T1);

  int time = 0;
  float error = 0;

  while(true)
   
  {
 
    motor[belt] = beltSpeed;
   
    time = time1[T1];
 
    if(time > 2000 / 32){
   
      ClearTimer(T1);
     
      error = (360 / 32) - nMotorEncoder[belt];
     
      beltSpeed += error;
     
      nMotorEncoder[belt] = 0;
   
    }
   
  }

}

// task main, starts the task.



It works well, however, the problem here is that I have to reset the motor encoder every iteration. This makes it useless for other aspects of the program.

Any advice on how I can do this without resetting the encoder?

Thanks,

sqiddster


Fri Jun 15, 2012 6:01 pm
Profile
Expert

Joined: Thu Sep 29, 2011 11:09 pm
Posts: 184
Location: Michigan USA
Post Re: Troubles with encoders
Use an offset.

Do something like this:
Code:
int beltSpeed = 5;
int offset = 0;

// Code here, blah blah

task beltMove()

//makes the belt go at precisely 1 revolution every 2 seconds.

{

  ClearTimer(T1);

  int time = 0;
  float error = 0;

  while(true)
   
  {
 
    motor[belt] = beltSpeed;
   
    time = time1[T1];
 
    if(time > 2000 / 32){
   
      ClearTimer(T1);
     
      error = (360 / 32) - (nMotorEncoder[belt] - offset);
     
      beltSpeed += error;
     
      offset += ; // fill in the offset addition to whatever you want   
    }
   
  }

}

// task main, starts the task.

_________________
Matt


Fri Jun 15, 2012 6:50 pm
Profile WWW
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1347
Post Re: Troubles with encoders
Do you mean something like this? What's wrong with clearing the encoder in every loop? If you don't clear it then you need to make sure the encoder value doesn't wrap around if you run it too long.
Code:
task main()
{
    int power = 5;
    float period = 2.0/32;// in seconds
    float currTime;       // in seconds
    float prevPos = 0;    // in revolutions
    float currPos = 0;    // in revolutions
    float currSpeed = 0;  // in revolutions per second
    float targetSpeed = 0.5;// in revolutions per second
    float Kp = 1.0;    // need to tune this proportional constant

    //
    // Assuming you are not running it long enough for the encoder value to wrap around.
    //
    nMotorEncoder[belt] = 0;
    ClearTimer(T1);
    while (true)
    {
        currTime = time1[T1]/1000.0;
        if (currTime > period)
        {
            ClearTimer(T1);
            currPos = nMotorEncoder[belt]/360.0;
            currSpeed = (currPos - prevPos)/currTime;
            power += Kp*(targetSpeed - currSpeed);
            motor[belt] = power;
            prevPos = currPos;
        }
    }
}


Fri Jun 15, 2012 7:19 pm
Profile
Rookie

Joined: Mon Jun 11, 2012 9:28 pm
Posts: 37
Post Re: Troubles with encoders
@mattallen37, not sure what you mean there exactly.

@MHTS, that looks interesting. So it's another way to do what I did?
In fact, I am both running it long enough for the values to wrap, and I need to use it for another application.

I might as well post this now, I don't want a whole program written however some advice as to which direction to take would be nice.

I am making a GBC (great ball contraption) color sorter, with a central belt and two arms. The arms push the balls off to whatever side they should go on, and thus sort the balls. The arms are not each assigned a color - rather, each arm should push every second ball.
Yes, it is gratuitously inefficient but this is the point of the gbc.
The belt ticks forward twice every revolution, and the current rate gives the standard GBC 1bps throughput rate.
There is a clocker connected to the belt that lets each ball available fall into its slot. a color sensor scans the ball awaiting the clocker and so we can determine the color of the ball a certain amount of 'degrees away' if you will. I also know the amount of 'degrees' from the scan point to each arm.

Image

Hopefully I was some help. Basically, the problem lies in the wrapping of the rotation values, as well as not resetting as the motor encoder needs to be used for both the belt regulator and each arm's distance measure.
Oh, the arms are not evenly offset either - i.e. when the ball in the slot is below one of the arms, the other arm does not have a slot below it.

I hope this makes sense, please let me know if you can help. again, I do not want you to write up a whole program for me, I am just a bit lost as to how to achieve this. I am thinking 1-d arrays to store the balls colors? No idea how to implement this...

Thanks again!


Fri Jun 15, 2012 8:40 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1347
Post Re: Troubles with encoders
If you need to run it long enough to have the encoder values to wrap, you may want to reset the encoder in every iteration. You still can keep track of the "distance moved" by having a cumulative variable before you reset the encoder. For example:
Code:
task main()
{
    int power = 5;
    float period = 2.0/32;// in seconds
    float deltaTime;      // in seconds
    float deltaPos = 0;   // in revolutions
    float pos = 0;         // in revolutions
    float speed = 0;      // in revolutions per second
    float targetSpeed = 0.5;// in revolutions per second
    float Kp = 1.0;    // need to tune this proportional constant

    nMotorEncoder[belt] = 0;
    ClearTimer(T1);
    while (true)
    {
        deltaTime = time1[T1]/1000.0;
        if (deltaTime > period)
        {
            ClearTimer(T1);
            deltaPos = nMotorEncoder[belt]/360.0;
            nMotorEncoder[belt] = 0;
            pos += deltaPos;
            speed = deltaPos/deltaTime;
            power += Kp*(targetSpeed - speed);
            motor[belt] = power;
        }
    }
}


Sat Jun 16, 2012 12:45 am
Profile
Moderator
Moderator
User avatar

Joined: Wed Mar 05, 2008 8:14 am
Posts: 3165
Location: Rotterdam, The Netherlands
Post Re: Troubles with encoders
Even at full speed (!170RPM), it would take a about ~585 hours of continuous running for the encoder values to wrap. That's more than 24 days!

You need to change the way you calculate your RPM. The encoder delta between the previous and current iteration is always whole, so there is no need to use a float there. The delta will also be small, even if you only poll every second, it would be around 1000 at full speed.

pseudo code:
while true
delta_encoder = nMotorEncoder[motor] - previous_encoder;
previous_encoder = nMotorEncoder[motor];
rpm = (delta_encoder * (1000.0 / time1[T1])) / 360.0;
time1[T1] = 0;
wait a bit

Calculations:
encoders are signed longs, so range between -2147483648 and 2147483647
at 360 ticks per revolution, that leaves us with 2147483647 / 360 = 5965232 revs.
at 170 rpm, that's about 5965232 / 170 = 35089 minutes
at 60 minutes per hour, that's 35089 / 60 = 585 hours
at 24 hours per day, that's 585 / 24 = 24 days (and a few hours)

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


Sat Jun 16, 2012 2:35 am
Profile WWW
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1347
Post Re: Troubles with encoders
According to the RobotC documentation unless they have changed it in the latest version:
Quote:
nMotorEncoder[]
Current value of the motor encoder. Range is -32768 to 32767 so it will "wrap" after about ~90 revolutions. The user's program should reset the value of the encoder often to avoid the value "resetting" itself when the maximum distance is met.

nMotorEncoder is a 16-bit signed value and it will wrap around 90 revolutions. So at 170 RPM, you will wrap around at about half minute. It's true that you don't need to use float to represent speed. The code was just to demonstrate calculating the speed with an easy to understand unit (revolution per second). You can certainly represent it with an integer and multiply it with a scale factor of 1000 to reduce precision loss. I converted it to float just because the FRC library always normalizes units to float between -1.0 and 1.0 so I got used to that scale.
Besides, even if it takes 24 days to wrap the encoder value, I don't want to make the assumption that the machine will not run that long. I've seen some stores that have lego demo in their display window running day and light non-stop. So it could easily run beyond 24 days :)


Sat Jun 16, 2012 3:36 am
Profile
Moderator
Moderator
User avatar

Joined: Wed Mar 05, 2008 8:14 am
Posts: 3165
Location: Rotterdam, The Netherlands
Post Re: Troubles with encoders
From the include file RobotCIntrinsics.c:
Code:
#if defined(bAssociateMotorWithEncoderSensor) || defined(bSmartMotorsWithEncoders)
  intrinsic long variable(nMotorEncoder,                  opcdSourceMotorEncoder, kNumbOfTotalMotors, tMotor);
#endif

..and in loadBuildOptions.h, bSmartMotorsWithEncoders is defined for the NXT platform.

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


Sat Jun 16, 2012 3:57 am
Profile WWW
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1347
Post Re: Troubles with encoders
That's true. So the RobotC documentation is inaccurate!


Sat Jun 16, 2012 4:00 am
Profile
Moderator
Moderator
User avatar

Joined: Wed Mar 05, 2008 8:14 am
Posts: 3165
Location: Rotterdam, The Netherlands
Post Re: Troubles with encoders
MHTS wrote:
That's true. So the RobotC documentation is inaccurate!

That's *never* happened before! *grin*

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


Sat Jun 16, 2012 4:30 am
Profile WWW
Expert

Joined: Tue Feb 28, 2012 3:10 pm
Posts: 195
Post Re: Troubles with encoders
use more sensors. instead of depending on a fixed speed belt and perfect timing, just trigger when a ball passes a certain point. would that work and actually make it simpler?

edit: thought about it some more; I know your idea of a GBC is to amaze with unnecessary complexity, but will you be showing your code as well? Its even simpler than I was thinking. Move the belt X ticks at any speed, do something, (reset ticks if you want, repeat). the process of trying to maintain a constant speed, then convert that into a distance, then time some event to that, is overkill.

_________________
Mike aka Spiked3
http://www.spiked3.com


Last edited by Spiked3 on Sat Jun 16, 2012 3:40 pm, edited 1 time in total.



Sat Jun 16, 2012 5:39 am
Profile
Rookie

Joined: Mon Jun 11, 2012 9:28 pm
Posts: 37
Post Re: Troubles with encoders
@mightor

Well, I am glad to hear the documentation is wrong! That eliminates the wrapping problem...
I think I see what you are saying. Let me give it a try.


Sat Jun 16, 2012 9:45 am
Profile
Rookie

Joined: Mon Jun 11, 2012 9:28 pm
Posts: 37
Post Re: Troubles with encoders
I'm having trouble coming up with an RPM value.

Not sure what you did here: rpm = (delta_encoder * (1000.0 / time1[T1])) / 360.0;

but it's not working for me.

also, after that, I assume I do something like beltSpeed += (intendedRPM - rpm) * kp ?
is that correct?
Thanks again!


Sat Jun 16, 2012 10:38 am
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1347
Post Re: Troubles with encoders
sqiddster wrote:
I'm having trouble coming up with an RPM value.

Not sure what you did here: rpm = (delta_encoder * (1000.0 / time1[T1])) / 360.0;

but it's not working for me.

also, after that, I assume I do something like beltSpeed += (intendedRPM - rpm) * kp ?
is that correct?
Thanks again!

Actually, that equation is to generate rps (revolutions per second) not per minute:
delta_encoder/360.0 gives you delta_revolutions
time1[T1]/1000.0 gives you delta_seconds
rps = delta_revolutions/delta_seconds
rps = (delta_encoder/360.0)/(time1[T1]/1000.0)
rps = (delta_encoder*1000.0)/time1[T1]/360.0

And what do you mean by "not working"?


Sat Jun 16, 2012 12:53 pm
Profile
Moderator
Moderator
User avatar

Joined: Wed Mar 05, 2008 8:14 am
Posts: 3165
Location: Rotterdam, The Netherlands
Post Re: Troubles with encoders
I am half not good at code, half not good at hardware and half not good at math.

Sorry :)

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


Sat Jun 16, 2012 3:58 pm
Profile WWW
Display posts from previous:  Sort by  
Reply to topic   [ 26 posts ]  Go to page 1, 2  Next

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.