[FTC]: Servo to button programming teleop

[jnoyeslaser]

I am a first year FTC coach trying to learn the world of RobotC programming after years of FLL coaching. My team is stuck on programming their two arms to pick up blocks. The following code is our attempt. The robot will drive using joysticks but we can't make the second controller move the arm nor can we make the buttons work. I have looked through the post both here and on the robotc forum but find nothing to help me function them the way the kids would like. They want the trigger buttons to open and close their "hand" servo, the 1-4 buttons to move the "shoulder or elbow" servo up and down, and the DC motor "lift" to be moved by the joystick on controller 2. Anything you can give us would be great help as we compete in 2 weeks. Thank you so much

#pragma config(Hubs, S1, HTMotor, HTMotor, HTServo, none)
#pragma config(Sensor, S1, , sensorI2CMuxController)
#pragma config(Motor, mtr_S1_C1_1, motorRight, tmotorTetrix, openLoop)
#pragma config(Motor, mtr_S1_C1_2, motorLeft, tmotorTetrix, openLoop, reversed)
#pragma config(Motor, mtr_S1_C2_1, motorLift, tmotorTetrix, openLoop)
#pragma config(Motor, mtr_S1_C2_2, motorL, tmotorTetrix, openLoop)
#pragma config(Servo, srvo_S1_C3_1, servoTleft, tServoStandard)
#pragma config(Servo, srvo_S1_C3_2, servoAleft, tServoStandard)
#pragma config(Servo, srvo_S1_C3_3, servoTright, tServoStandard)
#pragma config(Servo, srvo_S1_C3_4, servoAright, tServoStandard)
#pragma config(Servo, srvo_S1_C3_5, servo5, tServoNone)
#pragma config(Servo, srvo_S1_C3_6, servo6, tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//

#include "JoystickDriver.c"
void initializeRobot()
{
// Place code here to sinitialize servos to starting positions.
// Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.
servo [servoAleft] = 250;
servo [servoAright] = 150;
servo [servoTleft] = 100;
servo [servoTright] = 100;


return;
}

int RarmPos = 0;

task main()
{
//servoAleft needs to be set//
initializeRobot();

waitForStart(); // wait for start of tele-op phase

{
servo [servoAleft] = 250;
servo [servoAright] = 150;
servo [servoTleft] = 100;
servo [servoTright] = 100;

while(1 == 1)
{
getJoystickSettings(joystick);
//deadzone on drive motor on joy1
if(joystick.joy1_y2<5 && joystick.joy1_y2>-5)
{
motor[motorLeft] = 0;
}
else
{
motor[motorLeft]= joystick.joy1_y2/2;
}
if(joystick.joy1_y1<5 && joystick.joy1_y1>-5)
{
motor[motorRight] = 0;
}
else
{
motor[motorRight] = joystick.joy1_y1/1.5;
}


//right arm tele two tests have been tried

// if(joy2Btn(5))
//{
//servo[servoAright] = 200; //Open
//}
//if(joy2Btn(7))
//{
//servo[servoAright] = 75; //Closed
// }
if (joy2Btn(6))
{
RarmPos += 5;
if (RarmPos > 255)
{
RarmPos = 255;
}
}
else if (joy2Btn(8))
{
RarmPos -= 5;
if (RarmPos < 0)
{
RarmPos = 0;
}
}
servo[servoAright] = RarmPos;



//Lift Motor joystick 2 Y1 (left joystick) This one doesn't seem to function properly

if(joystick.joy2_y1<5 && joystick.joy2_y1>-5)
{
motor[motorLift] = 0;
}
else
{
motor[motorLift]= joystick.joy2_y1/2;
}

//left arm tele



if (joy1Btn(5))
{
servo[servoAleft] = 200; //Open
}
if (joy1Btn(7))
{
servo[servoAleft] = 75; //Closed
}
//Top Servo Motors to work the shoulder of the arm using the buttons on the right

if (joy2Btn(1))
{
servo[servoTleft] = 20; //Closed
}
if (joy2Btn(3))
{
servo[servoTleft] = 125; //Open
}
if (joy2Btn(2))
{
servo[servoTright] = 20; //Closed
}
if (joy2Btn(4))
{
servo[servoTright] = 125; //Open
}
}
}
}