Despite my lack experience outside of EasyC, I tried writing some driver control code in PROS for a mecanum drive & flywheels.
For the drive, I converted the RobotC sample code for mecanum drive w/ deadzones. I did the flywheel code by myself. I don't have access to the robot as of right now to test it, so it would be great if someone could read through it & point out any issues.
void operatorControl() {
//Create "deadzone" variables. Adjust threshold value to increase/decrease deadzone
int X2 = 0, Y1 = 0, X1 = 0, threshold = 7;
//Loop Forever
while(1)
{
int rightStickLeft = -joystickGetAnalog(1, 3);
int leftStickUp = -joystickGetAnalog(1, 3);
int leftStickLeft = joystickGetAnalog(1, 4);
//Define Buttons
int button1 = joystickGetDigital(1, 8, JOY_UP);
int button2 = joystickGetDigital(1, 8, JOY_LEFT);
int button3 = joystickGetDigital(1, 8, JOY_DOWN);
int button4 = joystickGetDigital(1, 8, JOY_RIGHT);
int button5 = joystickGetDigital(1, 7, JOY_DOWN);
//Define Drive
int frontright = 3;
int backright = 9;
int frontleft = 2;
int backleft = 8;
//Sets left flywheels to 4&5, right to 6&7
int rightFW1 = 4;
int rightFW2 = 5;
int leftFW1 = 6;
int leftFW2 = 7;
int intake1 = 1;
int intake2 = 10;
//Sets motor speed presets
int full = 127;
int threequarters = 96;
int half = 63;
int quarter = 32;
//Create "deadzone" for Y1/Ch3
if(abs(leftStickLeft) > threshold){
Y1 = leftStickLeft;
}else{
Y1 = 0;
}
//Create "deadzone" for X1/Ch4
if(abs(leftStickUp) > threshold){
X1 = leftStickUp;
}else{
X1 = 0;
}
//Create "deadzone" for X2/Ch1
if(abs(rightStickLeft) > threshold){
X2 = rightStickLeft;
}else{
X2 = 0;
}
//Remote Control Commands
motorSet(frontright, Y1 + X2 - X1);
motorSet(backright, Y1 + X2 + X1);
motorSet(frontleft, Y1 - X2 + X1);
motorSet(backleft, Y1 - X2 - X1);
//if button 1 is pressed, set flywheels to 1/4 speed
if(abs(button1) == false){
motorSet(rightFW1, quarter),
motorSet(rightFW2, quarter),
motorSet(leftFW1, -quarter),
motorSet(leftFW2, -quarter);
//if button 2 is pressed, set flywheels to 1/2 speed
}else if(abs(button2) == false){
motorSet(rightFW1, half),
motorSet(rightFW2, half),
motorSet(leftFW1, -half),
motorSet(leftFW2, -half);
//if button 3 is pressed, set flywheels to 3/4 speed
}else if(abs(button3) == false){
motorSet(rightFW1, threequarters),
motorSet(rightFW2, threequarters),
motorSet(leftFW1, -threequarters),
motorSet(leftFW2, -threequarters);
//if button 4 is pressed, set flywheels to full speed
}else if(abs(button4) == false){
motorSet(rightFW1, full),
motorSet(rightFW2, full),
motorSet(leftFW1, -full),
motorSet(leftFW2, -full);
//if button 5 is pressed, coast flywheels to a stop
}else if(abs(button5) == false){
motorStop(rightFW1),
motorStop(rightFW2),
motorStop(leftFW1),
motorStop(leftFW2);
}
delay(1);}
}