Automatic stacking help

In my last two weeks of robotics I have tried many, many, ways to make my robot stack cones automatically. I have tried just if, else if, and else statements, if statements that change a switch-case situations, and two switch-case situations that are in the same task as well as separate tasks to try to make my chain bar and lift stack these cones automatically.
I didn’t want to ask because I am stubborn and wanted to do this all by myself. So now I am sad to say that I am asking all of my fellow programmers how in the world did you do it? Make two mechanisms work together to stack cones or cubes for Skyrise, with out having the two interfere with each other.
I am using a quad encoder on the chain bar and (yes) a scissor lift with a Sonar sensor to tell when the lift has reached the top of the stack. I am down for any suggestions.
Thanks - Nick from 5813E

First, I suggest you change that quad encoder to a potentiometer.
Second, sonar sensors are unreliable so I would stay away from that. Instead of using a sonar sensor, you could use another potentiometer to track where the lift is. Then you could create something in the program in which you simply press a button on the joystick to raise the lift (one cone higher if you have successfully stacked the last cone).

Okay so I have thought about this solution, even tried it. 1) its tedious. 2) if you are to add another X to the scissor lift the point that was 1 cone high wouldn’t be the same as it was before. Scissor and DR4B do not lift up linearly. The Sonar sensor is trying to get rid of that problem altogether. I guess my question should have been If any body knows of a better way to make a state machine (basically) how would it be done? Is there another solution that I’ve missed?

those lifts move linearly just the sensors and the height of the lift do not directly correspond linearly. More like exponentially. Sorry for the confusion.

You could keep a counter for how many cones you think you have stacked and create an array corresponding to which height (i.e. sensor reading) to go to for x cones stacked.

Try a little process called linearization. For example, put a potentiometer somewhere on the robot’s scissor lift and for every angle you set the lift at, note how high the lift goes. Plot those on a graph with height for y and potentiometer angle for x. Look for what function of the angle the height increase is linear to. For example, let’s say the graph looks like a quadratic function (y = kx^2, where k is a constant of proportionality). Take that graph, square the x-values, then see if the new graph looks like a straight line. If so, you can say that the height goes up with the square of the x. If not, try another way of manipulating the x-values, like cubing the x-values, multiplying the tangent of x, etc. What you’re looking for is to define the equation y=mx+b (where y, in your case, is height, m is a constant of proportionality and b is the y-intercept. The real-world meaning of b is like a starting height, where for example, the claw could be 2 inches off the ground even at its fully lowered state.) and to define what x represents. X could be for example, the square of the angle. You can then take that equation, put a little statement in your code to calculate height based on the square of the angle and the starting height.

One note though: I used x^2 as an example only, not what the graph will look like for you. Based on the fact you’re using a scissor lift, the graph most likely will represent a trigonometric function, like sine, cosine, or tangent. Just plot it and see where it goes.