Helpful encoder code I made
Page 1 of 1

Author:  natebot13 [ Sat Jan 19, 2013 12:29 am ]
Post subject:  Helpful encoder code I made

Those in FTC I think would really enjoy this code that I wrote for our robot. We have an arm with three motors that all need to be in the correct positions in order to score the three heights of the rack. We found that the manual control was tedious and time consuming, especially when we are under the pressure of a 2 minute match. We came to the conclusion that we needed to simply be able to push a button, and the robot would move to a specific position. I had tried using nMotorEncoderTarget, but I wasn't sure what it was doing, how it worked, and wanted to put myself up to the challenge of writing this myself. So that's how I came up with MotorToServo.c. This little include adds a method called moveTo() which, if there's an encoder on the motor, causes the motor to move to a specified position and holds it there as long as the method is constantly called. This works great inside of teleop while(true) loops as it will constantly set the motor to that specific value, and hold it there until the value is changed, or the method is no longer called.

Here's how it works:
Once the function is called, it checks the value of the encoder. Depending on where it is and what the target value is, it decided whether to move forward, backward, or not move at all. It also has a resolution option which allows the user to define how precise the movements must be, along with a definable minimum and maximum speed the motor should go.

Here's how to use it:
The function definition looks like this:
 bool moveTo(tMotor mot, int target, int minS = 10, int maxS = 100, int resolution = 10)

Once this is called, the motors will start moving if the encoder is not close to the target. You must call it continuously for it to work properly as the motors will not stop by themselves outside of the function, unless your code stops them manually elsewhere. Also, once the motors reach the destination, the function will return true stating that it has reached it's destination. This can be helpful if you want one motor to move at a time, each waiting for the last motor to reach where it needs to go before the next motor can start moving.
Here's an example of how we use it in our code:
    if(joystick.joy2_TopHat == 6)
        shoulderLoc = 0;
        wristLoc = 0;
        radiusLoc = 0;
        moving = true;
        once = true;
    if(moving == true)
        moveTo(wrist, wristLoc, 5, 50, 30);
        moveTo(radius, radiusLoc, 20, 50, 50);
        moveTo(shoulder, shoulderLoc, 5, 100, 10);
    else if(once == true)
        once = false;

This allows the motors to move to the correct locations as long as the left D-pad button is pressed. If we see that something went wrong, and it's not going to get to the correct destination, we can release the left button, and it immediately stops the motors, allowing us to use our normal, manual control.

Setting it up:
All you need to do to get this function is download the MotorToServo.c file, put it in the same directory as your main teleop program, and at the top of your code, under the #include "JoystickDriver.c" write:
#include "MotorToServo.c"

Then you can use the MoveTo() command anywhere in your program.

This will help us a TON in the next three tournaments this year, and I really hope it helps many other teams as well! Please, let me know if you will be using it, and let me know of any bugs, issues, fixes, suggestions, feedback, the list goes on. I would love to see what you guys think and be more in tuned to the FTC community!


MotorToServo.c [769 Bytes]
Downloaded 851 times

Author:  SergeantFTC [ Mon Jan 21, 2013 10:31 pm ]
Post subject:  Re: Helpful encoder code I made

Funny. I've been working on doing this exact same thing for a while now. The difference is, I just do the calculations to find the appropriate value to feed nMotorEncoderTarget. This means that you don't have to repeatedly call anything. The documentation on how that array works is pretty confusing, so I totally get that you gave up on it. I haven't gotten to test my code yet, but once I get it working and tested, I'll post it here. I didn't see you call an initialization function. How do you know that a particular encoder mark will always be in the same place?

Good job though, I kind of took the lazy way out by using the built-in stuff. :D

Author:  natebot13 [ Tue Jan 22, 2013 1:01 pm ]
Post subject:  Re: Helpful encoder code I made

Ya, the encoder target didn't make much sense to me, and I wanted something that I knew exactly what was going on behind the scenes. This gave me more versatality and I think more options. The encoder initilization happens with a separate program that when run, simply sets all encoders to zero. This is because we have to manually set where we want the base, zero point to be. Where our robot starts is not at the default position, and may not be an exact position.

This function, if it's in a usual teleop while(true) loop, is ideal since it will constantly be called. But I can see how simply setting nMotorEncoderTarget would only need to be called once. Does nMotorEcoderTarget hold its position once it has reached it's destination? Or does it stop the motors and not care anymore that the arm has fallen back down? Just curious.

I'd like to see your code and see how it uses encoder target. I hope you can get it to work!


Author:  SergeantFTC [ Tue Jan 22, 2013 2:17 pm ]
Post subject:  Re: Helpful encoder code I made

Basically, when you set nMotorEncoderTarget to a value, it just goes for that number of ticks. If you set it to negative, it won't look for the motor to go backwards, it will set the motor to "coast" mode. I assume that if you set the motor to turn backwards, it will stop after that number of ticks in the opposite direction, but I haven't tested that yet. One advantage to using it is that I believe it will have the motor slow to a stop.

I see. I handled initialization by having the motors move backwards until the encoder stops ticking, setting the encoders to zero, and setting the motor to the desired position. This way there is nothing for the user to forget, and also no way for the user to initialize the encoders to the wrong starting position.

Yeah, that's decent for TeleOp, but what about autonomous? Do you have to start another task? Also, it seems you have to hold down a button on the controller in TeleOp until the motor finishes moving, is that correct?

That is a GREAT point about it holding the motors. You're right, if the motor were to move after being set to a position, it wouldn't care. However, in practice, that isn't a big issue, because a motor that has electricity running to it at all is pretty hard to move, even if the power is set to zero. Also, we want the drivers to have manual control over the motors, so having it hold the spot isn't really desirable for us. Also, if it starts falling, you could just re-bump the button.

What team are you on?

--A fellow homeschooler

Author:  natebot13 [ Wed Jan 23, 2013 5:24 am ]
Post subject:  Re: Helpful encoder code I made

I see. Ya the code that I wrote makes the motor slow to a stop. It's the only way the Tetrix motors can stop at the correct position without passing the target.

For autonomois mode it's simply a while loop as the motors are moving to their destination. And once the motors have reached where they need to go, the while loop breaks. I see what you're saying you might need another task to keep calling the function, but like you said, the motors do have a little power running through them at all times, stiffening their movements, holding up arms.

Ya, as long as the button is pressed, the motors will move. This allows us to visually make sure that the motors are going to move correctly. If we see that the arm isn't positioned right (a gear slipped, an axle hub slipped, etc) then can simply let go of the button and the motors will stop moving to a unavailable destination, which could release some magic smoke.

Oh ya, and the reason the include is called MotorToServo.c is because it essentially turns a motor into a servo. So the exact same way that you might use a servo, (adding or subtracting from the servo position variable) you would program the motor the same.

So I guess what I mean by it holds it position is, only as long as the function is called; if not, the robot has its default motor braking that holds the motors even at zero power. And that's fine for our robot, and hopefully many other robots too.

I hope that clarifies some things.

I'm on team 4119, Brain Nuggets

Yay! Another homeschooler!

Author:  SergeantFTC [ Wed Jan 23, 2013 7:46 pm ]
Post subject:  Re: Helpful encoder code I made

Oh, ok, cool. How do you detect when it's done to break the loop? Timing?

I like the idea of being able to stop the motors from moving if necessary. I'm thinking that I might use your function for our linear slide lift. Maybe. I'll have to play with it.

Apparently that makes perfect sense to me, since I called my file "NXTMotorAsServoPOC.c" :)

Gotcha, that makes sense.

I'm on MOE 365 FTC. (

So, at our meeting last night I got my code for this working. I'd appreciate it if you'd give it a try and let me know if you can find any bugs in it (assuming that you have time of course). I'm attaching it.

NXTmotorAsServoPOC.c [1.59 KiB]
Downloaded 851 times

Author:  natebot13 [ Wed Jan 23, 2013 10:22 pm ]
Post subject:  Re: Helpful encoder code I made

Cool! We have a meeting tomorrow night and I'll see what I can test.

If you notice, the function is a bool, and I have it set so that once the motor has reached it's destination, the function returns true. So, for example you could have an empty loop like:
/*extra starting stuff here*/
shouldLoc = 1581;
while(MoveTo(shoulder, shouldLoc, 10, 100, 5) != true)
   /*do nothing until the motor has reached*/
stopMot(shoulder); //just in case the motor's didn't completely stop.
/*more stuff after the motor has moved*/

I knew this would come in handy not only for us! Maybe.


Page 1 of 1 All times are UTC - 5 hours [ DST ]
Powered by phpBB © 2000, 2002, 2005, 2007 phpBB Group