task main()
{

	while(true)                        // infinite loop:
	{
 	 	getJoystickSettings(joystick);     // update buttons and joysticks

		motor[ArmUpDown] = scaledJoyDrive(joystick.joy1_y1);
		motor[ScoopLeft] = scaledJoyDrive(joystick.joy1_y2);
		motor[ScoopRight] = scaledJoyDrive(joystick.joy1_y2);

		// press button Y to make scoop go up 15 degrees
		if(joy1Btn(4) == 1)                  // If Joy1-Button1 is pressed:
			scoopNudge(15);

		// press button A to make scoop go down 15 degrees
		if(joy1Btn(2) == 1)                  // If Joy1-Button1 is pressed:
			scoopNudge(-15);

		}
}
//-------------------------main---------------------------
task main()
{
  initializeRobot();

  waitForStart();

  //start tasks
  StartTask(RunTankDrive);
  //StartTask(RunArm);
  StartTask(RunClaw);

  while(true) //teleop phase
  {
    getJoystickSettings(joystick);

    //Sensor-related thing joy1Btn(1)
    //Sensor-related thing joy1Btn(2)
    //Sensor-related thing joy1Brn(3)

  }
}
task ScoringArm()
{
	while(true)
	{
		//-----------joystick 2------------//
		//high priority
		getJoystickSettings(joystick);
		if(joy2Btn(6) && !joy2Btn(8)) //move arm up
		{
			runArm(80);
		}
		else if(joy2Btn(8) && !joy2Btn(6)) //move arm down
		{
			runArm(-40);
		}
		else //if not pressed/pressed at same time
		{
			joy1Arm();
		}
	}
}
void joy1Arm()
{
	while(true)
	{
		//-----------joystick 1------------//
		//low priority
		getJoystickSettings(joystick);
		if(joy1Btn(6) && !joy1Btn(8)) //move arm up
		{
			runArm(80);
		}
		else if(joy1Btn(8) && !joy1Btn(6)) //move arm down
		{
			runArm(-40);
		}
		else //if not pressed/pressed at same time
		{
			runArm(STOP);
		}
	}
}
task IntakeToggleTwo()
{
	bool toggle = false;
	while(true)
	{
		getJoystickSettings(joystick);
		if(joy1Btn(2))
		{
			toggle = !toggle;
			wait10Msec(25);
		}
		if(toggle)
		{
			runIntake(100);
		}
		else
		{
			runIntake(0);
		}
	}
}
Exemplo n.º 6
0
task main()
{

		init();
		float servoff=0;
		waitForStart();
		StartTask(RSL);
		StartTask(input);
		StartTask(armControl);
		while(true){
				getJoystickSettings(joystick);
				if(!(joy1Btn(8)==1&&joy1Btn(7)==1)){
					motor[left]=joystick.joy1_y1;
					motor[right]=-joystick.joy1_y2;
				}	else{
					motor[left]=joystick.joy1_y1/4;
					motor[right]=-joystick.joy1_y2/4;
				}
				if(joy1Btn(2)==1&&serv<255){
						serv+=1;
				}
				if(joy1Btn(4)==1&&serv>0){
						serv-=1;
				}
				if(joy1Btn(3)==1){
					locked=true;
					encLock=nMotorEncoder[grab];
				}
				if(joy1Btn(6)==1){motor[grab]=-15;locked=false;}
				else if(joy1Btn(5)==1){motor[grab]=15;locked=false;}
				else motor[grab]=0;

				if(joy1Btn(8)==1&&joy1Btn(7)!=1)motor[motorC]=-10;
				else if(joy1Btn(7)==1&&joy1Btn(8)!=1)motor[motorC]=10;
				else motor[motorC]=0;
				servo[bucket]=serv;
				int ap=joystick.joy2_y2;
				motor[arm]=abs(ap)>5?ap:0;
		}
}
Exemplo n.º 7
0
void drive() {                  // Code for driving the robot
#if ( _TARGET == "VirtWorld" )
    getJoystickSettings(joystick);
    motor[port10] = joystick.joy1_y2 / speed_divisor ;
    motor[port1] = -joystick.joy1_y1 / speed_divisor ;
#else
    if (forward_drive == 1)
    {
        if ( vexRT[Ch3] > DEADBAND || vexRT[Ch3] < -DEADBAND ) {
            // Controlling the left wheel using channel 3
            motor[leftwheel] = vexRT[Ch3] / speed_divisor ;
        } else {
            motor[leftwheel] = 0 ;
        }
        if ( vexRT[Ch2] > DEADBAND || vexRT[Ch2] < -DEADBAND ) {
            //Controlling the right wheel using channel 2
            motor[rightwheel] = -vexRT[Ch2] / speed_divisor ;
        } else {
            motor[rightwheel] = 0 ;
        }
    }
    else
    {
        if ( vexRT[Ch2] > DEADBAND || vexRT[Ch2] < -DEADBAND ) {
            // Reverse  the left wheel using channel 2
            motor[leftwheel] = -vexRT[Ch2] / speed_divisor ;
        }
        else {
            motor[leftwheel] = 0 ;
        }
        if ( vexRT[Ch3] > DEADBAND || vexRT[Ch3] < -DEADBAND ) {
            // Reverse the right wheel using channel 3
            motor[rightwheel] = vexRT[Ch3] / speed_divisor ;
        }
        else {
            motor[rightwheel] = 0 ;
        }
    }
#endif
}
Exemplo n.º 8
0
task main(){
  float x1, y1, x2, y2;
  bool harvesting = false;
  bool belting = false;

	waitForStart();
	initTeleOp();
	StartTask(arm);
  while(true){
  	getJoystickSettings(joystick);

  	//AUGMENTED-TANK JOYSTICKING SYSTEM
	  x1 = normalize10(reverse * joystick.joy1_x1);
	  x2 = normalize10(reverse * joystick.joy1_x2);
	  y1 = normalize10(reverse * joystick.joy1_y1);
	  y2 = normalize10(reverse * joystick.joy1_y2);

	  if(slow) {
	  	x1 *= 0.2;
	  	x2 *= 0.2;
	  	y1 *= 0.2;
	  	y2 *= 0.2;
	  }

	  //float JoyToWheel = 95.0 / max(max(max(abs(y2 + x2),abs(y1 - x1)),max(abs(y2 - x1),abs(y2 + x2))), 10);
		float joyToWheel = 1.0;
		if(reverse == -1) {
			motor[motorFrontRight] =  normalize10(y1 - x2) * joyToWheel;
		  motor[motorFrontLeft] = normalize10(y2 + x1) * joyToWheel;
		  motor[motorBackRight] =  normalize10(y1 + x1) * joyToWheel;
		  motor[motorBackLeft] =  normalize10(y2 - x2) * joyToWheel;
		}
		else {
		  motor[motorFrontLeft] =  normalize10(y1 + x2) * joyToWheel;
		  motor[motorFrontRight] = normalize10(y2 - x1) * joyToWheel;
		  motor[motorBackLeft] =  normalize10(y1 - x1) * joyToWheel;
		  motor[motorBackRight] = normalize10(y2 + x1) * joyToWheel;
		}
  }
}
Exemplo n.º 9
0
void getJoyValues(){
  getJoystickSettings(joystick);

  //DRIVING JOYSTICK VALUES
 drivejy1=joystick.joy1_y1;
 drivejy2=joystick.joy1_y2;
 driveButtons=joystick.joy1_Buttons;
 driveTopHat =joystick.joy1_TopHat;
 driveTopHat=-1;

 driveButton1=((driveButtons&1)>0);driveButton2=((driveButtons&2)>0);driveButton3=((driveButtons&4)>0);driveButton4=((driveButtons&8)>0);driveButton5=((driveButtons&16)>0);
 driveButton6=((driveButtons&32)>0);driveButton7=((driveButtons&64)>0);driveButton8=((driveButtons&128)>0);driveButton9=((driveButtons&256)>0);driveButton10=((driveButtons&512)>0);

 //ARM JOYSTICK VALUES
 armTopHat =joystick.joy2_TopHat;
 armButtons=joystick.joy2_Buttons;
 armjy1= joystick.joy2_y1;
 armjy2= joystick.joy2_y2;

 armButton1=((armButtons&1)>0);armButton2=((armButtons&2)>0);armButton3=((armButtons&4)>0);armButton4=((armButtons&8)>0);armButton5=((armButtons&16)>0);
 armButton6=((armButtons&32)>0);armButton7=((armButtons&64)>0);armButton8=((armButtons&128)>0);armButton9=((armButtons&256)>0);armButton10=((armButtons&512)>0);
}
Exemplo n.º 10
0
//chuteServo task
task chuteServo() {
	while(true) {

	//Get joystick values
	getJoystickSettings(joystick);

	//if B is pressed, servo moves
	if(joy2Btn(03) == 1) {
		servo[chuterelease] = 130;
		wait1Msec(500); //wait time
		position = 1; //position is set to 1 once the servo is open
		//wait1Msec(2000);
	}
	if (joy2Btn(02) == 1) {
		servo[chuterelease] = 0;
		wait1Msec(500); //wait time
		position = 0; //position is set to 0 once the servo is closed
		//wait1Msec(2000);
		}
		wait10Msec(5);
	}
}
Exemplo n.º 11
0
task Drive()
{
  getJoystickSettings(joystick);
  int jLeft = -(int)joystick.joy1_y1;
  int jRight = (int)joystick.joy1_y2;
  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
  }
}
Exemplo n.º 12
0
task Arm_task()
{

	int armPower;
	int armPower_raw;
  int limit = 75;
  int cut = 4;
	float Gain2 = 0.07;
	float Gain3 = 0.1;

  while (true)
	{
		getJoystickSettings(joystick);

		armPower_raw = joystick.joy1_y2;

		if (abs(armPower_raw) > deadband)
		{
			armPower = (armPower_raw - sgn(armPower_raw * deadband));

			armPower = ((armPower * Gain2) * (abs(armPower) * Gain3));

			if (armPower > limit)
			{
				armPower = limit;
			}
			else if (armPower < -limit)
			{
				armPower = -limit;
			}
			motor[Arm] = armPower/cut;
		}
		else
		{
			armPower = 0;
			motor[Arm] = armPower;
		}
	}
}
Exemplo n.º 13
0
//
// main() is where program execution begins
//
task main()
{
  waitForStart();   // wait for start of tele-op phase

  while (true)	// infinite loop
  {
  	// Read the latest joystick data from the field/driverstation
  	getJoystickSettings(joystick);

  	// Read the left and right joystick y axis
  	int left = joystick.joy1_y1;
  	int right = joystick.joy1_y2;

  	// Apply deadband function
  	left = deadband(left, DRIVE_JOYSTICK_DB);
  	right = deadband(right, DRIVE_JOYSTICK_DB);

  	// Drive with the joysticks
  	motor[leftDrive] = left;
  	motor[rightDrive] = right;
  }
}
Exemplo n.º 14
0
task main()
{

  //waitForStart();   // wait for start of tele-op phase

  while (true)
  {
	  getJoystickSettings(joystick);

	  //------------MAIN ROBOT CONTROL------------//

	  //====LEFT JOYSTICK====//

	  //***FORWARD***//

		  motor[frontLeft] = 40; //counterclockwise
		  motor[frontRight] = 40; //clockwise
		  motor[rearLeft]  = 40; //counterclockwise
		  motor[rearRight]	= 40; //clockwise

  }
}
Exemplo n.º 15
0
task DriveTank()
{
	while(true)
	{
		getJoystickSettings(joystick);

		//@todo - make this work
		//ReverseDriveMotors(ReverseDrive());

		//Control for left side
		if(abs(joystick.joy1_y1)>ANALOG_DEAD_ZONE)
			DriveLeftMotors(joystick.joy1_y1 / SlowModeDivisor());
		else
			StopLeftDriveMotors();

			//Control for right side
		if(abs(joystick.joy1_y2)>ANALOG_DEAD_ZONE)
			DriveRightMotors(joystick.joy1_y2 / SlowModeDivisor());
		else
			StopRightDriveMotors();
	}
}
Exemplo n.º 16
0
task main()
{
    initializeRobot();   // Initialize The Robot's Servos and Motor's

    waitForStart();   // wait for start of tele-op phase

    StartTask(drive);  // give driver control of the wheels

    while (true)
    {
        getJoystickSettings(joystick);
        // Gets Joystick Settings

        //D-pad direction is up?
        if (joystick.joy2_TopHat == TOPHAT_UP)
        {
            servoDestination += SERVO_RATE;
        }

        // D-pad direction is down?
        if (joystick.joy2_TopHat == TOPHAT_DOWN)
        {
            servoDestination -= SERVO_RATE;
        }

        // Keep servo position values within the range [0, 255].
        servoDestination = (servoDestination > 255) ? 255 : (((servoDestination < 0) ? 0 : servoDestination));

        // Send destination to servo.
        servo[lockingServo] = servoDestination;

        // Don't increment variables too quickly.
        wait1Msec(OneFrameMS);



    }
}
Exemplo n.º 17
0
task flagControl(){

	//Boolean which determines whether
	bool spinning = false;

	//Integer variable used to set the Limit for the motor encoder.
	int EncoderLimit = 0;

	int turning = 1;

	//Sets the motor encoder to zero at the beginning of the task.
	nMotorEncoder[motorFlagSpinner] = 0;

	//Continuously obtain the joystick settings and set them equal to their respective motor's power.
	while(true){

		getJoystickSettings(joystick);

		//When the A button on the second controller is pressed,
		//spin the flagSpinner until the flag is raised to max height.
	if(joy2Btn(2) == true){
		motor[motorFlagSpinner] = 60;
	} // End of if(joy2Btn(2) == true)

	if(joy2Btn(4) == true){
		motor[motorFlagSpinner] = 100;
		} // End of if(joy2Btn(1) == true)

	else {
		motor[motorFlagSpinner] = 0;
	} // End of else

	// Add a wait of 7 Milliseconds to the code in order to reduce latency.
	wait1Msec(10);

	}//End of while loop while(true)

}//End of task flagControl()
Exemplo n.º 18
0
task main() {
	servoChangeRate[flagRaiserExtender] = 5;
	servoChangeRate[blockBlocker] = 8;

	servo[flagRaiserExtender] = 220;
	servo[wrist] = 90;
	servo[blockBlocker] = 30;

	servo[autoArm] = 145;
	servo[autoBlock] = 200;
	waitForStart();
	//servo[flagRaiserExtender] = 0;

	while (true) {
		bFloatDuringInactiveMotorPWM = false;
		getJoystickSettings(joystick);
		driver();
		arm();
		datalogging();
		// RobotC function for keeping the robot on
		//alive();
	}
}
Exemplo n.º 19
0
//////////////////////////////////////////TASK MAIN///////////////////////////////////////////////////////////////
task main()
{
	#ifndef DEBUG_ENABLED
	bDisplayDiagnostics = false;
	#endif
	//servo[servo1]=400;
	//servo[servo2]=400;
	nMotorEncoder[FourBar] = 0;
	while(true)  //infinite loop
	{
		getJoystickSettings(joystick);



		//vCupPosition();
		fourbarlift();
		drivespeed();
		tankdrive();
		wait1Msec(50);
		eraseDisplay();

	}
}
Exemplo n.º 20
0
task main()
{
	waitForStart();

	while (true)
	{
		getJoystickSettings(joystick);
		//driver controls
		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); //y coordinate for the right joystick on controller 1
		//arm/hand controls
		int cont2_right_yval = avoidWeird(joystick.joy2_y2, 20);
		int cont2_left_yval = avoidWeird(joystick.joy2_y1, 20);
		//int cont1_dPad = joystick.joy1_TopHat; //Value of the dPad for controller 2

		drive(cont1_left_yval, cont1_left_xval);
		shoulderMovement(cont2_right_yval);
		handMovement(cont2_left_yval);
		flag_raise(cont1_right_yval);

	}
}
task main()
{
	int a = ServoValue[goalGripper];
	servo[goalGripper] = a;
	while(true)
	{
		getJoystickSettings(joystick);
		if (joystick.joy1_TopHat == 0)
		{
			a += 1;
			servo[goalGripper] = a;
		}
		else if(joystick.joy1_TopHat == 4)
		{
			a -= 1;
			servo[goalGripper] = a;
		}
		eraseDisplay();
		nxtDisplayCenteredTextLine(3, "servo position:");
		nxtDisplayCenteredTextLine(4, "%d", ServoValue[goalGripper]);
		wait1Msec(100);
	}
}
Exemplo n.º 22
0
task main()
{
	initializeRobot();

	while(true)

	{
		getJoystickSettings(joystick);

		if(joy1_TopHat > 0)
		{
			for (int i = 1; i < 10; i++)
			{
				if (joy1Btn (i) == 1)

				{
					nxtDisplayCenteredTextLine( 1, "Button %d pushed.",i );

			  }
		  }
	  }
  }
}
Exemplo n.º 23
0
task main() {
	int ly,
		lx;
	bool driverCtl;
	bool btn5Last;

	SensorFullCount[gyro] = 21600;

	startTask(posTrack);
	startTask(ctls);

	initPid(&drivePid, .1, .001, .01);
	initPid(&gyroPid, .2, .002, .02);

	while(1) {
		getJoystickSettings(joystick);

		if(joy1Btn(5) && !btn5Last)
			driverCtl = !driverCtl;
		btn5Last = joy1Btn(5);

		ly = (fabs(joystick.joy1_y1) >= 15)
			? joystick.joy1_y1
			: 0;

		lx = (fabs(joystick.joy1_x1) >= 15)
			? joystick.joy1_x1
			: 0;

		if(driverCtl)
			arcCtl(ly, lx);
		else
			arcCtl(drivePid.out, gyroPid.out);

		wait1Msec(20);
	}
}
Exemplo n.º 24
0
task main()
{
  waitForStart();

  int isIn = -1;
  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 cont2_left_yval = avoidWeird(joystick.joy2_y1, 35); //y coordinate for the left joystick on controller 2
    int cont2_right_yval = avoidWeird(joystick.joy2_y2, 35); //y coordinate for the right joystick on controller 2
    int cont2_dPad = joystick.joy2_TopHat; //Value of the dPad for controller 2

    if (joy1Btn(4) == 1)
    {
      isIn = 0;
    }
    if (joy1Btn(2) == 1)
    {
      isIn = 1;
    }
    if (joy1Btn(6) == 1)
    {
      isIn = -1;
    }

    drive(cont1_left_yval, cont1_left_xval);
    shoulderMovement(cont2_left_yval);
    elbowMovement(cont2_right_yval);
    contServos(isIn);
    ballHopperArm(cont1_right_yval);
    handMovement(cont2_dPad);
  }
}
Exemplo n.º 25
0
task main(){
	while(true){
		ClearTimer(T1);
		hogCPU(); //Prevent other tasks from running when this one is.

  	getJoystickSettings(joystick);     // update buttons and joysticks
  	eraseDisplay();

//-------------------------- Start of the State Machine ----------------//
		switch (sm_cmd) {
		case LIGHT_OFF:                     //State
      // Do this when the light is off
		  nxtDisplayString(2, "Light Off");
			if (FallEdge(joy1Btn(1),btn_z1)){ //Transition
				// Do this on a transition
				sm_cmd=LIGHT_ON;
			}

			break;
		case LIGHT_ON:                      //State
		  nxtDisplayString(2, "Light On");
			if (FallEdge(joy1Btn(1),btn_z1)){ //Transition
				// Do this on a transition
				sm_cmd=LIGHT_OFF;
			}
	  }
// ------------------------- End of the State Machine ------------------//

	  count=count+1; // Count the number of times the foreground runs.
	  timeLeft=FOREGROUND_MS-time1[T1]; // Calculate the time used in the foreground
	  releaseCPU(); // Let other tasks run now.
	  wait1Msec(timeLeft);// The time other tasks have to run before foreground takes control.
	 	writeDebugStreamLine("Count=[%2i] State[%2i]",count,sm_cmd);

	}// While Loop
}// Foreground Task
task main()
{

  getJoystickSettings(joystick);
  bool wait;
  while (true)
  {
  wait = false;
  while (wait==false)
  {
    motor[leftMotor] = joystick.joy1_y1*-1;
  motor[rightMotor] = joystick.joy1_y2*-1;
    wait1Msec(1);
    if (joy1Btn(5))
    {
      wait = true;
    }
    else if (joy1Btn(6))
    {
      wait = true;
    }

  }
  bool direction;
  if (joy1Btn(5))
  {
    direction = true;
  }
  else if (joy1Btn(6))
  {
    direction = false;
  }
  joy1Btn(6);
  initServos(direction);
}
}
task main()
{
	short zero = 6;
	float liftmod = 75;
	initializeRobot();
	waitForStart();
	while(true){
		getJoystickSettings(joystick);

		if(joystick.joy1_y1 > zero){ //up
			if(liftCheck(liftmod)){
				motor[lift] = liftmod;
			}
		}else if(joystick.joy1_y1 < -zero){ //down
			if(liftCheck(liftmod)){
				motor[lift] = -liftmod;
			}
		}else{
			motor[lift] = 0;
		}
	}


}
Exemplo n.º 28
0
task main() {
  while (true) {
    getJoystickSettings(joystick);
	  int x1 = joystick.joy1_x1;
	  int y1 = joystick.joy1_y1;
	  int x2 = joystick.joy1_x2;
	  int y2 = joystick.joy1_y2;

	  if (abs(x1) > 15) {
        motor[Q1] = x1;
	    motor[Q2] = x1;
	    motor[Q3] = x1;
	    motor[Q4] = x1;
	  }

	  if (abs(x2) > 15) {
	    motor[Q1] = -x2;
	    motor[Q2] = x2;
	    motor[Q3] = -x2;
	    motor[Q4] = x2;
	   
	  }
	  else {
	    motor[Q1] = x2;
	    motor[Q2] = x2;
	    motor[Q3] = x2;
	    motor[Q4] = x2;
      }	   
      if (abs(y2) > 15) {
        motor[Q1] = y2;
        motor[Q2] = y2;
        motor[Q3] = -y2;
        motor[Q4] = -y2;
      } 
  }
}
Exemplo n.º 29
0
task main()
{
  if( !InitializeRobot() )
  {
  	// fix robot code here
	}

//  waitForStart();	// Wait for the beginning of autonomous phase

  // Place your robot specific autonomous code here

	while( true )
  {
		getJoystickSettings( joystick );

		HandleJoystick1();
  	HandleJoystick2();

  	if( KillRobot() )
  	{
  		break;
  	}
  }
}
Exemplo n.º 30
0
task main()
{

  int forwardLeft, forwardRight, reverse=1;

  initializeRobot();

  waitForStart();   // wait for start of tele-op phase- used for the Samantha Module

  while (true)
  {
	 getJoystickSettings(joystick);
	//joystick handling

	if(joy1Btn(5)==1){
	  //if button 5 is pressed, robot will only drive straight forward/back, ignoring any turns
	  forwardLeft=(reverse*joystick.joy1_y1)/2;
    forwardRight=forwardLeft;
  }else{
    //normal joystick motion
	  forwardLeft=(reverse*joystick.joy1_y1+joystick.joy1_x1)/2;
    forwardRight=(reverse*joystick.joy1_y1-joystick.joy1_x1)/2;
  }

    //cancels out joystick defects
    if(abs(forwardLeft)<10){
      forwardLeft=0;}
    if(abs(forwardRight)<10){
      forwardRight=0;}
    //Drive!
    motor[driveLFront]=forwardLeft;
    motor[driveLBack]=forwardLeft;
    motor[driveRFront]=forwardRight;
    motor[driveRBack]=forwardRight;
  }
}