void TeleOp() //le teleop method
// @(^~^)@ DANA'S MONKEY...oh i'm a gummibear yes i'm a gummibear oh yes i'm a funny yummy chummy crummy gummibear oh ahhh
{
getJoystickSettings(joystick); //get updates from joystick
//wheels
if ( abs(joystick.joy1_y2) > 15 )
motor[motorR] = -joystick.joy1_y2*0.5;
else
motor[motorR] = 0;
if ( abs(joystick.joy1_y1) > 15 )
motor[motorL] = -joystick.joy1_y1*0.5;
else
motor[motorL] = 0;
/*
//arms
if(joy2Btn7() == 1) //if(joystick.joy2_TopHat == 0)
motor[motorArmR] = 25;
else if(joy2Btn() == 5) //else if(joystick.joy2_TopHat == 4)
motor[motorArmR] = -25;
else
motor[motorArmR] = 0;
//dana fixed this:
if(joy2Btn() == 8) //if(joystick.joy2_TopHat == 2)
motor[motorArmL] = 35;
else if(joy2Btn() == 6) //else if(joystick.joy2_TopHat == 6)
motor[motorArmL] = -35;
else
motor[motorArmL] = 0;
/*
//scissors
if ( abs(joystick.joy2_y1) > 15 )
motor[motorScsL] = joystick.joy2_y1*0.4;
else
motor[motorScsL] = 0;
//left right servos
if(joystick.joy2_TopHat == 2)
servo[servo3] = ServoValue[servo3] - caseyIsTall;
else if(joystick.joy2_TopHat == 6)
servo[servo3] = ServoValue[servo3] + caseyIsTall;
if(joy2Btn(1) == 1)
servo[servo4] = ServoValue[servo4] + caseyIsTall;
else if(joy2Btn(3) == 1)
servo[servo4] = ServoValue[servo4] - caseyIsTall;
*/
//spinning servos
/*
if(joystick.joy2_TopHat == 4)
servo[servo5] = ServoValue[servo5] + caseyIsTall;
else if(joystick.joy2_TopHat == 0)
servo[servo5] = ServoValue[servo5] - caseyIsTall;
if(joy2Btn(2) == 1)
servo[servo6] = ServoValue[servo6] - caseyIsTall;
else if(joy2Btn(4) == 1)
servo[servo6] = ServoValue[servo6] + caseyIsTall;
*/
/*
//spinning servos continuous
if(joystick.joy2_TopHat == 4)
servo[servo5] = alexLikesMenRoof;
else if(joystick.joy2_TopHat == 0)
servo[servo5] = alexLikesMenFloor;
else
servo[servo5] = 127;
*/
/*
if (joy2Btn() == 2)
servo[servo1] = alexLikesMenFloor;
else if (joy2Btn() == 4)
servo[servo1] = alexLikesMenRoof;
else
servo[servo1] = 127;
*/
//lego motor servos
if(joy1Btn(1) == 1)
motor[nxtMotorA] = 50;
else if(joy1Btn(3) == 1)
motor[nxtMotorA] = -50;
else
motor[nxtMotorA] = 0;
}
task main()
{
waitForStart();
bFloatDuringInactiveMotorPWM = false;
nMotorEncoder[shoulderJoint] = 0;
writeDebugStreamLine("encoder set to: %d", nMotorEncoder[shoulderJoint]);
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){
// Button 4 controls the wrist servo movement
if ((ServoValue[handJoint] + 5) < maxHandValue){
servo[handJoint] = ServoValue[handJoint] + 5;
}
}
if (joy1Btn(2) == 1){
// Button 2 controls the wrist servo movement
if ((ServoValue[handJoint] - 5) > minHandValue){
servo[handJoint] = ServoValue[handJoint] - 5;
}
}
if (joy1Btn(6) == 1){
// Button 6 deploys the arm to scoring position
fold_arm(false);
}
if (joy1Btn(5) == 1){
// Button 5 folds the arm down
fold_arm(true);
}
if (joy1Btn(1) == 1){
// Holding button 1 while driving gives a speed boost
maxVal = 100;
}
if (joy1Btn(1) != 1){
// Anytime button 1 is not pressed, the speed is at 40
maxVal = 40;
}
if (joy1Btn(8) == 1){
// Button 8 resets the encoder value to 0
nMotorEncoder[shoulderJoint] = 0;
currentEncoder = nMotorEncoder[shoulderJoint];
tempEncoder = currentEncoder;
writeDebugStreamLine("encoder reset to: %d", currentEncoder);
}
drive(cont1_left_yval, cont1_left_xval, maxVal);
shoulderMovement(cont1_right_yval);
handMovement(cont1_dPad);
}
}
void processWheelMovement(){
int forwardLeft, forwardRight;
forwardLeft=(joystick.joy1_y2+joystick.joy1_x2)/2;//Two joystick drive - Max B. 2011
forwardRight=(joystick.joy1_y2-joystick.joy1_x2)/2;
if(abs(forwardLeft)<10)
forwardLeft=0;
if(abs(forwardRight)<10)
forwardRight=0;
if(joy1Btn(8)==1){
forwardRight/=4;
forwardLeft/=4;
}
if(joy1Btn(9)==1){ //copyrighted Hooks feature by Lev K. 2011
motor[LeftHook]=35;
motor[RightHook]=35;
}else{
if(joy1Btn(10)==1){
motor[LeftHook]=-50;
motor[RightHook]=-50;
}else{
motor[LeftHook]=0;
motor[RightHook]=0;
}
}
if(joy1Btn(12)==1){//copyrighted PULSE feature by Max B. 2011. Do compare complexity to above script..
if(!btn12Down){
switch(joystick.joy1_TopHat){
case 0:
turnOnWheels(MODE_MOVE_STRAIGHT | DIR_FWD, true);
break;
case 4://forward or backward straight
turnOnWheels(MODE_MOVE_STRAIGHT | DIR_BACK, true);
break;
case 2:
turnOnWheels(MODE_PIVOT | TURN_RIGHT, true);
break;
case 6://turning (pivot) motion
turnOnWheels(MODE_PIVOT | TURN_LEFT, true);
break;
case 7:
turnOnWheels(MODE_SWING | TURN_LEFT | DIR_FWD, true);
break;
case 1://swing forward
turnOnWheels(MODE_SWING | TURN_RIGHT | DIR_FWD, true);
break;
case 3:
turnOnWheels(MODE_SWING | TURN_RIGHT | DIR_BACK, true);
break;
case 5://swing backward
turnOnWheels(MODE_SWING | TURN_LEFT | DIR_BACK, true);
break;
}
wait10Msec(10);
turnOffWheels();
btn12Down=true;
}
}else{//regular drive
btn12Down=false;
switch(joystick.joy1_TopHat){
case 0:
case 4://forward or backward straight
turnOnWheels(MODE_MOVE_STRAIGHT | DIR_FWD, true, (forwardLeft+forwardRight)/2);
break;
case 2:
case 6://turning (pivot) motion
turnOnWheels(MODE_PIVOT | TURN_LEFT, true, forwardRight);
break;
case 7:
case 1://swing forward
turnOnWheels(MODE_SWING | TURN_LEFT | DIR_FWD, true, forwardRight-forwardLeft);
break;
case 3:
case 5://swing backward
turnOnWheels(MODE_SWING | TURN_RIGHT | DIR_BACK, true, forwardRight-forwardLeft);
break;
default://no quantization
setWheelPwr(forwardLeft, forwardRight, (2*forwardLeft+forwardRight)/3, (2*forwardRight+forwardLeft)/3);
break;
}
}
if(joy1Btn(6)==1&&!btn6Down){//lower bridge
btn6Down=true;
putDownBridge();
StopTask(moveUpBridge);
StartTask(moveUpBridge);
}
if(joy1Btn(6)!=1)
btn6Down=false;
}
task arm() {
getJoystickSettings(joystick);
while(true) {
if(joystick.joy2_TopHat == 0) {
motor[conveyor] = 100;
}
else if(joystick.joy2_TopHat == 4) {
motor[conveyor] = -100;
}
else {
motor[conveyor] = 0;
}
if(joy1Btn(5)) {
servo[grabber] = 255;
}
else if(joy1Btn(7)) {
servo[grabber] = 0;
}
else {
servo[grabber] = 127;
}
if(joy1Btn(6)) {
motor[harvester] = 100;
}
else if(joy1Btn(8)) {
motor[harvester] = -100;
}
else {
motor[harvester] = 0;
}
if(joy1Btn(2)) {
servo[grabberLift1] = 255;
servo[grabberLift2] = 0;
}
else if(joy1Btn(4)) {
servo[grabberLift1] = 0;
servo[grabberLift2] = 255;
}
else {
servo[grabberLift1] = 127;
servo[grabberLift2] = 127;
}
//if(joy2Btn(2)) {
// servo[sock] = 255;
//}
//else if(joy2Btn(4)) {
// servo[sock] = 0;
//}
//else {
// servo[sock] = 127;
//}
if(joy2Btn(5)) {
motor[lift1] = 100;
}
else if(joy2Btn(7)) {
motor[lift1] = -100;
}
else {
motor[lift1] = 0;
}
if(joy2Btn(6)) {
motor[lift2] = 100;
}
else if(joy2Btn(8)) {
motor[lift2] = -100;
}
else {
motor[lift2] = 0;
}
if(joy2Btn(3)) {
servo[kickstand] = 140;
}
else {
servo[kickstand] = 255;
}
if(joy1Btn(9)) {
while(joy1Btn(9));
slow = !slow;
PlaySound(soundBeepBeep);
}
if(joy1Btn(10)) {
while(joy1Btn(10));
reverse = -1 * reverse;
PlaySound(soundBeepBeep);
}
}
}
// This is the superdrive task. If you can think of any better names, please tell me :P
// This task incorporates two modes, regular mecanum driving and free-spinning mode
task superDrive(){
float x1,y1,x2,y2,LF,RF,LB,RB= 0;
int minJoy = 12;
float turning;
float mag; // magnitude of the joystick vector
float initialHeading = radheading; float calcHeading = radheading; // sets a base heading for the
float movementAmount, turningAmount, totalAmount; // for apportioning power to turning and moving
while(true){
// Get joystick values
x1 = joystick.joy1_x1 * .5;y1 = joystick.joy1_y1 * .5;
x2 = joystick.joy1_x2 * .5;y2 = joystick.joy1_y2 * .5;
// function for making new initial heading
if (joy1Btn(5)==1){
initialHeading = radheading;
}
// starts free-spinning mode
if (joy1Btn(6)==1){
// find joystick vector angle
joyAngle = atan2(y1, x1);
// find joystick vector magnitude
mag = sqrt(x1*x1+y1*y1)/2;
if (mag>64)
mag = 64;
// get calculated heading
calcHeading = radheading - initialHeading;
// find the direction needed to move
moveDirection(joyAngle + calcHeading, mag);
// fix movement drifting
if (abs(joystick.joy1_x1)<minJoy&&abs(joystick.joy1_y1)<minJoy/*&&abs(joystick.joy1_x2)<minJoy*/){
FLset=0;FRset=0;
}
// find turning magnitude
turning = x2/2;
//fix turning drifting
if (abs(joystick.joy1_x2)<minJoy){
turning = 0;
}
// apportion motor capacity to movement and turning
// TODO make this better... although that will be difficult
totalAmount = 1 + turning + mag*3;
movementAmount = (mag*3)/totalAmount;
turningAmount = turning/totalAmount;
// Apply finished values to motors
motor[FL] = FLset*movementAmount+turning*turningAmount;
motor[FR] = FRset*movementAmount-turning*turningAmount;
motor[BL] = FRset*movementAmount+turning*turningAmount;
motor[BR] = FLset*movementAmount-turning*turningAmount;
} else {
// Resets movement values
LF = 0;RF = 0;LB = 0;RB = 0;
// Get joystick values
x1 = joystick.joy1_x1 * .5;y1 = joystick.joy1_y1 * .5;
x2 = joystick.joy1_x2 * .5;y2 = joystick.joy1_y2 * .5;
// Handle Strafing Movement
LF += x1;RF -= x1;LB -= x1;RB += x1;
// Handle Regular Movement
LF += y1;RF += y1;LB += y1;RB += y1;
// Handle Turning Movement
LF += x2;RF -= x2;LB += x2;RB -= x2;
if (abs(joystick.joy1_x1)<minJoy&&abs(joystick.joy1_y1)<minJoy&&abs(joystick.joy1_x2)<minJoy){
LF = 0;RF = 0;LB = 0;RB = 0;
}
// Apply Finished values to motors.
motor[FL] = LF;
motor[FR] = RF;
motor[BL] = LB;
motor[BR] = RB;
}
wait10Msec(1);
}
}
task move()
{
static int speed = 50, m_speed = -50, slow_speed = 20, slow_m_speed = -20;
Forward_L = 0;
Backward_L = 0;
Forward_R = 0;
Backward_R = 0;
Flag_Raise = 127;
while (joy1Btn(10) != 1)
getJoystickSettings(joystick);
while(true)
{
if (joy1Btn (7))
{
while (joy1Btn (7)) /* turn left */
{
Forward_L = speed;
Backward_L = speed;
Forward_R = speed;
Backward_R = speed;
}
while (Forward_L >= 0)
{
Forward_L -= 1;
Backward_L -= 1;
Forward_R -= 1;
Backward_R -= 1;
wait1Msec(10);
}
if (Forward_L != 0 || Forward_R != 0 || Backward_L != 0 || Backward_R != 0)
{
Forward_L = 0;
Backward_L = 0;
Forward_R = 0;
Backward_R = 0;
}
}
if (joy1Btn (8))
{
while (joy1Btn (8)) /* turn right*/
{
Forward_L = m_speed;
Backward_L = m_speed;
Forward_R = m_speed;
Backward_R = m_speed;
}
while (Forward_L <= 0)
{
Forward_L += 1;
Backward_L += 1;
Forward_R += 1;
Backward_R += 1;
wait1Msec(10);
}
if (Forward_L != 0 || Forward_R != 0 || Backward_L != 0 || Backward_R != 0)
{
Forward_L = 0;
Backward_L = 0;
Forward_R = 0;
Backward_R = 0;
}
}
if (joy1Btn (4))
{
while (joy1Btn (4)) /* move forward*/
{
Forward_L = m_speed;
Backward_L = m_speed;
Forward_R = speed;
Backward_R = speed;
}
while (Forward_L <= 0)
{
Forward_L += 1;
Backward_L += 1;
Forward_R -= 1;
Backward_R -= 1;
wait1Msec(10);
}
if (Forward_L != 0 || Forward_R != 0 || Backward_L != 0 || Backward_R != 0)
{
Forward_L = 0;
Backward_L = 0;
Forward_R = 0;
Backward_R = 0;
}
}
if (joy1Btn (2))
{
while (joy1Btn (2)) /* move backward */
{
Forward_L = speed;
Backward_L = speed;
Forward_R = m_speed;
Backward_R = m_speed;
}
while (Forward_L >= 0)
{
Forward_L -= 1;
Backward_L -= 1;
Forward_R += 1;
Backward_R += 1;
wait1Msec(10);
}
if (Forward_L != 0 || Forward_R != 0 || Backward_L != 0 || Backward_R != 0)
{
Forward_L = 0;
Backward_L = 0;
Forward_R = 0;
Backward_R = 0;
}
}
if (joy1Btn (1))
{
while (joy1Btn (1)) /* move left */
{
Forward_L = speed;
Backward_L = m_speed;
Forward_R = m_speed;
Backward_R = speed;
}
while (Forward_L >= 0)
{
Forward_L -= 1;
Backward_L += 1;
Forward_R += 1;
Backward_R -= 1;
wait1Msec(10);
}
if (Forward_L != 0 || Forward_R != 0 || Backward_L != 0 || Backward_R != 0)
{
Forward_L = 0;
Backward_L = 0;
Forward_R = 0;
Backward_R = 0;
}
}
if (joy1Btn (3))
{
while (joy1Btn (3)) /* move right */
{
Forward_L = m_speed;
Backward_L = speed;
Forward_R = speed;
Backward_R = m_speed;
}
while (Forward_L <= 0)
{
Forward_L += 1;
Backward_L -= 1;
Forward_R -= 1;
Backward_R += 1;
wait1Msec(10);
}
if (Forward_L != 0 || Forward_R != 0 || Backward_L != 0 || Backward_R != 0)
{
Forward_L = 0;
Backward_L = 0;
Forward_R = 0;
Backward_R = 0;
}
}
if (joystick.joy1_TopHat == 0)
{
while (joystick.joy1_TopHat == 0) /* slow move forward */
{
Forward_L = slow_m_speed;
Backward_L = slow_m_speed;
Forward_R = slow_speed;
Backward_R = slow_speed;
}
while (Forward_L <= 0)
{
Forward_L += 1;
Backward_L += 1;
Forward_R -= 1;
Backward_R -= 1;
wait1Msec(10);
}
if (Forward_L != 0 || Forward_R != 0 || Backward_L != 0 || Backward_R != 0)
{
Forward_L = 0;
Backward_L = 0;
Forward_R = 0;
Backward_R = 0;
}
}
if (joystick.joy1_TopHat == 4)
{
while (joystick.joy1_TopHat == 4) /* slow move backward */
{
Forward_L = slow_speed;
Backward_L = slow_speed;
Forward_R = slow_m_speed;
Backward_R = slow_m_speed;
}
while (Forward_L >= 0)
{
Forward_L -= 1;
Backward_L -= 1;
Forward_R += 1;
Backward_R += 1;
wait1Msec(10);
}
if (Forward_L != 0 || Forward_R != 0 || Backward_L != 0 || Backward_R != 0)
{
Forward_L = 0;
Backward_L = 0;
Forward_R = 0;
Backward_R = 0;
}
}
if (joystick.joy1_TopHat == 6)
{
while (joystick.joy1_TopHat == 6) /* slow move left */
{
Forward_L = slow_speed;
Backward_L = slow_m_speed;
Forward_R = slow_m_speed;
Backward_R = slow_speed;
}
while (Forward_L >= 0)
{
Forward_L -= 1;
Backward_L += 1;
Forward_R += 1;
Backward_R -= 1;
wait1Msec(10);
}
if (Forward_L != 0 || Forward_R != 0 || Backward_L != 0 || Backward_R != 0)
{
Forward_L = 0;
Backward_L = 0;
Forward_R = 0;
Backward_R = 0;
}
}
if (joystick.joy1_TopHat == 2)
{
while (joystick.joy1_TopHat == 2) /* slow move right */
{
Forward_L = slow_m_speed;
Backward_L = slow_speed;
Forward_R = slow_speed;
Backward_R = slow_m_speed;
}
while (Forward_L <= 0)
{
Forward_L += 1;
Backward_L -= 1;
Forward_R -= 1;
Backward_R += 1;
wait1Msec(10);
}
if (Forward_L != 0 || Forward_R != 0 || Backward_L != 0 || Backward_R != 0)
{
Forward_L = 0;
Backward_L = 0;
Forward_R = 0;
Backward_R = 0;
}
}
if (joy1Btn (6))
{
while (joy1Btn (6)) /* slow turn right*/
{
Forward_L = slow_m_speed;
Backward_L = slow_m_speed;
Forward_R = slow_m_speed;
Backward_R = slow_m_speed;
}
while (Forward_L <= 0)
{
Forward_L += 1;
Backward_L += 1;
Forward_R += 1;
Backward_R += 1;
wait1Msec(10);
}
if (Forward_L != 0 || Forward_R != 0 || Backward_L != 0 || Backward_R != 0)
{
Forward_L = 0;
Backward_L = 0;
Forward_R = 0;
Backward_R = 0;
}
}
if (joy1Btn (5))
{
while (joy1Btn (5)) /* slow turn left */
{
Forward_L = slow_speed;
Backward_L = slow_speed;
Forward_R = slow_speed;
Backward_R = slow_speed;
}
while (Forward_L >= 0)
{
Forward_L -= 1;
Backward_L -= 1;
Forward_R -= 1;
Backward_R -= 1;
wait1Msec(10);
}
if (Forward_L != 0 || Forward_R != 0 || Backward_L != 0 || Backward_R != 0)
{
Forward_L = 0;
Backward_L = 0;
Forward_R = 0;
Backward_R = 0;
}
}
while (joy1Btn (9))
{
Flag_Raise = 0; /* 0 - Full Power/ Speed Reverse; 127 - Stop; 256 - Full Power/ Speed Forward */
}
Forward_L = 0;
Backward_L = 0;
Forward_R = 0;
Backward_R = 0;
Flag_Raise = 127;
}
}
task main()
{
waitForStart();
while(true)
{
getJoystickSettings(joystick);
//first joypad
//right drive
const int threshold =8; // Int 'threshold' will allow us to ignore low readings that keep our robot in perpetual motion.
if(abs(joystick.joy1_y1) > threshold) // If the right analog stick's Y-axis readings are either above or below the threshold:
{
motor[RightMotor] = joystick.joy1_y1; // Motor D 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[RightMotor] = 0; // Motor D is stopped with a power level of 0.
}
//left drive
if(abs(joystick.joy1_y2) > threshold) // If the left analog stick's Y-axis readings are either above or below the threshold:
{
motor[LeftMotor] = joystick.joy1_y2; // Motor E is assigned a power level equal to the left analog stick's Y-axis reading.
}
else // Else if the readings are within the threshold:
{
motor[LeftMotor] = 0; // Motor E is stopped with a power level of 0.
}
//Flag Raiser
if(joy1Btn(5))
{
motor[FlagRaiser] = 80;
}
else if(joy1Btn(7))
{
motor[FlagRaiser] = -80;
}
else
{
motor[FlagRaiser] = 0;
}
//Second Joypad
//Tower Lift--has two paralell motors mounted on the uprights
if(joy2Btn(2))//A
{
motor[TowerLift]=-80;//down
}else if(joy2Btn(3))//B
{
motor[TowerLift]=80;//up
}else//not A, or B...
{
motor[TowerLift]=0;//stop
}
//lift--has one motor mounted at the tower cross-section
if(joy2Btn(4))//X
{
writeDebugStreamLine("arm: %i", nMotorEncoder[potentiometer]);
motor[Lift]=80;//down
}else if(joy2Btn(1))//Y
{
writeDebugStreamLine("arm: %i", nMotorEncoder[potentiometer]);
motor[Lift]=-80;//up
}else//not X, or Y...
{
motor[Lift]=0;//stop
}
//Block Gate
if(joy2Btn(6))//LB
{
servo[BlockGate]=254;// opens the block gate
}else//not LB...
{
servo[BlockGate]=5;//close block gate
}
//ejector
if(joy2Btn(8)&&servo[BlockGate] !=5)
{
servo[Ejector]=100;//opens the ejector servo
}else
{
servo[Ejector]=5;//closes the ejector servo
}
}//end of while(true)
}//main end block
task main ()
{
initializeRobot();
waitForStart();
StartTask(autoCover);
while (1)
{
getJoystickSettings(joystick);
//Holonomic Drive Variables
const int t = 8;
const float standardDriveScale = 0.79;
const float precisionDriveScale = 0.15;
int x1 = (abs(joystick.joy1_x1) > t) ? joystick.joy1_x1 : 0;
int y1 = (abs(joystick.joy1_y1) > t) ? joystick.joy1_y1 : 0;
int x2 = (abs(joystick.joy1_x2) > t) ? joystick.joy1_x2 : 0;
//int y2 = (abs(joystick.joy1_y2) > t) ? joystick.joy1_y2 : 0;
int stdFrontLeftMotorSetting = (- x1 - y1 + x2);
int stdFrontRightMotorSetting = (x1 - y1 - x2);
int stdRearLeftMotorSetting = (x1 - y1 + x2);
int stdRearRightMotorSetting = (- x1 - y1 - x2);
int egFrontLeftMotorSetting = x2;
int egFrontRightMotorSetting = y1;
int egRearLeftMotorSetting = y1;
int egRearRightMotorSetting = x2;
//Standard holonomic
if (driveMode == DRIVE_MODE_STD) {
motor [frontLeft] = ((joy1Btn (5) == 1) || (joy1Btn (6) == 1) || (joy1Btn (7) == 1) || (joy1Btn (8) == 1)) ? stdFrontLeftMotorSetting * precisionDriveScale : stdFrontLeftMotorSetting * standardDriveScale;
motor [frontRight] = ((joy1Btn (5) == 1) || (joy1Btn (6) == 1) || (joy1Btn (7) == 1) || (joy1Btn (8) == 1)) ? stdFrontRightMotorSetting * precisionDriveScale : stdFrontRightMotorSetting * standardDriveScale;
motor [rearLeft] = ((joy1Btn (5) == 1) || (joy1Btn (6) == 1) || (joy1Btn (7) == 1) || (joy1Btn (8) == 1)) ? stdRearLeftMotorSetting * precisionDriveScale : stdRearLeftMotorSetting * standardDriveScale;
motor [rearRight] = ((joy1Btn (5) == 1) || (joy1Btn (6) == 1) || (joy1Btn (7) == 1) || (joy1Btn (8) == 1)) ? stdRearRightMotorSetting * precisionDriveScale : stdRearRightMotorSetting * standardDriveScale;
}
else if (driveMode == DRIVE_MODE_EG) {
motor [frontLeft] = (egFrontLeftMotorSetting * precisionDriveScale - ((joy1Btn(7) == 1) ? 20 : (joy1Btn(8) == 1) ? -20 : 0));
motor [frontRight] = (egFrontRightMotorSetting * precisionDriveScale + ((joy1Btn(7) == 1) ? 20 : (joy1Btn(8) == 1) ? -20 : 0));
motor [rearLeft] = (egRearLeftMotorSetting * precisionDriveScale - ((joy1Btn(7) == 1) ? 20 : (joy1Btn(8) == 1) ? -20 : 0));
motor [rearRight] = (egRearRightMotorSetting * precisionDriveScale + ((joy1Btn(7) == 1) ? 20 : (joy1Btn(8) == 1) ? -20 : 0));
}
if (joy1Btn(1) == 1 && joy1Btn(2) == 1 && joy1Btn(3) == 1 && joy1Btn(4) == 1) {
if (driveMode == DRIVE_MODE_STD) {
switchDriveMode(DRIVE_MODE_EG);
}
else if (driveMode == DRIVE_MODE_EG) {
switchDriveMode(DRIVE_MODE_STD);
}
}
if (joy2Btn(10) == 1) {
if (mechMode == MECH_MODE_STD) {
switchMechMode(MECH_MODE_INVERTED);
}
else if (mechMode == MECH_MODE_INVERTED) {
switchMechMode(MECH_MODE_STD);
}
}
if (mechMode == MECH_MODE_STD) {
if (joy2Btn(7) == 1) {
motor [arm] = -100;
}
else if (joy2Btn(5) == 1) {
motor [arm] = 100;
} else {
motor [arm] = 0;
}
if (joy2Btn(6) == 1) {
motor [leftLift] = 100;
motor [rightLift] = 100;
}
else if (joy2Btn(8) == 1) {
motor [leftLift] = -100;
motor [rightLift] = -100;
} else {
motor [leftLift] = 0;
motor [rightLift] = 0;
}
}
else if (mechMode == MECH_MODE_INVERTED) {
if (joy2Btn(7) == 1) {
motor [arm] = 100;
}
else if (joy2Btn(5) == 1) {
motor [arm] = -100;
} else {
motor [arm] = 0;
}
if (joy2Btn(6) == 1) {
motor [leftLift] = -100;
motor [rightLift] = -100;
}
else if (joy2Btn(8) == 1) {
motor [leftLift] = 100;
motor [rightLift] = 100;
} else {
motor [leftLift] = 0;
motor [rightLift] = 0;
}
}
if (joy2Btn(6) != 1 && joy2Btn(8) != 1) {
motor [rightLift] = abs(joystick.joy2_y1) > t ? joystick.joy2_y1 * standardDriveScale : 0;
motor [leftLift] = abs(joystick.joy2_y2) > t ? joystick.joy2_y2 * standardDriveScale : 0;
}
if (joystick.joy2_TopHat == 2) {
servo [scoopCover] = 230;
}
else if (joystick.joy2_TopHat == 6) {
servo [scoopCover] = 20;
}
if (joystick.joy2_TopHat == 0) {
servo [leftLatch] = 20;
servo [rightLatch] = 238;
}
else if (joystick.joy2_TopHat == 4) {
servo [leftLatch] = 252;
servo [rightLatch] = 10;
}
if (joy2Btn(1) == 1) {
motor [flagSpinner] = -100;
}
else if (joy2Btn(3) == 1) {
motor [flagSpinner] = 100;
} else {
motor [flagSpinner] = 0;
}
if (joy2Btn(2) == 1) {
servo [spinnerLift] = 255;
}
else if (joy2Btn(4) == 1) {
servo [spinnerLift] = 0;
} else {
servo [spinnerLift] = 127;
}
}
}
// 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 = 25;
int teleTopSpeed = 20;
int teleMotorSpeed = 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_y2 / 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_y1 / 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
teleMotorSpeed = joystick.joy2_y1 / JOYSTICK_Y1_MAX * teleTopSpeed; // Map the teleMotorSpeed variable to joystick 2_y1
if (abs(teleMotorSpeed) < JOYSTICK_DEAD_ZONE) teleMotorSpeed = 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[teleMotor] = teleMotorSpeed;
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(1) == 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);
}
if (joy2Btn(1) == 1) {
// Move Ramp Up
motor[rampMotor] = RAMP_POWER;
wait1Msec(RAMP_DURATION);
motor[rampMotor] = 0;
wait1Msec(NUDGE_DELAY);
}
if (joy2Btn(3) == 1) {
// Move Ramp Down
motor[rampMotor] = RAMP_POWER * -1;
wait1Msec(RAMP_DURATION);
motor[rampMotor] = 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()
{
// Some kind of gyro sensor calibration
calibrateGyro(gyro);
while(true)
{
// Update joystick values
getJoystickSettings(joystick);
// Drive code
motor[left] = speedCurve(joystick.joy1_y2);
motor[right] = speedCurve(joystick.joy1_y1);
//motor[slide] = speedCurve(joystick.joy1_y1) / 2;
//motor[intake] = speedCurve(joystick.joy1_y2) / 2;
// Bumper controls for linear slides
if(joy1Btn(5) || joy2Btn(5))
{
motor[slide] = 75;
}
else if(joy1Btn(6) || joy2Btn(6))
{
motor[slide] = -75;
}
else
{
motor[slide] = 0;
}
// Trigger controls for intake systems
if(joy1Btn(7) || joy2Btn(7))
{
if (motor[intake] == 100)
{
motor[intake] = 0;
}
else
{
motor[intake] = 100;
}
wait1Msec(100);
}
if(joy1Btn(8) || joy2Btn(8))
{
if (motor[intake] == -50)
{
motor[intake] = 0;
}
else
{
motor[intake] = -50;
}
wait1Msec(100);
}
if(joystick.joy1_TopHat == 0)
{
motor[right] = 75;
motor[left] = 75;
}
// Turn on the gyro stabilization, ask Ethan
while(joystick.joy1_TopHat == 0)
{
getJoystickSettings(joystick);
if(SensorValue(gyro) > offset) //turning right
{
motor[right] = motor[right] + 1;
}
if(SensorValue(gyro) < offset) //turning left
{
motor[left] = motor[left] + 1;
}
gyroed = true;
}
if(joystick.joy1_TopHat == 4)
{
motor[right] = -75;
motor[left] = -75;
}
// Turn on the gyro stabilization backwards, ask Ethan
while(joystick.joy1_TopHat == 4)
{
getJoystickSettings(joystick);
if(SensorValue(gyro) > offset) //turning right
{
motor[right] = motor[right] - 1;
}
if(SensorValue(gyro) < offset) //turning left
{
motor[left] = motor[left] - 1;
}
gyroed = true;
}
if(joystick.joy1_TopHat == 6)
{
turnDegrees(gyro, left, 70, 75);
}
if(joystick.joy1_TopHat == 2)
{
turnDegrees(gyro, right, 70, 75);
}
// Reset gyro stuff
if(gyroed)
{
motor[left] = 0;
motor[right] = 0;
gyroed = false;
}
if(joystick.joy1_y1 == lastJoyVal && joystick.joy1_y1 != 0)//detect loss of connection
{
cycles ++;
}
else
{
cycles = 0;
lastJoyVal = joystick.joy1_y1;
}
if(cycles == 32767)//shut off motors if we lose connection
{
motor[left] = 0;
motor[right] = 0;
motor[slide] = 0;
joystick.joy1_y1 = 0;
joystick.joy1_y2 = 0;
cycles = 0;
}
if (joy1Btn(3) || joy2Btn(3))
{
if (servo[gripper] == 255)
{
servo[gripper] = 0;
}
else
{
servo[gripper] = 255;
}
wait1Msec(100);
}
}//end while loop
}//end task main