Create new intake command to allow orienting the totes manually

[jcorcoran]

It would be interesting to test a command which would allow the operator to manually re-orient a tote within the intake by allowing the direction of the two intake wheels to be manipulated (not just drive both in or both out). e.g. drive one wheel in while the other is driven out.

I think something like this would be a good place to start.

We may need to limit the rates of travel (along X)and we almost certainly need to add a deadband along X... maybe something like this will work:

double x = joystick.getLeftAxisX() //this is a made up method call, replace with the right one
double deadband = 0.5;

if(Math.abs(x) < deadband) {
  x = 0.0;
} else if (x > 0) {
  //positive X value greater than deadband
  x = x - deadband;  //limits the max output of the stick to (1- deadband)
} else {
  //negative X value greater than deadband
  x = x + deadband;  //limits the max output of the stick to (1- deadband)
}

//send x and y values from stick out to the motors

[jcorcoran]

In case the image isn't clear I was thinking the left wheel could be sent -y+x, right could be sent -(y+x). The graph shows the resultant actions for that equation.

Check the math, this is just an idea I had, if you have a better one or see an error in my logic, fix it, make it your own.