Would anyone be willing to share with me your autostacking code, or a super general version of it, even just pseudocode? If so, please PM me since I will have questions.
PM sent
If anyone doesn’t mind, I would also like to see a general version since our team is doing a redesign for worlds.
I would appreciate greatly if someone could share some pseudo code. At this point I don’t even know the "ingredients ". Thanks to anyone who can share wisdom.
If someone could help me with this too, that would be great. My team has struggled with thing for about a month.
The basic idea is that there are presets for each cone stack. You have it so that one button press = one cone. From there you can add a subtracting button and what not until you have it nicely. For driver loads I would have it so that you hold down one joystick and while its held down it runs your driver load presets and then you can put the joy stick back up to pause it. This was one thing I have seen I do not know what other teams use
It really depends on your robot design, general controls structure and the level of automation you need.
One extreme would be going to the match load tray and pressing a button once for 12 cones (with another button/press to bail out early), but more reasonable middle ground is initiating parts of the sequence and mostly benefit from robot doing the precision moves and sequencing.
A robot with a lift, an arm and an active claw (typical RD4B garden variety) may need to perform 8 steps each cycle and actually counting the cones is the least of the problem. My students started with full manual (driver needed to initiate all 8 steps manually), then grouped safe subsequences. Then added the cone counting, which allowed them to build more automation on top of that.
The full sequence (worst case design, I’d say):
- Close claw (optional for passive intake)
- Lift up
- Arm up
- Lift down a bit (to seat the cone on the stack)
- Open claw (for passive release, you can skip this and some other steps)
- Lift up
- Arm down
- Lift down
First automation that came was 5-8. If the driver opens the claw in the “arm up” state, all 4 things happen,
with the “8. Lift down” going to a fixed preset.
Second obvious thing is, the above sequence is perfect place to count the cones.
That allows 3rd step - letting the robot decide how high the lift needs to go at step 2, from where you can immediately cascade 3, 4 and 5. Or skip 4 and 6, while triggering the rest of the sequence, which gets us to 1-2 button presses per cone. Of course with reasonable checks in place so the robot recognize something is amis and aborts - you already have sensors everywhere, don’t you?
If you have the room, you can put an ultrasonic sensor on your lift to eliminate the need for preset arm heights. The arm can just raise until the sensor sees that there isn’t a cone in front of it, so you know it’s high enough.
Although I will note that this late into the season, drivers are generally experienced enough that they’re actually faster than an autostack program. Especially if you have partner controls where one can drive while the other stacks.
This is something I don’t really understand. As a programmer, I can tune the autostacking to be as fast as possible, with timing and precision beyond that of human capacity. Assuming the hardware is sound, I don’t see what could possibly make a human faster than an optimized autostacking program.
@sazrocks it’s just what the self-confident drivers say. Maybe because they feel bored when not pressing all those buttons in those few hundred millis between the cones. All of them. But you want to have self-confident drivers 
@nenik I’m a self-confident programmer actually, but sure keep thinking I’m the driver just bc of the side of the debate I’m on.
Honestly, I can’t tell you why. I used to believe the exact same thing as you, even arguing with several other forum members about how programming will always be faster than a human if properly executed. I spent over 48 total hours coming up with an extremely fast, consistent, efficient autostack program. There was no way to physically make it better.
Then, my driver one day started trying to practice stacking themselves in case the algorithm was every messed up in a match. At first they were very obviously slower than the program, but over the course of several days they somehow got faster than it. I honestly can’t tell you exactly what it is, because I myself am not certain.
So please, it’s not that I’m an overconfident driver (in fact, I’m not even the driver, I’m the programmer, so if anything I should be on your side). If I’m not on your side, it’s because I was originally, but then I opened up my point of view to something new and tried it, and it actually proved itself to be better. That’s all.
Not to knock you, but it seems like the PID loop was not well tuned. The motors should reach full power, or you are not using them to their fullest extent, which would explain why the driver could be faster. A good compromise if you are noticing that some driver control is needed, though, can be letting the driver drop the cone when they determine it is aligned, while they can wait a little longer if there is some wobbling or something.
Here’s my robot autostacking for an example. It’s much better than my automatic control of the main lift. Also, this isn’t even as fast as it got. It was noticeably faster at State (unfortunately, we had unrelated issues). But, I have been told on the basis of this video that my autostacking program seems to be one of the better ones in the world (how this could possibly hold true is beyond me; you should probably be able to do better).
Man I wish I knew how to code like that. Quick question, do I need to use a PID to get the DR4B to hold still. If I do, what would be the easiest way to go about it?
2496V has used a semi-autostack (I am reluctant to say autostack becasue I’m not entirely sure what qualifies as an “autostack”) in competition and found it to be fairly useful and with few failures, but does have certain drawbacks.
pros - less wasted movements, easier to drive, cleaner stacks potentially. cons- failure prone from sensors, less fine control, lift PID can break itself when caught, and other usual PID problems
Our program can be found here. Relevant files will be src/opcontrol.cpp, src/dr4b.cpp, and src/integrator.cpp (referring to the 4b translational mechanism).
Here is a video of it in action during competition:
We found through some testing that a fully automatic program was not ideal. The driver still preferred the ability of manual control on our claw and control over when the stack code itself was triggered. Our button layout essentially uses the 4 “trigger” buttons. They are as follows:
**5U - Claw open, 5D- Claw close, 6U - Stack cone, 6D- Return to ground position
**Thus leaving the driver to intake cones manually and manually release the cone at the top, but otherwise the movement of the lift and 4b is fully automatic.
We tried a similar version of this program that was a single button control (hit button, grab cone automatically, stack, release, and return to ground) but found this to be considerably less reliable and potentially wasting cycle time if a cone missed for any number of factors. Our system allowed canceling at any time. However, this being said if more development time was spent I’m confident it is possible for a more complete and much better autostack. I hope this helps someine. Code is a bit messy, but if you do make something really cool out of it lmk or maybe even open a PR 
!
Bear in mind, at some point no amount of code can make up for driver skill and practice. We use “autostack” but it would be entirely useless without our talented drivers. If anything, the autostack should be viewed an assist so that drivers can focus more on positioning and gameplay/strategy.
I’d highly recommend making sure your lift can hold itself up in every position with only rubber bands, no motor power required. This is a great and frequently cited resource for getting started on an actual PID loop. If you have a task running PID managing the height, then all you need to control the lift is to tell it what its target value is. Break up your autostacking into a lot of parts that each run automatically. That’s what I did, because I initially had a manual/automatic hybrid. It’s still a little hybrid, but basically automatic.
I am neither the programmer nor the driver. I am a mentor. And a data scientist when necessary. When in doubt, we do a datalog (well, I always suggest a datalog anyway :-)). When you get a datalog from the driver and overlay it with the datalog from the program, you’ll see where the differences are. It might be the PID tuning or it might be some unnecessary delays in the program, where the driver may decide to initiate the next move little earlier (e.g. the arm doesn’t need to be at 0 degrees to send the lift down). It may be the speed vs. reliability, where the driver clearly sees when he can cut the corners, while your sensors won’t give you that kind of feedback/confidence.
Ultimately, the driver’s eyes are the best sensors. On the other hand, the the driver’s cortex has worse lag than your Cortex
What you learn from the driver log can often apply to the program.
If you see the driver consistently initiates the lift movement at arm angle of 30, you can replicate that, for example. We have learned that with well positioned lift after “step 2” of my overview, the driver skips steps 4. and 6. with no loss in reliability. So the code now does the same (with maybe one of 8 cones not stacked properly, still working on that). The data may also reveal hard to replicate behaviors, such as the driver triggerring the claw early when he’s confident the cone was grabbed really well. The program can’t do that - no sensor to report that little detail.
From the log, you can also read your lift speed and from that derive the loading/torque on the motors. And maybe go with faster gear.
But ultimate speed is elsewhere - instead of chasing milliseconds on a complicated routine, you could build a different intake design, allowing simpler routine and making all of the above pointless.
I’ve seen that video. My autostack is about the same speed, maybe faster, and my lift is 2 motor 1:7 hi-speed and my secondary lift 1 motor 1:3 torque. Still, my driver stacks faster 
EDIT: My PID is tuned as finely as can be. Any more aggressive of a P and it shakes too much. It’s just perfectly smooth yet fast. I also start the secondary lift moving way before the primary lift is done lifting, so no time wasted there. Point being, the entire process is tuned to be as fast as can be.
Also, side note, the buttons for the secondary lift are automated. Pressing the button will move the chainbar to either the stacking position or the grabbing position. But the lift and rollers are on manual controls.
The Resistance (86868R) is a one-man team, so he has had a complete auto-stack button since the first match of the season. When he’s using it, you can see that he just holds down one button on the joystick while he puts driverload cones on the platform with the other. It’s a beauty.