Skip to content

Instantly share code, notes, and snippets.

@robotics5040

robotics5040/Teleop.c Secret

Created Dec 3, 2013
Embed
What would you like to do?
TeleOp program as of 12/3/2013
#pragma config(Hubs, S1, HTMotor, HTServo, HTMotor, HTMotor)
#pragma config(Sensor, S2, SensorIR, sensorHiTechnicIRSeeker1200)
#pragma config(Sensor, S3, SensorColor, sensorCOLORFULL)
#pragma config(Sensor, S4, SensorSonic, sensorSONAR)
#pragma config(Motor, mtr_S1_C1_1, motorD, tmotorTetrix, openLoop)
#pragma config(Motor, mtr_S1_C1_2, motorE, tmotorTetrix, openLoop)
#pragma config(Motor, mtr_S1_C3_1, motorF, tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor, mtr_S1_C3_2, motorG, tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor, mtr_S1_C4_1, motorH, tmotorTetrix, openLoop, encoder)
#pragma config(Motor, mtr_S1_C4_2, motorI, tmotorTetrix, openLoop, encoder)
#pragma config(Servo, srvo_S1_C2_1, servo1, tServoNone)
#pragma config(Servo, srvo_S1_C2_2, servo2, tServoNone)
#pragma config(Servo, srvo_S1_C2_3, servo3, tServoNone)
#pragma config(Servo, srvo_S1_C2_4, servo4, tServoNone)
#pragma config(Servo, srvo_S1_C2_5, servo5, tServoNone)
#pragma config(Servo, srvo_S1_C2_6, servo6, tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
//Motor F = Right Back
//Motor G = Right Front
//Motor H = Left Back
//Motor I = Left Front
//Motor D = Flag Manipulator Spin
//Motor E = Block Manipulator Lift
//Servo 1 = Flag Manipulator Tilt
/////////////////////////////////////////////////////////////////////////////////////////////////////
//
// DirectInput Button Notes
//
// Buttons 0=X 1=A 2=B 3=Y 4=LB 5=RB 6=LT 7=RT 8=Back 9=Start 10=LSD 11=RSD
// POV (D-Pad) -1=None 0-7=Positions (0=up, 1-7 +CW)
//
/////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////
//
// Tele-Operation Mode Code Template
//
// This file contains a template for simplified creation of an tele-op program for an FTC
// competition.
//
// You need to customize two functions with code unique to your specific robot.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////
#include "JoystickDriver.c" //Include file to "handle" the Bluetooth messages.
/////////////////////////////////////////////////////////////////////////////////////////////////////
//
// initializeRobot
//
// Prior to the start of tele-op mode, you may want to perform some initialization on your robot
// and the variables within your program.
//
// In most cases, you may not have to add any code to this function and it will remain "empty".
//
/////////////////////////////////////////////////////////////////////////////////////////////////////
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.
return;
}
/////////////////////////////////////////////////////////////////////////////////////////////////////
//
// Main Task
//
// The following is the main code for the tele-op robot operation. Customize as appropriate for
// your specific robot.
//
// Game controller / joystick information is sent periodically (about every 50 milliseconds) from
// the FMS (Field Management System) to the robot. Most tele-op programs will follow the following
// logic:
// 1. Loop forever repeating the following actions:
// 2. Get the latest game controller / joystick settings that have been received from the PC.
// 3. Perform appropriate actions based on the joystick + buttons settings. This is usually a
// simple action:
// * Joystick values are usually directly translated into power levels for a motor or
// position of a servo.
// * Buttons are usually used to start/stop a motor or cause a servo to move to a specific
// position.
// 4. Repeat the loop.
//
// Your program needs to continuously loop because you need to continuously respond to changes in
// the game controller settings.
//
// At the end of the tele-op period, the FMS will autonmatically abort (stop) execution of the program.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////
task main()
{
initializeRobot();
waitForStart(); // wait for start of tele-op phase
bool esMotors = false; //Toggled state of Motor E-Stop mode
bool esmHold = false; //If the Motor E-Stop button (B) is being held
while (true)
{
getJoystickSettings(joystick);
int btnDebug = joy1Btn(3);
//Emergency Stop
if (joy1Btn(9) == 1 && joy2Btn(9) == 1) //Program will stop when E-Stop button (start) hit on both controllers
{
while (joy1Btn(9) == 1 && joy2Btn(9) == 1) //Wait until the buttons are released
{
getJoystickSettings(joystick);
}
while (joy1Btn(9) != 1 || joy2Btn(9) != 1) //Wait until they are pressed again
{
getJoystickSettings(joystick);
}
getJoystickSettings(joystick);
}
//Motor Emergency Stop
if (joy1Btn(2) == 1 && !esmHold) //Will toggle motor e-stop when Motor E-Stop button (B) hit on controller 1
{
if (esMotors)
{
esMotors = false;
esmHold = true;
}
else
{
esMotors = true;
esmHold = true;
}
}
else
{
esmHold = false;
}
//Controller 1 - Motors
if (joy1Btn(10) == 1 || (joystick.joy1_y2 < 5 && joystick.joy1_y2 > -5) || esMotors) //Threshold & Preset Speed Button - RSD - 0
{
motor[motorG] = 0;
motor[motorF] = 0;
}
else
{
if (joy1Btn(7) == 1 && joystick.joy1_y2 > 0) //Preset Speed Button - RT - 75
{
motor[motorG] = 75;
motor[motorF] = 75;
}
else if (joy1Btn(5) == 1 && joystick.joy1_y2 > 0) //Preset Speed Button - RB - 30
{
motor[motorG] = 30;
motor[motorF] = 30;
}
else if (joy1Btn(7) == 1 && joystick.joy1_y2 < 0) //Preset Speed Button - RT - -75
{
motor[motorG] = -75;
motor[motorF] = -75;
}
else if (joy1Btn(5) == 1 && joystick.joy1_y2 < 0) //Preset Speed Button - RB - -30
{
motor[motorG] = -30;
motor[motorF] = -30;
}
else //No Presets - RS
{
motor[motorG] = joystick.joy1_y2;
motor[motorF] = joystick.joy1_y2;
}
}
if (joy1Btn(11) == 1 || (joystick.joy1_y1 < 5 && joystick.joy1_y1 > -5) || esMotors) //Threshold & Preset Speed Button - LSD - 0
{
motor[motorH] = 0;
motor[motorI] = 0;
}
else
{
if (joy1Btn(6) == 1 && joystick.joy1_y1 > 0) //Preset Speed Button - LT - 75
{
motor[motorH] = 75;
motor[motorI] = 75;
}
else if (joy1Btn(4) == 1 && joystick.joy1_y1 > 0) //Preset Speed Button - LB - 30
{
motor[motorH] = 30;
motor[motorI] = 30;
}
else if (joy1Btn(6) == 1 && joystick.joy1_y1 < 0) //Preset Speed Button - LT - -75
{
motor[motorH] = -75;
motor[motorI] = -75;
}
else if (joy1Btn(4) == 1 && joystick.joy1_y1 < 0) //Preset Speed Button - LB - -30
{
motor[motorH] = -30;
motor[motorI] = -30;
}
else //No Presets - LS
{
motor[motorH] = joystick.joy1_y1;
motor[motorI] = joystick.joy1_y1;
}
}
//Controller 2 - Block Manipulator & Flag Manipulator
if (joystick.joy2_y1 < 5 && joystick.joy2_y1 > -5) //Threshold
{
motor[motorC] = 0;
}
else //Tilt Block Manipulator - LS
{
motor[motorC] = joystick.joy2_y1;
}
if (joystick.joy2_y2 < 5 && joystick.joy2_y2 > -5) //Threshold
{
motor[motorE] = 0;
}
else //Lift Block Manipulator - RS
{
motor[motorE] = joystick.joy2_y2;
}
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.