Programming

We have nine programs:

• Do nothing -- only parks under the bridge
• Blue/Red-foundation-turn -- turns foundation horizontal and parks closest to wall.
• Blue/Red-fondation-no-turn -- pulls foundation vertically and parks closest to wall.
• Blue/Red-skystone-delivery -- delivers skystones and one regular stone, parks closest to neutral bridge.
• Blue/Red-skystone-delivery-placement-foundation- -- delivers skystone and places on foundation, while moving the foundation, parks closest to neutral bridge.

We made a PID controller which we use to go straight in our autonomous.

We made our own because we wanted to learn about how the proportional, integral, and derivative controllers work.

P: The proportional part of our controller changes as the error changes. So as our robot drifts, the power to the side needed increases because of the P. Our gain used for the proportional part is 0.02.

I: The integral part of our controller increases if the robot is stuck off course for a long period of time. We multiply the error by the delta time over a span of 0.01 seconds, and add the previous I to it, so that if the robot is stuck off target over some time, the I will come in and correct it. Our gain used for the integral part is 0.00045.

D: The derivative part of our controller smoothes out the sharper turns. We find it by finding the slope of the line over every 0.01 seconds. The gain used is 0.00009.

In the PID code, the speed ramps up, then stays at a constant speed, then ramps down to make a trapezoidal curve.

We have a class called common methods which contains all of our functions and all of our static variables. Printouts of code is below.

PIDStraightInches: This function uses PID to go forward straight a certain number of inches with encoders. It uses a trapezoid to change the speed to drive smoothly.

PIDSideInches: This function uses PID to go sideways straight a certain number of inches with encoders. It uses a trapezoid curve to change the speed to drive smoothly.

moveTurnDegrees: This function spins a certain number of degrees using encoders.

moveForwardInches: This function moves forward a certain number of inches using encoders.

moveSideInches: This function moves sideways a certain number of inches using encoders.

moveArchDegrees: This function moves in an arch a certain radius, and a certain number of degrees, we mainly use this to turn the foundation.

liftOrDropStones: This function either picks up or drops stones, using either servo, left or right.

getAngle: This function gets the current angle of the imu after initializing the angle value measurer.

absolute: This is the function we used to get the absolute value of a number, before our coaches informed us that we could use Math.abs.

We pre-programmed multiple levels to make it easier to stack the blocks.

We computed the angle for the arm to rotate by first finding the hypotenuse, then computing the sin because we know the base length. Then, we use the pre-computed motor tics per degree and multiply that by the angle.

Before going to this angle, we make sure the arm is contracted, and if not we contract it, then we go to that position with the encoder.

Next, we extend the arm by using the hypotenuse that was computed earlier, multiplied by a factor that we have and subtract the contracted arm length. Then we divide it by the spindle circumference, then we multiply it by ticks per rotation of the extension motor. Finally, we go to that position, and the arm is at that level