task main()
{
waitForStart();
while (true)
{
getJoystickSettings(joystick);
motor[Left] = joystick.joy1_y1; //Left Drive
motor[Right] = joystick.joy1_y2; //Right Drive
motor[Lift] = joystick.joy2_y1; //Both Lift Motors
motor[Wrist] = joystick.joy2_y2*0.5; //Wrist Motor
if (joy2Btn(8))
{
motor[Spinner] = 100; //Spins block intake
}
else if(joy2Btn(7))
{
motor[Spinner] = -100; //Reverses the spin
}
else
{
motor[Spinner] = 0; //Prevents spontaneous movement
}
if (joy1Btn(8)||joy2Btn(6))
{
servo[Flag] = 255; //Spins Flag
}
else if(joy1Btn(7)||joy2Btn(5))
{
servo[Flag] = 0; //Reverses the spin
}
else
{
servo[Flag] = 128; //Prevents spontaneous movement
}
if (joy1Btn(8)||joy2Btn(6))
{
servo[Flag2] = 255; //Spins Flag
}
else if(joy1Btn(7)||joy2Btn(5))
{
servo[Flag2] = 0; //Reverses the spin
}
else
{
servo[Flag2] = 128; //Prevents spontaneous movement
}
}
}
// Task to be run for changing the state of the goal grabber arms using an actib
task goalGrabber()
{
while(true)
{
wait1Msec(100);
if (joy2Btn(1) && goalgrabbed == false)
{
// Drop the servos to grab goal
servo[servoRight] = 100; // These are test values until proper ones are found
servo[servoLeft] = 100;
}
else
{
// Lift servos to release the goal
servo[servoRight] = 120;
servo[servoLeft] = 80;
}
}
}
task runDump(){
while(true){
/*if(joy2Btn(BUTTON_DUMP)){
score = (score + 1)%2;
wait1Msec(300);
}*/
if(joy2Btn(BUTTON_STOREDUMP)){
servo[dump] = 65; //STORE
wait1Msec(250);
score = 2;
}
/* if(score == 2){
score = 0;
}*/
if(score == 0){
servo[dump] = 94+PI; //REGULAR
}
else if (score !=2){
servo[dump] = 170; //DOWN
}
}
}
task main()
{
initializeRobot();
//waitForStart(); // wait for start of tele-op phase
while (true)
{
getJoystickSettings(joystick);
if (!debounce) {
if (joy2Btn(RIGHT_TRIGGER_UP)) {
handle_event(RIGHT_TRIGGER_UP);
} else if (joy2Btn(RIGHT_TRIGGER_DOWN)) {
handle_event(RIGHT_TRIGGER_DOWN);
} else if (joy2Btn(LEFT_TRIGGER_UP)) {
handle_event(LEFT_TRIGGER_UP);
} else if (joy2Btn(LEFT_TRIGGER_DOWN)) {
handle_event(LEFT_TRIGGER_DOWN);
} else if (joy2Btn(BUTTON_ONE)) {
handle_event(BUTTON_ONE);
} else if (joy2Btn(BUTTON_FOUR)) {
handle_event(BUTTON_FOUR);
} else if (joy1Btn(BUTTON_ONE)) {
handle_joy1_event(BUTTON_ONE);
}
}
if (abs(joystick.joy1_y2) > 20) {
motor[driveRight] = drive_multiplier * joystick.joy1_y2;
}
else {
motor[driveRight] = 0;
}
if (abs(joystick.joy1_y1) > 20) {
motor[driveLeft] = drive_multiplier * joystick.joy1_y1;
}
else
{
motor[driveLeft] = 0;
}
}
}
void processScoringMovement(){
if(scoreState==0){//in middle happy
if(joy1Btn(5)==1||joy2Btn(5)==1){
btn5Down=true;
if(joy1Btn(7)==1||joy2Btn(7)==1){//confirmation for stationary dispensal
wait10Msec(50);
scoreOnStat();
btn5Down=false;
}
}else{
if(btn5Down){//just released, so put to rolling goal dispensing position
moveScoreBoxTo(SERVO_SCORE_BOX_TILT_ROLL);
scoreState=1;
}
btn5Down=false;
}
}
if(scoreState==1){//about to dispense to rolling goal
if(joy1Btn(5)==1||joy2Btn(5)==1){//cancel
wait10Msec(50);
moveScoreBoxTo(SERVO_SCORE_BOX_TILT_MID);
scoreState=0;
}
if(joy1Btn(7)==1||joy2Btn(7)==1){//score
wait10Msec(50);
openScoreBoxDoor();
scoreState=2;
}
}
if(scoreState==2){//dispensing now
if(joy1Btn(7)==1||joy1Btn(5)==1||joy2Btn(7)==1||joy2Btn(5)==1){//close
wait10Msec(50);
closeScoreBoxDoor();
scoreState=1;
}
}
}
task main()
{
initializeRobot();
//Comment out for tests
//waitForStart();
///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////
//// ////
//// Add your robot specific autonomous code here. ////
//// ////
///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////
while (true)
{
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
/*+++++++++++++++++++++++++++++++++++++++++++++| Notes |++++++++++++++++++++++++++++++++++++++++++++++
Conveyorbot - Remote Control
- NOTE: Conveyorbot can be used to pick up 4 of the game objects with its intake and score them
anywhere on the field.
- This program allows you to remotely control your robot using the USB Logitech Joysticks.
- This program also ignores low values that would cause your robot to move when the joysticks fail to
return back to exact center. You may need to adjust the 'threshold' value to get it just right.
Robot Model(s): TETRIX Conveyorbot (Virtual Worlds)
[I/O Port] [Name] [Type] [Description]
Motor Port D rightMotor TETRIX Motor Right side motor
Motor Port E leftMotor TETRIX Motor Left side motor
Motor Port F armMotor TETRIX Motor Arm motor
Motor Port G intakeMotor TETRIX Motor Intake motor
Sensor Port 2 gyro HiTechnic Gyro HiTechnic Gyro
Sensor Port 3 light Light Sensor NXT Light Sensor (Active)
Sensor Port 4 sonar Sonar Sensor NXT Sonar Sensor
*** Competition Information ***
Name:
Competition:
Average FPS (displayed on the Virtual World):
*** Program Information ***
Name of Program:
Type of Robot:
Any other relevant information:
----------------------------------------------------------------------------------------------------*/
//Integer variable that allows you to specify a "deadzone" where values (both positive or negative)
//less than the threshold will be ignored.
while (1==1) //Loop Forever
{
//Get the Latest joystick values
getJoystickSettings(joystick);
//Variable Control
int threshold = 10; //Motor Variables
int wSpeed = 10;
int hookDown = 10; //Servo Variables
int hookUp = 10;
int bucketUp = 10;
int bucketDown = 10;
//Operator Control Joy 2
if(joystick.joy2_TopHat == 0) //Up and Down on D-Pad adjust bucket height
{
motor[wenchA] = wSpeed;
motor[wenchB] = wSpeed;
}
else if(joystick.joy2_TopHat == 4)
{
motor[wenchA] = -wSpeed;
motor[wenchB] = -wSpeed;
}
else
{
motor[wenchA] = 0;
motor[wenchB] = 0;
}
//---
if(joy2Btn(4) == 1) //Buttons 4 and 1 control hook position
{
servo[hook] = hookDown;
}
if(joy2Btn(1) == 1)
{
servo[hook] = hookUp;
}
//---
if(joy2Btn(2) == 1) //Buttons 2 and 3 control bucket position
{
servo[bucket] = bucketDown;
}
if(joy2Btn(3) == 1)
{
servo[bucket] = bucketUp;
}
//---
//Driving Control Joy 1
if(abs(joystick.joy1_y1) > threshold) // If the abs. value of the right analog stick's Y-axis readings are above the threshold...
{
motor[frontLeft] = joystick.joy1_y1; // ...move the right side of the robot.
motor[backLeft] = joystick.joy1_y1;
}
else // Else the readings are within the threshold, so...
{
motor[frontLeft] = 0; // ...stop the right side of the robot.
motor[backLeft] = 0;
}
//---
if(abs(joystick.joy1_y2) > threshold) // If the abs. value of the left analog stick's Y-axis readings are above the threshold...
{
motor[frontRight] = joystick.joy1_y2; // ...move the left side of the robot.
motor[backRight] = joystick.joy1_y2;
}
else // Else the readings are within the threshold, so...
{
motor[frontRight] = 0; // ...stop the left side of the robot.
motor[backRight] = 0;
}
}
}
}
task joystickTwo(){
//Check joystick two input here
getJoystickSettings(joystick);
if(abs(joystick.joy2_y1)>JOYTHRESHOLD){
}
else if(abs(joystick.joy2_y2)>JOYTHRESHOLD){
}
if(abs(joystick.joy2_x1)>JOYTHRESHOLD){
}
else if(abs(joystick.joy2_x2)>JOYTHRESHOLD){
}
if(joy2Btn(0)){
}
if(joy2Btn(1)){
}
if(joy2Btn(2)){
}
if(joy2Btn(3)){
}
if(joy2Btn(4)){
}
if(joy2Btn(5)){
}
if(joy2Btn(6)){
}
if(joy2Btn(7)){
}
if(joy2Btn(8)){
}
if(joy2Btn(9)){
}
if(joy2Btn(10)){
}
if(joy2Btn(11)){
}
if(joy2Btn(12)){
}
}
task main()
{
int threshold = 40;
/*//removed for testing
servo[Winch]=127;
*/
//
// going straight
//
int straight_FRS = 161;
int straight_BRS = 154;
int straight_FLS = 132;
int straight_BLS = 134;
// mandible servo positions
//
int openwideR = 240;
int openwideL = 0;
int shutR = 0;
int shutL = 228;
// mouth
int mouthup=60;
int mouthdown=214;
//
int elevatorposition=0;
int uppositionscore=62;
int downpositionscore= 5;
// gate
int gateup = 5;
int gatedrive = 50;
int gatedown = 202;
//
int lockup = 0;
int lockdown = 85;
//
int scoreopen = 5;
int scoreclose = 245;
int FRS =0;
int BRS = 0;
int FLS = 0;
int BLS = 0;
int correction=0;//correction for left motor
// servo initialization
servo[leftmandible] = openwideL;
servo[rightmandible] = openwideR;
// servo[mouth] = mouthup;
servo[score] = scoreclose;
servo[lock] = lockup;
servo[gate] = gateup;
//
// wheels go straight to start
//
servo[frontRS]= straight_FRS;
servo[backRS]= straight_BRS;
servo[frontLS]= straight_FLS;
servo[backLS]= straight_BLS;
servo[mouth]= mouthup;
int lastservopos = 0; // 1 = straight, 2 = spin , 3 = turn 45 deg to right, 4 = turn 45degrees to left, 5 = turn wheels to side
waitForStart();
//turnServos(1, lastservopos);
servo[gate] = gatedrive;
waitForStart();
while(true)
{
getJoystickSettings(joystick); //Constantly updates values on the joystick
// left joystick
if((abs(joystick.joy1_y1) > threshold)|| (abs(joystick.joy1_x1) > threshold) || (abs(joystick.joy1_x2) > threshold))
{
if ((abs(joystick.joy1_y1) > threshold) && (abs (joystick.joy1_x1) > threshold)) // going 45 degrees
{
BRS = FRS = BLS = FLS = -(abs(joystick.joy1_y1) * 90)/joystick.joy1_y1; // TAKE POWER AND DIRECTION from Y axis
if (((joystick.joy1_y1 * joystick.joy1_x1)) > 0) // if both positive or both negative go to right
{
turnServos(3, lastservopos); // turn wheels to right to go 45 degrees
lastservopos = 3;
} else
{
turnServos(4, lastservopos); // turn wheels to left to go 45 degrees
lastservopos = 4;
}
} else if (((abs(joystick.joy1_y1) > threshold)) &&(abs(joystick.joy1_x1) <= threshold)) // go straight
// if going forward or back
{
BRS = FRS = FLS = BLS = (abs(joystick.joy1_y1)*90)/joystick.joy1_y1;
// servos
turnServos(1, lastservopos);
lastservopos = 1;
// motors
} else if ((abs(joystick.joy1_x1) > threshold) && (abs(joystick.joy1_y1) <= threshold)) // go sideways
{
BRS = FRS = BLS = FLS = (abs(joystick.joy1_x1)*100)/joystick.joy1_x1; // positive = go right, negative = go left.
FLS*=-1;
// servos
turnServos(5, lastservopos); // turn wheels to right to go sideways
lastservopos = 5;
// motors
} else if (abs(joystick.joy1_x2) > threshold) // spin
{
FRS = BRS = -(abs(joystick.joy1_x2)*40)/joystick.joy1_x2; // positive = go right, negative = go left.
BLS = FLS = -FRS; // have to switch direction to make it spin.
// servos
turnServos(2, lastservopos); // turn wheels to right to go sideways
lastservopos = 2;
}
}else
{
BRS = FRS = BLS = FLS = 0;
}
// Now tell motors to go
motor[RightFront] = FRS;
motor[LeftFront] = FLS;
motor[RightBack] = BRS;
motor[LeftBack] = BLS;
// Mandible controls
if(joy1Btn(8))
{
servo[rightmandible] = shutR;
servo[leftmandible] = shutL;
}
if(joy1Btn(6))
{
servo[rightmandible] = openwideR;
servo[leftmandible] = openwideL;
}
// Mouth controls
if(joy1Btn(4))
{
servo[mouth]=mouthup;
wait1Msec(30);
servo[rightmandible] = openwideR; // open mandibles so they don't hit elevator
servo[leftmandible] = openwideL;
}
if(joy1Btn(2))
{
servo[mouth]=mouthdown;
}
///////////// JOYSTICK 2 //////////////////
// elevator controls
/*
if(abs(joystick.joy2_y2) > threshold)// absolute value so can be negative or positive
{
motor[elevator] = 100*joystick.joy2_y2/abs(joystick.joy2_y2);//Raises the elevator
}
else
{
motor[elevator] = 0;
}
*/
// scoring
if(joy2Btn(8))
{
servo[score] = scoreopen;
}
else
{
servo[score]=scoreclose;
}
// rolling goal
if(joy2Btn(2)) // gate down to capture rolling goal
{
servo[gate] = gatedown;
}
if (joy2Btn(4)) // gate up
{
servo[lock] = lockup;
wait1Msec(5);
servo[gate] = gatedrive;
}
if (joy2Btn(3)) // lock on
{
servo[lock] = lockdown;
}
if (joy2Btn(1)) // lock up - release rolling goal
{
servo[lock] = lockup;
}
}
}
task main() {
nVolume = 3;
waitForStart();
while(true) {
bFloatDuringInactiveMotorPWM = false;
getJoystickSettings(joystick);
if((joystick.joy1_y1 < 10 && joystick.joy1_y1 > -10) && (joystick.joy1_x1 < 10 && joystick.joy1_x1 > -10))
{
joystick.joy1_y1 = 0;
joystick.joy1_x1 = 0;
}
if((joystick.joy1_x2 < 10 && joystick.joy1_x2 > -10) && (joystick.joy1_y2 < 10 && joystick.joy1_y2 > -10))
{
joystick.joy1_x2 = 0;
joystick.joy1_y2 = 0;
}
/* uncomment following section for RC Car steering */
/*if (!revDir) {
if (!sloMo) {
motor[motorLeft] = (joystick.joy1_y1 + joystick.joy1_x2);
motor[motorRight] = (joystick.joy1_y1 - joystick.joy1_x2);
} else {
motor[motorLeft] = 0.16 * (joystick.joy1_y1 + joystick.joy1_x2);
motor[motorRight] = 0.16 * (joystick.joy1_y1 - joystick.joy1_x2);
}
} else {
if (!sloMo) {
motor[motorLeft] = -(joystick.joy1_y1 + joystick.joy1_x2);
motor[motorRight] = -(joystick.joy1_y1 - joystick.joy1_x2);
} else {
motor[motorLeft] = -0.16 * (joystick.joy1_y1 + joystick.joy1_x2);
motor[motorRight] = -0.16 * (joystick.joy1_y1 - joystick.joy1_x2);
}
}*/
/* uncomment following section for tank steering */
if (!sloMo) {
motor[motorLeft] = (joystick.joy1_y1);
motor[motorRight] = (joystick.joy1_y2);
} else {
motor[motorLeft] = 0.16 * (joystick.joy1_y1);
motor[motorRight] = 0.16 * (joystick.joy1_y2);
}
if (canSloMo) {
if (!sloMo) {
if (joy1Btn(12)) {
sloMo = true;
canSloMo = false;
PlayImmediateTone(400, 10);
}
} else {
if (joy1Btn(12)) {
sloMo = false;
canSloMo = false;
PlayImmediateTone(600, 10);
}
}
}
if (!canSloMo) {
if (!joy1Btn(12)) {
canSloMo = true;
}
}
if (canRevDir) {
if (!revDir) {
if (joy1Btn(11)) {
revDir = true;
canRevDir = false;
PlayImmediateTone(300, 10);
}
} else {
if (joy1Btn(11)) {
revDir = false;
canRevDir = false;
PlayImmediateTone(800, 10);
}
}
}
if (!canRevDir) {
if (!joy1Btn(11)) {
canRevDir = true;
}
}
// ------------------------------- End Drive Code -------------------------------
if (joy2Btn(5)) {
motor[motorLift] = 100;
} else if (joy2Btn(7)) {
motor[motorLift] = -100;
} else {
motor[motorLift] = 0;
}
if (joystick.joy2_TopHat == 0)
motor[motorBelt] = 100;
else if (joystick.joy2_TopHat == 4)
motor[motorBelt] = -100;
else
motor[motorBelt] = 0;
if (joy2Btn(6)) {
servo[srvoF1] = srvoVal[0];
servo[srvoF2] = srvoVal[0];
}
if (joy2Btn(8)) {
servo[srvoF1] = srvoVal[1];
servo[srvoF2] = srvoVal[1];
}
} //while true
} //task main
bool isScoopIn() {
return joy2Btn(6) == 1;
}
task main()
{
initializeRobot();
int threshold = 20;
waitForStart(); // wait for start of tele-op phase
while (true)
{ getJoystickSettings(joystick);
if(abs(joystick.joy1_y2) > threshold) // If the right analog stick's Y-axis readings are either above or below the threshold:
{
motor[motorD] = joystick.joy1_y2; // Motor D is assigned a power level equal to the right analog stick's Y-axis reading.
motor[motorE] = joystick.joy1_y2; // Motor E is assigned a power level equal to the right analog stick's Y-axis reading.
}
else // Else if the readings are within the threshold:
{
motor[motorD] = 0; // Motor D is stopped with a power level of 0.
motor[motorE] = 0; // Motor E is stopped with a power level of 0.
}
if(abs(joystick.joy1_y1) > threshold) // If the left analog stick's Y-axis readings are either above or below the threshold:
{
motor[motorF] = joystick.joy1_y1; // Motor E is assigned a power level equal to the left analog stick's Y-axis reading.
motor[motorG] = joystick.joy1_y1; // Motor G is assigned a power level equal to the right analog stick's Y-axis reading.
}
else // Else if the readings are within the threshold:
{
motor[motorF] = 0; // Motor E is stopped with a power level of 0.
motor[motorG] = 0; // Motor G is stopped with a power level of 0.
}
// BUTTONS TO CONTOL SERVO ARM
// Control arm via shoulder buttons, 5 and 6... 5=up, 6=down
if(joy2Btn(5)) // If Button 5 is pressed:
{
servo[servo1] = 180; // Raise Servo 1 to position 180.
servo[servo2] = 180; // Raise Servo 2 to position 180.
servo[servo5] = 180; // Raise Servo 5 to position 180.
}
if(joy2Btn(7)) // If Button 7 is pressed:
{
servo[servo1] = 0; // Lower Servo 1 to position 0.
servo[servo2] = 0; // Lower Servo 2 to position 0.
servo[servo5] = 0; // Lower Servo 5 to position 0.
}
if(joy2Btn(6) ==1)
{
motor[motorC] = -25;
}
if(joy2Btn(8) ==1)
{
motor[motorC] = 25;
}
if(joy2Btn(4) ==1) // If Button 4 is pressed:
{
servo[armservo] = 255;
//servo[spinservo2] = 0; // Raise Servo 3 to position 180.
}
if(joy2Btn(2) ==1) // If Button 2 is pressed:
{
servo[armservo] = 45;
//servo[spinservo2] = 255; // Lower Servo 3 to position 0.
}
if(joy2Btn(3) ==1)
{
servo[sprocketservo] = 99;
//servo[spr] = 127;
}
if(joy1Btn(1) ==1)
{
motor[motorB] = -100;
}
else
{
motor[motorB] = 0;
}
if(joy2Btn(1) ==1)
{
motor[motorA] = 100;
}
else
{
motor[motorA] = 0;
}
///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////
//// ////
//// Add your robot specific tele-op code here. ////
//// ////
///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////
// Insert code to have servos and motors respond to joystick and button values.
// Look in the ROBOTC samples folder for programs that may be similar to what you want to perform.
// You may be able to find "snippets" of code that are similar to the functions that you want to
// perform.
}
}
task main()
{
initializeRobot(); // Execute robot initialization routine
int Xcurrent, XcurrentTemp, Xdesired;
waitForStart();
while(true)
{
getJoystickSettings(joystick);
//clears the screen of automatic NXT diagnostic garbage
bDisplayDiagnostics = false;
bNxtLCDStatusDisplay = false;
//DC motor controls
//Left drive motor
if(abs(joystick.joy1_y1) <= 15) //Joystick deadband
{
motor[Left1] = 0;
motor[Left2] = 0;
}
else
{
motor[Left1] = joystick.joy1_y1*25/32; //Assign DriveMtrD (Motor 1) to the Left Y-Axis; scale joystick to motor (100/128)
motor[Left2] = joystick.joy1_y1*25/32; //Assign DriveMtrD (Motor 1) to the Left Y-Axis; scale joystick to motor (100/128)
}
//Right drive motor
if(abs(joystick.joy1_y2) <= 15) //Joystick deadband
{
motor[Right1] = 0;
motor[Right2] = 0;
}
else
{
motor[Right1] = joystick.joy1_y2*25/32; //Assign DriveMtrE (Motor 2) to the Right Y-Axis; scale joystick to motor (100/128)
motor[Right2] = joystick.joy1_y2*25/32; //Assign DriveMtrE (Motor 2) to the Right Y-Axis; scale joystick to motor (100/128)
}
//reads the protoboard to see which switches are pressed. Since they are globals, nothing is returned. We may want to make this it's own task eventually
ProcessProto();
//only allows the elevator to go up if the bottom limit switch is pressed
if(bottomLimit == 1)
{
if(joy2Btn(5))
{
motor[elevatorA] = 85;
motor[elevatorB] = 85;
}
else
{
motor[elevatorA] = 0;
motor[elevatorB] = 0;
}
// uses opportunity to reset the encoder if it is all the way at the bottom to help agianst compiling error
if (bottomLimit == 1) nMotorEncoder[elevatorA] = 0;
}
else //normal control if the bottom switch is not pressed
{
if(joy2Btn(5)) // Arm Up
{
motor[elevatorA] = 85;
motor[elevatorB] = 85;
}
else if(joy2Btn(7)) // Arm Down
{
motor[elevatorA] = -55;
motor[elevatorB] = -55;
}
else
{
motor[elevatorA] = 0; // Stop Arm Motion
motor[elevatorB] = 0;
}
// uses opportunity to reset the encoder if it is all the way at the bottom to help agianst compiling error
if (bottomLimit == 1) nMotorEncoder[elevatorA] = 0;
}
// Place RAM code here
if(joy1Btn(2))
{
nMotorEncoderTarget[RAMright] = 90;
nMotorEncoderTarget[RAMright] = 90;
ClearTimer(T1);
while(nMotorEncoder[RAMright] < 90 && time1[T1] < 500)
{
motor[RAMright] = 50;
motor[RAMleft] = 50;
}
motor[RAMright] = 0;
motor[RAMleft] = 0;
}
if(joy1Btn(4))
{
nMotorEncoderTarget[RAMright] = 0;
nMotorEncoderTarget[RAMright] = 0;
ClearTimer(T1);
while(nMotorEncoder[RAMright] > 0 && time1[T1] < 1000)
{
motor[RAMright] = -65;
motor[RAMleft] = -65;
}
motor[RAMright] = 0;
motor[RAMleft] = 0;
}
// Place WAM code here
// Place code to auto score a ring here
// Place code to Pick-up a ring here
Xcurrent = nMotorEncoder[elevatorA];
XcurrentTemp = Xcurrent;
Xdesired = readJoystick(XcurrentTemp); //returns how many encoder counts need to go to if button pressed. If no button pressed, Xdesired stays the same
if (Xdesired != Xcurrent) //if the user has changed the Xdesired fromt he current encoder position by pressing the preset button, then it does the p loop function to get to theat height
{
//PickUpRing(); // this funciton will use the p loop function to go until either the gripper hits the ground to pick up a singe ring or the limit switch in the arm is pressed to pick up two
elevatorGoToHeight(Xdesired);
}
}
}
///////////////////////////////////////////////////////////////////////////////////////////////
//This is the P loop to have the arm go to the desired location
int elevatorGoToHeight(int Xdesired) //Xdesired is in encoder ticks
{
int XcurrentLocal;
int errorX;
int Xspeed;
float proportionConstant = .2; // Proportional constant value.
// Get current encoder value
XcurrentLocal = nMotorEncoder[elevatorA];
while(Xdesired != XcurrentLocal) //only run the loop when you are not at the correct position.
{
XcurrentLocal = nMotorEncoder[elevatorA];
Xdesired = readJoystick(Xdesired); //reads controller to see if fickle user wants to change to another preset level. if no button is being pressed, Xdesired stays the same
nxtDisplayTextLine(3, "AutoMode");
nxtDisplayTextLine(4, "Xcur %d", XcurrentLocal);
nxtDisplayTextLine(5, "Xdes %d", Xdesired);
//kicks out of the loop when you are close enough to the disired spot
if ((XcurrentLocal < (Xdesired+150)) && (XcurrentLocal > (Xdesired-150)))
{
motor[elevatorA] = 0;
motor[elevatorB] = 0;
break;
}
//calculates how far from desired to determine how fast and what direction to go.
errorX = Xdesired - XcurrentLocal;
//////////////////////////////////////////////////////////
////////// SETS THE SPEED //////////
//////////////////////////////////////////////////////////
Xspeed = (int) (proportionConstant * errorX);
//////////////////////////////////////////////////////////
if (Xspeed > 0) //set min and max speed limits when going up
{
if (Xspeed > 90)
Xspeed = 90;
if (Xspeed < 40)
Xspeed = 40;
}
if (Xspeed < 0) //set the min and max speed limits when going down
{
if (Xspeed < -55)
Xspeed = -55;
if (Xspeed > -15)
Xspeed = -15;
}
//sets the speed of the motors to go to the position
motor[elevatorA] = Xspeed;
motor[elevatorB] = Xspeed;
nxtDisplayTextLine(7, "speed %d", Xspeed);
//stop button to cancel going to present loctation
if(joy2Btn(8) == 1)
{
motor[elevatorA] = 0;
motor[elevatorB] = 0;
break;
}
}
//turn off motors after leaving the while loop
motor[elevatorA] = 0;
motor[elevatorB] = 0;
return 0;
}
task main()
{
waitForStart();
servoChangeRate[handJoint] = 10;
int maxVal = 40;
while (true)
{
getJoystickSettings(joystick);
int cont1_left_yval = avoidWeird(joystick.joy1_y1, 20); //y coordinate for the left joystick on controller 1
int cont1_left_xval = avoidWeird(joystick.joy1_x1, 75); //x coordinate for the left joystick on controller 1
int cont1_right_yval = avoidWeird(joystick.joy1_y2, 20);
int cont1_dPad = joystick.joy1_TopHat; //Value of the dPad for controller 2
if (joy1Btn(4) == 1)
{
if ((ServoValue[handJoint] + 5) < maxHandValue)
{
servo[handJoint] = ServoValue[handJoint] + 5;
}
}
if (joy1Btn(2) == 1)
{
if ((ServoValue[handJoint] - 5) > minHandValue)
{
servo[handJoint] = ServoValue[handJoint] - 5;
}
}
if (joy1Btn(6) == 1)
{
fold_arm(false);
}
if (joy1Btn(5) == 1)
{
fold_arm(true);
}
if (joy1Btn(1) == 1)
{
maxVal = 100;
}
if (joy1Btn(1) != 1)
{
maxVal = 40;
}
if (joy2Btn(1) == 1)
{
servo[ramp] = 0;
}
if (joy2Btn(3) == 1)
{
servo[ramp] = 255;
}
if (joy2Btn(1) != 1 && joy2Btn(3) != 1)
{
servo[ramp] = 128;
}
drive(cont1_left_yval, cont1_left_xval, maxVal);
shoulderMovement(cont1_right_yval);
handMovement(cont1_dPad);
}
}
task main(){
bool liftMotorRun;
initializeRobot();
//waitForStart();
getJoystickSettings(joystick);
while( true ) { //infinite loop for control
motorRun = false;
if( abs(joystick.joy1_y1) > threshold ){ //right joystick control. When the left joystick is pushed forward, move forward
float fPower = (joystick.joy1_y1 * 100 / 128);
int iPower = fPower;
forward(iPower);
}
if( abs(joystick.joy1_x2) > threshold ){ //if right joystick is pulled left or right, turn to go in the same direction.
float fPower = (-joystick.joy1_x2 * 100 / 128);
int iPower = fPower;
turn(iPower);
}
bool liftTooLow = false;
if(SensorValue(touch) == 1){
liftTooLow = true;
}
if (joy2Btn(1) == 1)
{
servo[motorLift] = (ServoValue[motorLift] + 1);
}
if(joy2Btn(3) == 1)
{
servo[motorLift] = (ServoValue[motorLift] - 1);
}
if(joy2Btn(2) == 1)
{
motor[motorLift] = liftPower;
liftMotorRun = false;
}
if(joy2Btn(4) == 1 && liftTooLow == false)
{
motor[motorLift] = -liftPower;
liftMotorRun = false;
}
if(joy2Btn(2) == 1)
{
servo[servoBucket] += 1;
}
if(joy2Btn(5) == 1)
{
servo[servoFlagAdjust] = 127;
}
else if(joy2Btn(7) == 1)
{
servo[servoFlagAdjust] = -127;
}
else {
servo[servoFlagAdjust] = 0;
}
if(joy2Btn(4) == 1)
{
servo[servoBucket] -= 1;
}
if(joy1Btn(6) == 1)
{
motor[motorFlag] = 50;
}
if( motorRun == false ){ //stops the motors from running if nothing is pressed. Must be updated to account for all motor functions added to the program.
stopMotion();
}
if( liftMotorRun == false ){
stopLiftMotion();
}
if( abs(joystick.joy2_y1) > threshold ){
motor[motorLift] = (joystick.joy2_y1 * 100) / 128;
liftRun = true;
}
if(liftRun == false){
stopLiftMotion();
}
}
//if the touch sensor goes off, so does the motor.
}
bool isLiftUp() {
return joy2Btn(8) == 1;
}
bool isLiftDown() {
return joy2Btn(7) == 1;
}
task main() {
initializeRobot();
if(SensorValue[touch1] == 1){
liftTooLow = true;
}
while( true ) { //infinite loop for control
motorRun = false;
liftMotorRun = false;
flagMotorRun = false;
bucketMoving = false;
getJoystickSettings(joystick);
/* =====================================================
= Run Controller 1 - Drive Motors
===================================================== */
// Move the robot forwards and backwards
if( abs(joystick.joy1_y1) > JOY_THRESHOLD ){
float fPower = (joystick.joy1_y1 * 100 / 128);
int iPower = fPower;
forward(-iPower);
}
// Turn the robot left or right
if( abs(joystick.joy1_x2) > JOY_THRESHOLD ){
float fPower = (-joystick.joy1_x2 * 100 / 128);
int iPower = fPower;
turn(iPower);
}
if( joystick.joy1_TopHat == 4 ){ //moves forward slowly when D-pad is pressed up
forward(SLOW_MOVEMENT);
}
if(joystick.joy1_TopHat == 0 ){ //d-pad down, move back slow
forward(-SLOW_MOVEMENT);
}
if(joystick.joy1_TopHat == 2) { //turns right slowly if the rightmost D-pad is pressed
turn(-SLOW_MOVEMENT_TURN);
}
if(joystick.joy1_TopHat == 6) { //turns left slowly if the leftmost D-pad is pressed
turn(SLOW_MOVEMENT_TURN);
}
/* =====================================================
= Run Controller 2 - Activators
===================================================== */
// Raise and Lower the Lift
if(joy2Btn(5) == 1 && liftTooLow == false) {
motor[motorLift] = LIFT_POWER;
liftMotorRun = true;
}
if(joy2Btn(7) == 1) {
motor[motorLift] = -LIFT_POWER;
liftMotorRun = true;
}
// Run the flag motor forward or backward
if(joy2Btn(6) == 1) {
motor[motorFlag] = -FLAG_POWER;
flagMotorRun = true;
}
if(joy2Btn(8) == 1) {
motor[motorFlag] = (FLAG_POWER);
flagMotorRun = true;
}
if(joy2Btn(2) == 1 && joy2Btn(4) == 1){
servo[bucket] = BEGINNING_POINT + 20;
bucketMoving = true;
}
// Tilt the bucket backward and forward
if(joy2Btn(2) == 1 && bucketMoving == false) {
servo[bucket] += 1;
}
if(joy2Btn(4) == 1 && bucketMoving == false) {
servo[bucket] -= 1;
}
if(joy2Btn(1) == 1){
servo[bucket] = BEGINNING_POINT - 40;
}
// Reset bucket into correct position
if(joy2Btn(3) == 1){
servo[bucket] = BEGINNING_POINT + 70;
}
/* =====================================================
= Stop mottors that are not running
===================================================== */
if( motorRun == false ) {
stopMotion();
}
if( liftMotorRun == false ) {
stopLiftMotor();
}
if( flagMotorRun == false ) {
stopFlagMotor();
}
}
}
bool isScoopOut() {
return joy2Btn(5) == 1;
}
task main()
{
int pause=0;
int slow = 1;
int power;
int seekerVal = -1;
bFloatDuringInactiveMotorPWM = false; // brake
while(1)
{
//if( SensorValue(S2) != seekerVal && SensorValue(S2)!= 0)
//{
// seekerVal = SensorValue(S2);
// writeDebugStreamLine("Seeker Val: %d ", seekerVal);
//}
getJoystickSettings(joystick);
//Check to enable / disable robot controls
if( joy2Btn(2) == 1 )
{
pause = 0;
}
if( joy2Btn(3) == 1 || pause )
{
motor[motorB] = 0;
motor[motorC] = 0;
motor[motorA] = 0;
pause = 1;
continue;
}
if(joy1Btn(1))
slow = 0;
else if(joy1Btn(3))
slow = 1;
if( slow )
power = 10;
else
power = 26;
if(joy1Btn(5))
power *= 2;
if(joystick.joy1_x2 > 13 || joystick.joy1_x2 < -13) // turn
{
if(joystick.joy1_x2 < 0)
power = -1 * power;
motor[motorB] = power;
motor[motorC] = -1 * power;
}
else if( joystick.joy1_y1 > 13 || joystick.joy1_y1 < -13) /* straight */
{
if(joystick.joy1_y1 < 0)
power = -1 * power;
motor[motorB] = power;
motor[motorC] = power;
}
//else
else if(joy1Btn(2))
//motor[motorA] = -75;
pickup();
else
{
motor[motorA] = 0;
motor[motorB] = 0;
motor[motorC] = 0;
}
}
}
task main()
{
init();
waitForStart(); //Waits for FTC match to officialy start
//Main Loop
while(true){
getJoystickSettings(joystick);
armEncoder=nMotorEncoder[armR];
lsVal = LSvalRaw(LEGOLS);
if(ServoValue[servo1]==100&&up&&ServoValue[servo2]==215-ser){
ser=0;
up=false;
}
if(ServoValue[servo1]==0&&ServoValue[servo2]==215-ser&&!up){
ser=100;
up=true;
}
servo[servo1]=ser;
servo[servo2]=215-ser;
//Update Motors////////////
///////////////////////////
//Update Servos////////////
if(rampLock){
servo[servo3]=35;
}
else{
servo[servo3]=100;
}
///////////////////////////
//Hi Alejandro :D
//Update Sensors///////////
//Check for weighted Ring///
fs1 = HTFreadSensor(HTFS1);
fs2 = HTFreadSensor(HTFS2);
if(fs1<1024)
fs1+=1000;
if(fs2<1024)
fs2+=1000;
if(fs1>1410||fs2>1375){//||(fs2>1325&&fs1<1160&&fs1>1150)){
LSsetActive(light);
//motor[light]=100;
//PlaySound(soundBlip);
}
else{
LSsetInactive(light);
//motor[light]=0;
}
nxtDisplayCenteredTextLine(1,"Servo:%i %i",servo[servo1],ser);
nxtDisplayCenteredTextLine(2,"Force1: %i",fs1);
nxtDisplayCenteredTextLine(3,"Force2: %i",fs2);
nxtDisplayCenteredTextLine(4,"RawColor: %i",lsVal);
nxtDisplayCenteredTextLine(5,"Encoder: %i",armEncoder);
////////////////////////////
//Gamepad 2(Arm and claw Control)////////////////
getJoystickSettings(joystick);
if(joy2Btn(1)){ //Speed Toggle
while(joy2Btn(1)){}
toggle2 = !toggle2;
}
if(toggle2)
mult2 = 1;
if(!toggle2) //USE AN ELSE STATMENT!!!
mult2 = .5;
getJoystickSettings(joystick);
if(joy2Btn(5)){ //Arm Control
motor[armR] = 100*.5*mult2;//mult2;
//motor[armL] = 100*mult2;
}
else if(joy2Btn(6)){
motor[armR] = -100*mult2;
//motor[armL] = -100*mult2;
}
else{
motor[armR] = 0;
//motor[armL] = 0;
}
//////////////////////////////
//Gamepad 1(Drive Train Control)///////////////////
getJoystickSettings(joystick);
if(joy1Btn(1)){ //Speed Toggledjjj
while(joy1Btn(1)){}
toggle = !toggle;
}
if(joy1Btn(6)){
motor[ramp]=-50;
}
else if(joy1Btn(5)){
motor[ramp]=100;
}
else{
motor[ramp]=0;//UNESSECARRY BRACKETS
}
if(joy1Btn(4)){
while(joy1Btn(4)){}
rampLock=!rampLock;
}
if(toggle)
mult=.8;
if(!toggle||abs(armEncoder)>150)
mult=.5;
getJoystickSettings(joystick);
if(abs(joystick.joy1_y1)>threshold){ //Diagonal Forward Right & Backwards Left
motor[motorFL] = joystick.joy1_y1*mult;
motor[motorBL] = joystick.joy1_y1*mult;
}
else{
motor[motorBL] = 0;
motor[motorFL] = 0;
}
if(abs(joystick.joy1_y2)>threshold){
motor[motorFR] = joystick.joy1_y2*mult;
motor[motorBR] = joystick.joy1_y2*mult;
}
else{ //Stop Motors
motor[motorFR] = 0;
motor[motorBR] = 0;
}
////////////////////////////////////////////
time+=10;
wait1Msec(10);
}
}
//===========================================================================================
// Main program - handle joystick inputs and take appropriate actions
//===========================================================================================
task main()
{
int x1, y1;
float hypoteneuse;
bool abortProgram = false;
initializeRobot(); // run all initialisations
waitForStart(); // then wait for FCS to tell us to go
while(!abortProgram) // handle joystick inputs until the drive team aborts program
// drve team can abort program by pressing both "9" buttons at the same time
{ // run first read of joystick to verify connectivity
if(joystick.joy1_TopHat == -1) Joy1Enabled = true; // enable joystick only if tophat responds with -1 // otherwise disable that joystick
if(joystick.joy2_TopHat == -1) Joy2Enabled = true; // same test for joystick 2
if(joy1Btn(10) && joy2Btn(10)) abortProgram = true; // handle the abort program condition
//================================
getJoystickSettings(joystick); // get joystick data packet
x1 = (joystick.joy1_x1*100)/127; // extract X value for joystick one ranged to percentage
y1 = (joystick.joy1_y1*100)/127; // extract Y value for joystick one range to percentage
hypoteneuse=(float)sqrt((x1*x1)+(y1*y1)); // calculate third side of effective triangle
//---------------------
if(joy1Btn(5)) hypoteneuse = hypoteneuse; // max speed only if button 5 is pressed
else if(joy1Btn(7)) hypoteneuse = hypoteneuse/(float)5; // button seven gives slow slow speed
else hypoteneuse = hypoteneuse/(float)2; // otherwise default is half speed
//---------------------thisone
if ((abs(x1)<DEADBAND) && (abs(y1)<DEADBAND)) // if joystick is effectively in zero position
{
drive_speed=0; // then set speed and effective angle to zero
drive_angle=0; //
}
else // joystick is active, so calculate motor power values
{
drive_angle=radiansToDegrees(atan2(x1,y1)); // calculate drive angle from X and Y joystick values
drive_speed=hypoteneuse; // the speed is defined by the extent of the X-Y hypoteneuse
}
//==============================================================
// Code to handle JOG capability using TOPHAT on either joystick
//==============================================================
if(Joy2Enabled == true && joystick.joy2_TopHat!=-1) // if enabled and pressed then we have a request
{
if (!Jog_In_Progress) // only accept new requests if one isn't already active
{
requested_angle = 45*joystick.joy2_TopHat; // calculate and save the requested angle
Jog_In_Progress = true; // setup a jog in progress
ClearTimer(T1); // and start the timer
}
if (Jog_In_Progress) // jog movements take precedence over joystick control
{
if (time1[T1]>JOGTIME) // then the jog is complete
{
Jog_In_Progress = false; // so disable jog control
}
else // the jog is still underway
{
drive_angle = (float)requested_angle; // so the jog angle takes precedence over joystick activity
drive_speed = JOGSPEED; // and we also need to setup the default jog speed
}
}
if (drive_speed>100) drive_speed = 100; // limit max speed to 100 percent
}
else
{
if(Joy1Enabled == true)
{
if ((joystick.joy1_TopHat!=-1) && (!Jog_In_Progress)) // we have a new request for a jog
{
requested_angle = 45*joystick.joy1_TopHat; // calculate and save the requested angle
Jog_In_Progress = true; // setup a jog in progress
ClearTimer(T1); // and start the timer
}
if (Jog_In_Progress) // jog movements take precedence over joystick control
{
if (time1[T1]>JOGTIME) // then the jog is complete
{
Jog_In_Progress = false; // so disable jog control
}
else // the jog is still underway
{
drive_angle = (float)requested_angle; // so the jog angle takes precedence over joystick activity
drive_speed = JOGSPEED; // and we also need to setup the default jog speed
}
}
if (drive_speed>100) drive_speed = 100; // limit max speed to 100 percent
}
}
//================================================================================================
// now calculate individual motor speeds based on the motor angle versus the requested drive angle
//================================================================================================
float gyro_angle=constHeading; // assume we'll be using field relative mode
if(joy1Btn(11) != true) gyro_angle = 0; // otherwise configure robot relative drive
LF_speed = cosDegrees(LF_angle-drive_angle+gyro_angle)*drive_speed; // individual motor speeds are calculated
RF_speed = cosDegrees(RF_angle-drive_angle+gyro_angle)*drive_speed; // relative to the mounted angle of that
LR_speed = cosDegrees(LR_angle-drive_angle+gyro_angle)*drive_speed; // motor, but adjusted for the current
RR_speed = cosDegrees(RR_angle-drive_angle+gyro_angle)*drive_speed; // orientation of the robot per the GYRO
//====================================================================
// Now handle request for rotation, either standalone or while driving
//====================================================================
rotation_factor=0; // assume no rotation is requested
int speed_limit=drive_speed; // this is used to determine maximum allowable speed if there is also rotation
if (joy1Btn(3))
{
if(joy1Btn(5)) rotation_factor=-RotationSpeedFast;// clockwise rotation
else rotation_factor=-RotationSpeed;
}
if (joy1Btn(1))
{
if(joy1Btn(5)) rotation_factor=RotationSpeedFast; // counter clockwise rotation
else rotation_factor=RotationSpeed;
}
speed_limit-=abs(rotation_factor); // adjuist spped limit down to allow for rotation (if any)
//=================================================================================
// now prorate the power levels so the highest equals the requested power level
//=================================================================================
float highest = max(max(abs(LF_speed),abs(RF_speed)),max(abs(LR_speed),abs(RR_speed)));
float adjust_factor=1;
if (highest!=0) adjust_factor=speed_limit/highest;
LF_speed*=adjust_factor;
RF_speed*=adjust_factor;
LR_speed*=adjust_factor;
RR_speed*=adjust_factor;
LF_speed+=rotation_factor; // add in the rotation factor to each drive motor
RF_speed+=rotation_factor;
LR_speed+=rotation_factor;
RR_speed+=rotation_factor;
//=======================================================================
// finally operate the actual drive motors at the calculated power levels
//=======================================================================
motor[LF_motor]=(int)LF_speed;
motor[RF_motor]=(int)RF_speed;
motor[LR_motor]=(int)LR_speed;
motor[RR_motor]=(int)RR_speed;
//=========================================
// ring lifter
//=========================================
if (abs(joystick.joy2_y1)>DEADBAND) // lift activity has been requested by gunner
{
if(joystick.joy2_y1>DEADBAND) // they want it to go UP
{
if(joy2Btn(11)) lifter_state = HIGH_LIFT; // choose between HIGH and LOW levels based on button
else lifter_state=LOW_LIFT;
}
if(joystick.joy2_y1<-DEADBAND) // they want it to go DOWN
{
if(joy2Btn(11)) lifter_state=OVERRIDE_DOWN; // set override mode - ignore limit switches
else lifter_state=MANUAL_DOWN; // otherwise enable down only till limit switch
}
}
else if (abs(joystick.joy1_y2)>DEADBAND) // lift activity has been requested by driver
{
if(joystick.joy1_y2>DEADBAND) // they want it to go UP
{
if(joy1Btn(12)) lifter_state = HIGH_LIFT;
else lifter_state=LOW_LIFT;
}
if(joystick.joy1_y2<-DEADBAND) // they want it to go DOWN
{
if(joy1Btn(12)) lifter_state=OVERRIDE_DOWN;
else lifter_state=MANUAL_DOWN;
}
}
else lifter_state = STOPPED; // if no activity then ensure motor is not running
//=============================
// Gyro reset
//=============================
if(joy1Btn(9)) // this feature allows the drive team to reset the field
{ // relative zero position in the event that there is enough
constHeading = 0; // gyro noise that it gets messed up.
RequestedScreen=S_BIG_NUMS;
}
//=============================
// grabbers
//=============================
if(joy2Btn(4) || joy1Btn(4)) // set UP position (fully closed)
{
if(grabbers_state == grabbersMid || grabbersDown)
{
servoChangeRate[right_servo] = 10;
servoChangeRate[left_servo] = 10;
}
else
{
servoChangeRate[right_servo] = 4;
servoChangeRate[left_servo] = 4;
}
grabbers_state = grabbersUp;
}
if(joy2Btn(3) || joy1Btn(2)) // set MID position (release ring)
{
if(grabbers_state == grabbersDown || grabbers_state == grabbersMid)
{
servoChangeRate[right_servo] = 10;
servoChangeRate[left_servo] = 10;
}
else
{
servoChangeRate[right_servo] = 4;
servoChangeRate[left_servo] = 4;
}
grabbers_state = grabbersMid;
}
if(joy2Btn(2)) // set down position (safety zone)
{
if(grabbers_state == grabbersUp || grabbers_state == grabbersMid)
{
servoChangeRate[right_servo] = 4;
servoChangeRate[left_servo] = 4;
}
else
{
servoChangeRate[right_servo] = 10;
servoChangeRate[left_servo] = 10;
}
grabbers_state = grabbersDown;
}
if(joy2Btn(6) || joy1Btn(6)) // drive team wants grabbers more closed
{
if (!actioned1) // then this is a new request
{
grabbers_state = nudgeUp; // so tell the task to adjust position
actioned1 = true; // and indicate that we handled the request
}
}
else actioned1=false; // once buttons are released then we enable a new request
if(joy2Btn(8) || joy1Btn(8)) // drive team wants grabbers more open
{
if (!actioned2) // then this is a new request
{
grabbers_state = nudgeDown; // so tell the task to adjust position
actioned2=true; // and indicate that we handled the request
}
}
else actioned2 = false; // once the buttons are released we enable a new request
//if(abs(joystick.joy2_y2)>10) motor[lightStrip] = joystick.joy2_y2;
//else motor[lightStrip] = 0;
}
}
// Put the main driver control loop in its own tasks so
// the drivers never loses control of the robot!
task drive()
{
// Initialize variables
int lastMessage = 0;
int leftMotorSpeed = 0;
int rightMotorSpeed = 0;
int armMotorSpeed = 0;
int totalMessages = 0;
int topSpeed = MOTOR_POWER_DOWN_MAX;
int armTopSpeed = 65;
int scoopTopSpeed = 40;
int scoopMotorSpeed = 0;
while (true)
{
getJoystickSettings(joystick);
if (lastMessage != ntotalMessageCount) {
if (true) {
ClearTimer(T2);
lastMessage = ntotalMessageCount;
// New joystick messages have been received!
// Set your drive motors based on user input
// here.
leftMotorSpeed = joystick.joy1_y1 / JOYSTICK_Y1_MAX * topSpeed; // Map the leftMotorSpeed variable to joystick 1_y1
if (abs(leftMotorSpeed) < JOYSTICK_DEAD_ZONE) leftMotorSpeed = 0; // Make sure that the joystick isn't inside dead zone
rightMotorSpeed = joystick.joy1_y2 / JOYSTICK_Y1_MAX * topSpeed; // Map the rightMotorSpeed variable to joystick 1_y2
if (abs(rightMotorSpeed) < JOYSTICK_DEAD_ZONE) rightMotorSpeed = 0; // Make sure that the joystick isn't inside dead zone
armMotorSpeed = joystick.joy2_y2 / JOYSTICK_Y1_MAX * armTopSpeed; // Map the armMotorSpeed variable to joystick 2_y2
if (abs(armMotorSpeed) < JOYSTICK_DEAD_ZONE) armMotorSpeed = 0; // Make sure that the joystick isn't inside dead zone
scoopMotorSpeed = joystick.joy2_y1 / JOYSTICK_Y1_MAX * scoopTopSpeed; // Map the teleMotorSpeed variable to joystick 2_y1
if (abs(scoopMotorSpeed) < JOYSTICK_DEAD_ZONE) scoopMotorSpeed = 0; // Make sure that the joystick isn't inside dead zone
motor[armMotor] = armMotorSpeed; // Set the motor armMotor speed as armMotorSpeed
motor[leftMotor] = leftMotorSpeed; // Set the motor leftMotor speed as leftMotorSpeed
motor[rightMotor] = rightMotorSpeed; // Set the motor rightMotor speed as rightMotorSpeed
motor[scoopMotor] = scoopMotorSpeed;
if (joy1Btn(5) == 1) {
// Power up
topSpeed = MOTOR_POWER_UP_MAX;
}
if (joy1Btn(7) == 1) {
// Power down
topSpeed = MOTOR_POWER_DOWN_MAX;
}
if (joy2Btn(2) == 1) {
// Power up
armTopSpeed = ARM_MOTOR_POWER_DOWN;
}
if (joy1Btn(4) == 1) {
// Power up
armTopSpeed = ARM_MOTOR_POWER_UP;
}
if (joy1Btn(4) == 1) {
// Nudge forward
motor[leftMotor] = NUDGE_POWER;
motor[rightMotor] = NUDGE_POWER;
wait1Msec(NUDGE_DURATION);
motor[leftMotor] = 0;
motor[rightMotor] = 0;
wait1Msec(NUDGE_DELAY);
}
if (joy1Btn(2) == 1) {
// Nudge backward
motor[leftMotor] = NUDGE_POWER * -1;
motor[rightMotor] = NUDGE_POWER * -1;
wait1Msec(NUDGE_DURATION);
motor[leftMotor] = 0;
motor[rightMotor] = 0;
wait1Msec(NUDGE_DELAY);
}
if (joy1Btn(0) == 1) {
// Nudge left
motor[leftMotor] = 0;
motor[rightMotor] = NUDGE_POWER;
wait1Msec(NUDGE_DURATION);
motor[leftMotor] = 0;
motor[rightMotor] = 0;
wait1Msec(NUDGE_DELAY);
}
if (joy1Btn(3) == 1) {
// Nudge right
motor[leftMotor] = NUDGE_POWER;
motor[rightMotor] = 0;
wait1Msec(NUDGE_DURATION);
motor[leftMotor] = 0;
motor[rightMotor] = 0;
wait1Msec(NUDGE_DELAY);
}
totalMessages = ntotalMessageCount;
} else if (time1[T2] > 200) {
// We have not received a packet in over two-tenths of a
// second, which probably means communications have been
// lost.
// Put code to stop all drive motors to avoid
// damaging the robot here.
PlayImmediateTone(3000, 1); // play a warning tone
// Stop motors
motor[leftMotor] = 0;
motor[rightMotor] = 0;
}
}
}
}
task main()
{
//false=off/start position/etc.
bool sweeperState=false;
bool flagState=false;
servo[basketServo]=basketServoUp;
while(true)
{
//Riley's code
if(abs(joystick.joy1_y1)>deadZone)
{
motor[FrontLeft]=scaleMotor(joystick.joy1_y1);
motor[BackLeft]=scaleMotor(joystick.joy1_y1);
//debug code
/*
nxtDisplayTextLine(1, "%d",scaleMotor(joystick.joy1_y1) );
wait1Msec(5000);
*/
}
else
{
motor[FrontLeft]=0;
motor[BackLeft]=0;
}
if(abs(joystick.joy1_y2)>deadZone)
{
motor[FrontRight]=scaleMotor(joystick.joy1_y2);
motor[BackRight]=scaleMotor(joystick.joy1_y2);
}
else
{
motor[FrontRight]=0;
motor[BackRight]=0;
}
//End of Riley's code
//Toggling the sweeper
if(joy2Btn(SWEEPERBUTTON)==true && sweeperState==false)
{
motor[sweeper]=sweeperOn;
sweeperState=true;
}
else if(joy2Btn(SWEEPERBUTTON)==true && sweeperState==true)
{
motor[sweeper]=sweeperOff;
sweeperState=false;
}
//Moving the arm up or down
if((abs(ARMJOYSTICK))>deadZone && ARMJOYSTICK>0)
{
motor[armMotor]=armMotorUp;
}
else if((abs(ARMJOYSTICK))>deadZone && ARMJOYSTICK<0)
{
while(SensorValue[touchSensor]==0)
{
motor[armMotor]=-armMotorUp;
}
motor[armMotor]=armMotorUp;
wait1Msec(50);
motor[armMotor]=0;
}
else
{
motor[armMotor]=0;
}
//Change the basket servo position
if(joy2Btn(BASKETUPBUTTON)==true)
{
servo[basketServo]=basketServoUp;
}
else if(joy2Btn(BASKETDOWNBUTTON)==true)
{
servo[basketServo]=basketServoStart;
}
//Turns the flag motor on/off
if(joy2Btn(FLAGBUTTON)==true && flagState==false)
{
motor[flagMotor]=flagMotorOn;
flagState=true;
}
else if(joy2Btn(FLAGBUTTON)==true && flagState==true)
{
motor[flagMotor]=0;
flagState=false;
}
wait1Msec(1);
}
}
task main()
{
waitForStart(); // Wait for the tele-op period to begin
nMotorEncoder(motorLift1) = 0; // Reset the motor encoders for the arm
servoTarget(clawL) = 225; // Initialize the claw to be open
servoTarget(clawR) = 60;
PlaySoundFile("Leroy.rso"); // Shout "LEEROY JENKINS!", just for fun
wait1Msec(10); // Wait one tenth of a second
while (true)
{
getJoystickSettings(joystick); // Read the value of the joysticks
nxtDisplayTextLine(6, "Encoder: %d", nMotorEncoder[motorLift1]); // Display the value of the arm encoder
// The following code makes the directional pad on controller 1 give precise digital control of the drivetrain:
if (joystick.joy1_TopHat == 0) // If "up" on the directional pad is pressed:
{
go_forward(25); // Go forward at 25% speed
}
else if (joystick.joy1_TopHat == 1) // Else if "top right" on the directional pad is pressed:
{
strafe_forward_right(25); // Strafe diagonally forward and to the right at 25% speed
}
else if (joystick.joy1_TopHat == 2) // Else if "right" on the directional pad is pressed:
{
strafe_right(25); // Strafe to the right at 25% speed
}
else if (joystick.joy1_TopHat == 3) // Else if "bottom right" on the directional pad is pressed:
{
strafe_backward_right(25); // Strafe diagonally backward and to the right 25% speed
}
else if (joystick.joy1_TopHat == 4) // Else if "down" on the directional pad is pressed:
{
go_backward(25); // Go backward 25% speed
}
else if (joystick.joy1_TopHat == 5) // Else if "bottom left" on the directional pad is pressed:
{
strafe_backward_left(25); // Strafe diagonally backward and to the left 25% speed
}
else if (joystick.joy1_TopHat == 6) // Else if "left" on the directional pad is pressed:
{
strafe_left(25); // Strafe left 25% speed
}
else if (joystick.joy1_TopHat == 7) // Else if "top left" on the directional pad is pressed:
{
strafe_forward_left(25); // Strafe diagonally forward and to the left 25% speed
}
else if (joy1Btn(5)) // Else if button 6 is pressed:
{
rotate_clockwise(25); // Rotate the robot clockwise 25% speed
}
else if (joy1Btn(6)) // Else if button 5 is pressed:
{
rotate_counter_clockwise(25); // Rotate the robot counter-clockwise 25% speed
}
// The following code makes the joysticks on controller 1 give analogue control of the drivetrain:
else
{
motor[motorFL] = scale_motor(joystick.joy1_y1 + joystick.joy1_x1 + joystick.joy1_x2); // Scale the motors to the average
motor[motorFR] = scale_motor(joystick.joy1_y1 + -joystick.joy1_x1 - joystick.joy1_x2); //of the left joystick Y value and
motor[motorBR] = scale_motor(joystick.joy1_y1 + joystick.joy1_x1 - joystick.joy1_x2); //the right joystick X value
motor[motorBL] = scale_motor(joystick.joy1_y1 + -joystick.joy1_x1 + joystick.joy1_x2);
}
// The following code assigns the shoulder buttons on controller 2 to open and close the claw:
if (joy2Btn(5))
{
servoTarget(clawL) = 55; // If button 5 is pressed on controller 2, close the claw
servoTarget(clawR) = 170;
}
else if (joy2Btn(6))
{
servoTarget(clawL) = 115; // If button 6 is pressed on controller 2, open the claw
servoTarget(clawR) = 100;
}
else if (joy2Btn(8))
{
servoTarget(clawL) = ServoValue(clawL) + 1; // While button 7 is held, slowly close the claw
servoTarget(clawR) = ServoValue(clawR) - 1;
}
else if (joy2Btn(7))
{
servoTarget(clawL) = ServoValue(clawL) - 1; // While button 8 is held, slowly open the claw
servoTarget(clawR) = ServoValue(clawR) + 1;
}
else
{
servoTarget(clawL) = ServoValue(clawL); // Otherwise, do not move the claw
servoTarget(clawR) = ServoValue(clawR);
}
// The following code gives analogue control of the arm with the joystick, and assigns buttons 1-4 to move the arm to their respective rows on the scoring rack
if (joy2Btn(1)) // If button 1 is pressed, move the arm to the lowest row on the scoring rack
{
moveArm(level1Value);
}
else if (joy2Btn(2)) // If button 2 is pressed, move the arm to the middle row on the scoring rack
{
moveArm(level2Value);
}
else if (joy2Btn(3)) // If button 3 is pressed, move the arm to the top row on the scoring rack
{
moveArm(level3Value);
}
else if (joy2Btn(4)) // If button 4 is pressed, lower the arm all the way
{
moveArm(0);
}
else
{
motor[motorLift1] = (joystick.joy2_y1 / 12); // Otherwise, control the arm with joystick 1 on controller 2
motor[motorLift1] = (joystick.joy2_y1 / 24); // Or control the arm at half speed with joystick 2 on controller 2
// Note: these values are divided by 12 and 24 to bring the arm speed down to a reasonable level
}
}
}
task main()
{
waitForStart();
initializeRobot();
while(true)
{
getJoystickSettings(joystick);
int jLeft = (int)joystick.joy1_y1;
int jRight = -(int)joystick.joy1_y2;
//Driver Controls//////////////////////////////////////////////////////////////////////////
//////Drivetrain///////////////////////////////////////////////////////////////////////
//Speed Control//
if(joy1Btn(5))
{
speed = true;
}
//------------------------------
if(joy1Btn(7))
{
speed = false;
}
//-------------------------------
if (speed)
{
if (abs(jLeft) < 10) ///< core out the noise for near zero settings
motor[LeftMotor] = 0; ///< sets the left motor to 0% power
else
motor[LeftMotor] = jLeft; ///< set motors to joystick settings
if (abs(jRight) < 10) ///< core out the noise for near zero settings
motor[RightMotor] = 0; ///< sets the right motor to 0% power
else
motor[RightMotor] = jRight; ///< sets motors to joystick settings
}
//--------------------------------
else
{
if (abs(jLeft) < 10) ///< core out the noise for near zero settings
motor[LeftMotor] = 0; ///< sets the left motor to 0% power
else
motor[LeftMotor] = (jLeft/3); ///< set motors to joystick settings
if (abs(jRight) < 10) ///< core out the noise for near zero settings
motor[RightMotor] = 0; ///< sets the right motor to 0% power
else
motor[RightMotor] = (jRight/3); ///< sets motors to joystick settings
}
//CURRENTLY NULL//
/*
//////BAM////////////////////////////////////////////////////////////////////////////////
//up
if(joy1Btn(8))
{
servoTarget[bamLeft] = 255;
servoTarget[bamRight] = 0;
}
//down
else if(joy1Btn(6))
{
servoTarget[bamLeft] = 0;
servoTarget[bamRight] = 255;
}
//Idle
else
{
servoTarget[bamLeft] = 127;
servoTarget[bamRight] = 127;
}
*/
//Manipulator Controls/////////////////////////////////////////////////////////////////////
//////lifter///////////////////////////////////////////////////////////////////////////////
int touchVal;
touchVal = SensorValue(Touch);
int lifterEncoderVal;
lifterEncoderVal = nMotorEncoder[lifter];
nxtDisplayCenteredBigTextLine(2, "%d", touchVal);
nxtDisplayCenteredBigTextLine(5, "%d", lifterEncoderVal);
//up
if(joy2Btn(7) && (touchVal == 0))
{
motor[lifter] = -50;
}
//down
else if(joy2Btn(5))
{
motor[lifter] = 50;
}
//Idle
else
{
motor[lifter] = 0;
}
//OVERRIDE
//down
if(joy2Btn(4))
{
motor[lifter] = 50;
}
/*
float encoderVal;
nMotorEncoder[lifter] = encoderVal;
nxtDisplayBigTextLine(2, "encoder val: %lf", encoderVal);
*/
/*
//////Mechanical Stop/////////////////////////////////////////////////////////////////////////
//Down////
if(joy2Btn(2))
{
nMotorEncoderTarget[stopLeft] = 90;
nMotorEncoderTarget[stopRight] = 90;
ClearTimer(T1);
while ((nMotorEncoder[stopLeft] < 90) && (time1[T1] < 500))
{
motor[stopLeft] = -30;
motor[stopRight] = -30;
}
}
//Up////
if(joy2Btn(4))
{
nMotorEncoderTarget[stopLeft] = 0;
nMotorEncoderTarget[stopRight] = 0;
ClearTimer(T1);
while ((nMotorEncoder[stopLeft] > 0) && (time1[T1] < 500))
{
motor[stopLeft] = 30;
motor[stopRight] = 30;
}
motor[stopLeft] = 0;
motor[stopRight] = 0;
}
*/
//Ramp/////////////////////////////////////////////////////////////////////////
if(joy2Btn(10))
{
ClearTimer(T1);
while(time1[T1] < 1000)
{
servoTarget[ramp] = 255;
}
}
}
}
task main() {
#ifdef COMPETITION
waitForStart(); //do not remove!!!!!!
#endif
int ArmTarget = 0;
armSettings.kP = ARM_KP;
armSettings.maxError = ARM_MAX_ERROR;
armSettings.minOutput = -ARM_MAX_POWER;
armSettings.maxOutput = ARM_MAX_POWER;
float armRoughSetpoint = 0,
armFineSetpoint = 0;
float wristServoVal = 0;
long wristTime = 0;
while(true) {
getJoystickSettings( joystick );
// } else {
// writeDebugStreamLine("button 1 is off")
//}
//writeDebugStreamLine("%d", joystick.joy2_x1);
//see if btn 8 is depressed.
//if so set a scale factor for all movement calculations
//in omnidrive function
float vx = scaleJoystickValue(-1,1,joystick.joy1_x1),
vy = scaleJoystickValue(-1,1,joystick.joy1_y1),
scale = joy1Btn(8) ? DRIVE_SLOW_SCALE :
DRIVE_NORMAL_SCALE;
omniDrive(vx,vy,
sqrt(vx*vx+vy*vy) * scale,
scaleJoystickValue(-50,50,joystick.joy1_x2));
if( joy1Btn( 6 ) ){
motor[armMotor] = scaleJoystickValue(-50,50,joystick.joy2_y1);
} else {
motor[armMotor] = scaleJoystickValue(-75,75,joystick.joy2_y1);
}
#ifdef ENABLE_ARM
#define ARM_FIRST_LEVEL 2000
#define ARM_SECOND_LEVEL 1460
#define ARM_FULL_LEVEL 2370
#define ARM_SPEED_CONST 40
if
if(joy2btn(1)){
ArmTarget = 0;
} else if (joy2btn(2)) {
ArmTarget = ARM_FIRST_LEVEL;
} else if (joy2btn(3)){
ArmTarget = ARM_SECOND_LEVEL;
} else if (joy2btn(4)){
ArmTarget = ARM_FULL_LEVEL;
} else if (joystick.joy2_y1 != 0) {
ArmTarget = -1;
} else {
ArmTarget = ArmTarget;
}
if(ArmTarget = -1){
motor[armMotor] = (50/128) * joystick.joy2_y1;
} else {
if (abs(nmotorencoder[armMotor] - ArmTarget) <= 5){
motor[armMotor] = 0;
} else if (nmotorencoder[armMotor] - ArmTarget > 5) {
motor[armMotor] = ARM_SPEED_CONST;
} else if (nmotorencoder[armMotor] - ArmTarget < -5){
motor[armMotor] = -ARM_SPEED_CONST;
} else {
motor[armMotor] = motor[armMotor];
}
}
// if (joy1btn(6)){
// motor[armMotor] = scaleJoystickValue(-50,50,joystick.joy2_y1);
// } else {
// motor[armMotor] = scaleJoystickValue(-75,75,joystick.joy2_y1);
// }
// if(joy2Btn(5)) {
// armRoughSetpoint = scaleJoystickValue(ARM_MIN,
// ARM_MAX,
// joystick.joy2_y1);
// armFineSetpoint = 0;
// }
// if(joy2Btn(6)) {
// armFineSetpoint = scaleJoystickValue(-ARM_FINE_RANGE,
// ARM_FINE_RANGE,
// joystick.joy2_y2);
// } else {
// armRoughSetpoint += armFineSetpoint;
// armFineSetpoint = 0;
// }
// proportionalControl(armMotor,
// armSettings,
// clamp(armRoughSetpoint+armFineSetpoint,
// ARM_MIN,
// ARM_MAX));
#endif
#ifdef ENABLE_CLAW
#define wristServoValVal 45
if(!(joy2Btn(4) || joy2Btn(2))) {
wristTime = 0;
} else {
wristTime += time1[T1];
}
ClearTimer(T1);
if (joystick.joy2_TopHat == 0){
wristServoVal = 180;
} else if (joystick.joy2_TopHat == 4){
wristServoVal = 0;
} else if (wristTime > 1000 / WRIST_SERVO_SPEED ){
if (joy2Btn(4)){
wristServoVal += 1;
} else if (joy2Btn(2)){
wristServoVal -= 1;
}
wristTime -= 1000/WRIST_SERVO_SPEED;
}
if (wristServoVal >= 180){
wristServoVal = 180;
}else if (wristServoVal <= 20){
wristServoVal = 0;
}
servo[wristServo] = wristServoVal;
if(joy2Btn(1)) {
servo[clawServo] = 230;
} else if(joy2Btn(3)) {
servo[clawServo] = 130;
}
#endif
#ifdef ENABLE_RAMP
#define RAMP_SPEED 50
if( joy2Btn( 10 ) ) {
motor[rampMotor] = RAMP_SPEED;
} else if( joy2Btn( 9 ) ) {
motor[rampMotor] = - RAMP_SPEED;
} else {
motor[rampMotor] = 0;
}
//if(joy1Btn(10) && joy1Btn(9) && joy2Btn(10) && joy2Btn(9)) {
// break;
//}
#endif
} // while
} //task main
task main()
{
//Holonomic Drive Integers
int x1 = 0;
int y1 = 0;
int x2 = 0;
const int t = 8;
const float driveScale = 0.7;
initializeRobot ();
while ( true ) {
getJoystickSettings(joystick);
//Holonomic Drive
x1 = ( abs( joystick.joy1_x1 ) > t ) ? joystick.joy1_x1 : 0;
y1 = ( abs( joystick.joy1_y1 ) > t ) ? joystick.joy1_y1 : 0;
x2 = ( abs( joystick.joy1_x2 ) > t ) ? joystick.joy1_x2 : 0;
motor[frontLeft] = (y1 + x2 + x1) * driveScale;
motor[frontRight] = (y1 - x2 - x1) * driveScale;
motor[rearLeft] = (y1 + x2 - x1) * driveScale;
motor[rearRight] = (y1 - x2 + x1) * driveScale;
//This is the code for controlling the motorized linear slides that the ring grabber appparatus is mounted on.
//Up
if ((joy2Btn (5) == 1) && (SensorValue [liftUpSensor] == 0))
{
motor [linearSlides] = 100;
}
//Down; after the button statement is the sensor value statement so the lift doesn't go too far down.
else if ((joy2Btn (7) == 1) && (SensorValue [liftDownSensor] == 0 )) {
motor [linearSlides] = -100;
}
else motor [linearSlides] = 0;
//The following is the code for controlling the ring grabber apparatus.
//This is the code for controlling the arm that the grabber is mounted on.
if (joy2Btn (6) == 1 ) {
motor [grabberMount] = 50;
}
else if (joy2Btn (8) == 1) {
motor [grabberMount] = -50;
}
else motor [grabberMount] = 0;
/* The following is the other arm code; this allows the arm to be controlled with the joysticks (in addition to
the buttons. */
//motor [grabberMount] = (-(abs( joystick.joy2_y2 ) > t ) ? joystick.joy2_y2 : 0 );
//This is the code for controlling the grabber itself. It is 'commented' out because we currently do not use the 'active' grabber.
/*
if (joystick.joy2_x2 >110)
/*(joy2Btn (1) == 1)*/
/* if (grabberTarget >0)
{
grabberTarget -= grabberIncrement;
setGrabber (grabberTarget/100.0);
}
if (joystick.joy2_x2 <-110)
/*(joy2Btn (3) == 1)*/
/* if (grabberTarget <100)
{
grabberTarget += grabberIncrement;
setGrabber (grabberTarget/100.0);
} */
//This is the code for controlling the wrist, which the grabber is attatched to.
#if 0
if (joystick.joy2_y1 >110)
/*(joy2Btn (2) == 1)*/
if (wristTarget >0)
{
wristTarget -= wristIncrement;
setWrist (wristTarget/1000.0);
}
if (joystick.joy2_y1 <-110)
/*(joy2Btn (4) == 1)*/
if (wristTarget <1000)
{
wristTarget += wristIncrement;
setWrist (wristTarget/1000.0);
}
#else
const int rMax = 1023;
const int rMin = 407;
int inputdata = HTSPBreadADC(HTSPB, 2, 10);
float wrist = (inputdata - rMin);
wrist /= (rMax - rMin);
servo [wristMain] = (255 * wrist) + ( joystick.joy2_y1 / 3);
#endif
// LED weight sensor lights
#if 1
int inputRed = HTSPBreadADC(HTSPB, 0, 10);
int inputBlue = HTSPBreadADC(HTSPB, 1, 10);
int output = 0x00;
if ( inputRed > 512 )
output |= 0x01;
if ( inputBlue > 512 )
output |= 0x02;
if ( output != lastOutput ) {
HTSPBwriteLED( HTSPB, output );
HTSPBwriteIO( HTSPB, output );
lastOutput = output;
}
//wait1Msec(50);
#endif
/* Perhaps we will switch to this function system at the next meeting or possible time. Uncomment below,
the 'deployRamp' function above, and the latchSetting varible declaration. */
/* if ((joy1Btn (10) == 1) && (joy2Btn (10) == 1))
{
deployRamp (180);
} */
/*This conditional statement releases the ramp latch for the end game. Both 'start' buttons must be
pressed to release the ramp latch. The button combination that must be pressed is intentionally awkward
so that the ramp is not deployed by accident during the first 90 seconds of the teleop phase. */
/* Also, below there is (commented out) the or operator and an additional button combination in which
both drivers press all four colored buttons on the right side of the joysticks to deploy the ramp;
this is not currently implemented because it has not been tested and proven to work. */
if /*(*/((joy1Btn (10) == 1) && (joy2Btn (10) == 1)) /* || ((joy1Btn (1) == 1) && (joy1Btn (2) == 1) &&
(joy1Btn (3) == 1) && (joy1Btn (4) == 1) && (joy2Btn (1) == 1) && (joy2Btn (2) == 1) &&
(joy2Btn (3) == 1) && (joy2Btn (4) == 1))) */
{
servo [rampLatch] = rampLatchOpen;
}
}
}
task main()
{
waitForStart(); // wait for start of tele-op phase
while (true)
{
getJoystickSettings(joystick);
//---------------------------- MAIN ROBOT CONTROL ----------------------------//
//=============== LEFT JOYSTICK ===============//
//*** FORWARD ***//
if (joystick.joy1_y1 < -30 && noLeftX() && noRightX())
move(yAClock(), yAClock(), yClock(), yClock());
//move(yAClock(), yClock(), yAClock(), yClock());
//*** REVERSE ***//
else if (joystick.joy1_y1 > 30 && noLeftX() && noRightX())
move(yClock(), yClock(), yAClock(), yAClock());
//move(yClock(), yAClock(), yClock(), yAClock());
//*** STRAFE LEFT ***//
else if (joystick.joy1_x1 < -30 && noLeftY() && noRightX())
move(xClock(), xAClock(), xClock(), xAClock());
//move(xAClock(), xAClock(), xClock(), xClock());
//*** STRAFE RIGHT ***//
else if (joystick.joy1_x1 > 30 && noLeftY() && noRightX())
move(xAClock(), xClock(), xAClock(), xClock());
//move(xClock(), xClock(), xAClock(), xAClock());
////*** STRAFE UP-LEFT ***//
//if (joystick.joy1_x1 < -30 && joystick.joy1_y1 > 30 && noRightX())
// move(0, joyAvg(), -joyAvg(), 0);
////*** STRAFE UP-RIGHT ***//
//if (joystick.joy1_x1 > 30 && joystick.joy1_y1 > 30 && noRightX())
// move(-joyAvg(), 0, 0, joyAvg());
////*** STRAFE BACK-LEFT ***//
//if (joystick.joy1_x1 < -30 && joystick.joy1_y1 < -30 && noRightX())
// move(joyAvg(), 0, 0, -joyAvg());
////*** STRAFE BACK-RIGHT ***//
//if (joystick.joy1_x1 > 30 && joystick.joy1_y1 < -30 && noRightX())
// move(0, -joyAvg(), joyAvg(), 0);
//============= END LEFT JOYSTICK =============//
//============== RIGHT JOYSTICK ===============//
//*** TURN LEFT ***//
if (joystick.joy1_x2 > 20 && noLeftX() && noLeftY())
move(-abs(joystick.joy1_x2));
//*** TURN RIGHT ***//
if (joystick.joy1_x2 < -20 && noLeftX() && noLeftY())
move(abs(joystick.joy1_x2));
//============ END RIGHT JOYSTICK ==============//
// Stop all wheels if no joystick input
if (noLeftX() && noLeftY() && noRightX()) move(0);
//--------------------------- END MAIN ROBOT CONTROL -------------------------//
//------------------------------- LIFT CONTROL -------------------------------//
if (joystick.joy2_TopHat == 0) lift(80); // Up JOY2
else if (joystick.joy2_TopHat == 4) lift(-70); // Down JOY2
else if (joystick.joy2_TopHat != 0 && joystick.joy2_TopHat != 4) //revert to JOY1 if no input on JOY2
{
if (joystick.joy1_TopHat == 0) lift(80); // Up JOY1
else if (joystick.joy1_TopHat == 4) lift(-70); // Down JOY1
else if (joystick.joy1_TopHat != 0 && joystick.joy1_TopHat != 4) lift(0); // Stop JOY1
}
//----------------------------- END LIFT CONTROL -----------------------------//
//------------------------------- CLAW CONTROL--------------------------------//
if (joy1Btn(5)==1) servo[claw] = 180; //open on pressing 5
else if (joy1Btn(6)==1) servo[claw] = 250; //close on pressing 6
else if (joy1Btn(7)==1) servo[claw] = 90; //lie flat on pressing 7
//----------------------------- END CLAW CONTROL -----------------------------//
//------------------------------ LIGHT CONTROL -------------------------------//
if (joy2Btn(2)==1) motor[light] = 50; // Button 2: Lights on JOY2
if (joy2Btn(4)==1) motor[light] = 0; // Button 4: Lights off JOY2
//---------------------------- END LIGHT CONTROL -----------------------------//
}
}
task main()
{
int threshold = 20;
nMotorEncoder[Armmotor]=0;
while (true)
{
nxtDisplayString(0, "encoder=%d", nMotorEncoder[Armmotor]);//Up is Positive
getJoystickSettings(joystick);
if(abs(joystick.joy1_y2) > threshold)
motor[Rightmotor] = joystick.joy1_y2;
else
motor[Rightmotor] = 0;
if(abs(joystick.joy1_y1) > threshold)
motor[Leftmotor] = -joystick.joy1_y1;
else
motor[Leftmotor] = 0;
if(abs(joystick.joy2_y2) > threshold)
motor[Nxtleftmotor] = joystick.joy2_y2;
else
motor[Nxtleftmotor] = 0;
if(abs(joystick.joy2_y1) > threshold)
motor[Nxtrightmotor] = joystick.joy2_y1;
else
motor[Nxtrightmotor] = 0;
if(joy1Btn(6))
motor[Ballmotor] = 100;
else
if(joy1Btn(8))
motor[Ballmotor] = -100;
else
motor[Ballmotor] = 0;
int armPos=nMotorEncoder[Armmotor];
if(joy2Btn(6) && armPos < 720)
motor[Armmotor] = 80;
else
if(joy2Btn(8) && armPos > 0)
motor[Armmotor] = -10;
else
motor[Armmotor] = 0;
if(joy2Btn(2))
{
servo[Rightservo] = 116;
servo[Leftservo] = 128;
}
if(joy2Btn(3))
{
servo[Rightservo] = 0;
servo[Leftservo] = 255;
}
if(joy2Btn(5))
servo[Rotatorservo] = 225;
if(joy2Btn(7))
servo[Rotatorservo] = 0;
}
}
