If you are trying to control a pickup arm to an angle, you will need to implement a PID controller that will requires understanding the PID class, or knowing enough about PID to slap together your own rudimentary solution.
EX. (I'm a Labview/Python programmer, don't expect this to be exact)
Code:
double error = 0; //outside loop
double integrated_error = 0; // outside loop
double proportional_coeff = .025; //an example P value for a PI control (~1/41.6,)
double integral_coeff = 00025; //an example I value PI control
double angle = 41.6;
double enc_counts_to_angle_conv = PUT A VALUE HERE ;(Your encoder's counts per revolution / 360)
double pickup_power = 0; \\initialize pickup to a safe value
public static void Timer(); \\sets up a timer because the PI controller needs double loop_time = 0;
double last_loop_total = 0;
/** inside loop (teleop/autonomous periodic) */
start()
public static void start(); //start the timer
pickup_power = proportional_coeff * error + integral_coeff * integrated_error;
if (pickup_power >= 1) { pickup_power = 1;}
pickupTalon.set(pickup_power);
error = pickupEncoder.get() * enc_counts_to_angle_conv - angle; //get error in degrees
loop_time = ( public void get() - last_loop_total ) / 1000;
last_loop_get = public void get();
integrated_error += error * loop_time ;
I wrote this so you could understand more of what a PID class object would be doing, and how the coefficients you give it are being used. This may or may not work, but even if it does, you are infinitely better off using the PID class. Just set D to 0 if you don't want to use it.
In PID language, your angle is your setpoint. I didn't do it in this code, but to switch between having your arm in or out you would switch your setpoint between 0 and 41.6.
PID documentation:
http://robotics.francisparker.org/ja...ontroller.html
Note that your integrated error will be based on a 50ms loop, lower your I coeff accordingly.
Also, I forgot, you need to reset your integrated error once in a while. Like when you're within a certain tolerance of your setpoint.
http://robotics.francisparker.org/ja...ibj/Timer.html Also, here's the timer class I was using for this example.