View Single Post
  #1   Spotlight this post!  
Unread 26-01-2014, 03:33
mikets's Avatar
mikets mikets is offline
Software Engineer
FRC #0492 (Titan Robotics)
Team Role: Mentor
 
Join Date: Jan 2010
Rookie Year: 2008
Location: Bellevue, WA
Posts: 675
mikets is a glorious beacon of lightmikets is a glorious beacon of lightmikets is a glorious beacon of lightmikets is a glorious beacon of lightmikets is a glorious beacon of lightmikets is a glorious beacon of light
Re: [FTC]: Servo to button programming teleop

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.
Code:
#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);
    }
}
__________________
Reply With Quote