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;
}
Example #2
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);
	}
}
Example #3
0
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;
}
Example #4
0
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);
		}
	}
}
Example #5
0
// 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);
	}
}
Example #6
0
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
Example #8
0
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;
		}
	}
}
Example #9
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 = 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;

	  }
  }
}
}
Example #10
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