[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
}
}
}
}

[aklego]

I don't know if this is your problem but a really easy mistake to make is to plug the servos in upside down, either at the controller or at any extension wires. There is a "YRB" marking on the Tetrix controller which stands for Yellow Red Black and indicates the proper orientation of the wires.

[Just_In_Time]

I'm not entirely sure if this is the issue but you might want to try putting an "==1" after you assign a button number.
Each button assignment should look something like this:
if((joy1Btn(1) ==1));
{
//servo code here
}

Hope this helps.

JIT

[F Elliott]

The only issue I see with your code is I suspect joy1Btn(5) and joy1Btn(7) should be using joy2Btn(5) and joy2Btn(7). The rest looks fine.

Be sure you have the Joystick-Competition debugger window open and that the dual joysticks are turned on. Fiddle around with the settings in that window. I can't recall off the top of my head what combination works. When it's working you will see the button and joysticks in that window light up with white squares on either joystick. You can try this without running your program. I'll try to get you a screen shot tonight.

Also be aware that on the debugger window the buttons will be shown as 0-7 when pressed but the arguments to joy1Btn and joy2Btn take on corresponding values 1 to 8.

[mikets]

Assuming the robot is wired correctly, I can't find anything major that's wrong. However, I have optimized your code a little and made it more readable.

#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"

#define DEADBAND(n, b)      ((abs(n) > (b))? (n): 0)

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 (true)
    {
        getJoystickSettings(joystick);
        
        //deadzone on drive motor on joy1
        // mikets: why different divisors? Are you compensating the robot not running straight?
        motor[motorLeft] = DEADBAND(joystick.joy1_y2, 5)/2;
        motor[motorRight] = DEADBAND(joystick.joy1_y1, 5)/1.5;

        //
        // Joystick 2 controls the lift.
        //
        motor[motorLift] = DEADBAND(joystick.joy2_y1, 5)/2;

        //
        // Use joystick buttons 6 and 8 to raise and lower right arm.
        //
        if (joy2Btn(6))
        {
            RarmPos += 5;
            if (RarmPos > 255)
            {
                RarmPos = 255;
            }
        }
        else if (joy2Btn(8))
        {
            RarmPos -= 5;
            if (RarmPos < 0)
            {
                RarmPos = 0;
            }
        }
        servo[servoAright] = RarmPos;

        //
        // Use joystick buttons 5 and 7 to open and close hand.
        //
        if (joy2Btn(5))
        {
            servo[servoAleft] = 200; //Open
        }
        else if (joy2Btn(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
        }
        else if (joy2Btn(3))
        {
            servo[servoTleft] = 125; //Open
        }
        
        if (joy2Btn(2))
        {
            servo[servoTright] = 20; //Closed
        }
        else if (joy2Btn(4))
        {
            servo[servoTright] = 125; //Open
        }
        
        wait1Msec(50);
    }
}