Ejemplo n.º 1
0
void BackwardOnLine(int power, int distance, string stopEvent)
{
  if (abs(SensorValue[Gyroscope]) > 200)
    SensorValue[Gyroscope] = 0;
  SensorValue[ShaftLeft] = 0;
  ClearTimer(T3);
  int currentShaft = 0;
  int velocity = 0;
  while(abs(SensorValue[ShaftLeft]) < distance * 10)
  {
    float amountLeft = -fAmountRight() - 0.1 * fAmountLeft() - AmountTurn();
    float amountRight = fAmountLeft() - 0.1 * fAmountRight() - AmountTurn();

    amountLeft = amountLeft * power / 45;
    amountRight = amountRight * power / 45;

    motor[FrontLeft] = -power + amountLeft;
    motor[FrontRight] = power + amountRight;
    motor[BackLeft] = -power + amountLeft;
    motor[BackRight] = power + amountRight;

    if (time10(T4) > 10)
    {
	    velocity = abs(SensorValue[ShaftLeft] - currentShaft);
      currentShaft = SensorValue[ShaftLeft];
	    ClearTimer(T4);
    }
    if (velocity > 5) ClearTimer(T3);
    if (time10(T3) > 50) break;
    if (stopEventFunction(stopEvent)) break;
  }
}
Ejemplo n.º 2
0
void Backward(int power, int distance, string stopEvent)
{
  if (abs(SensorValue[Gyroscope]) > 200)
    SensorValue[Gyroscope] = 0;
  SensorValue[ShaftLeft] = 0;
  ClearTimer(T3);
  int currentShaft = 0;
  int velocity = 0;
  while(abs(SensorValue[ShaftLeft]) < distance * 10)
  {
    motor[FrontLeft] = -power - turnSens2 * AmountTurn();
    motor[FrontRight] = power - turnSens2 * AmountTurn();
    motor[BackLeft] = -power - turnSens2 * AmountTurn();
    motor[BackRight] = power - turnSens2 * AmountTurn();

    if (time10(T4) > 10)
    {
	    velocity = abs(SensorValue[ShaftLeft] - currentShaft);
      currentShaft = SensorValue[ShaftLeft];
	    ClearTimer(T4);
    }
    if (velocity > 5) ClearTimer(T3);
    if (time10(T3) > 50) break;
    if (stopEventFunction(stopEvent)) break;
  }
}
Ejemplo n.º 3
0
task main()
{
	nMotorEncoder[test] = 0;
	while(true)
	{
		//1230
		getJoystickSettings(joystick);

		nxtDisplayCenteredTextLine(2, "enc: %d",nMotorEncoder[test]);
		nxtDisplayCenteredTextLine(3, "AngVel: %f", LauncherAngularVelocity);
		nxtDisplayCenteredTextLine(1, "time: %f", time10[T2]);
		if(time1[T1]>200){
			UpdateEncoders();
		}

		if(joy1Btn(2)&& time10(T2)>100 && LauncherAngularVelocity > 1){
			motor[test] = 127;
			motor[test2] = -127;
			}else if(joy1Btn(1)&& time10(T2)>100 && LauncherAngularVelocity > 1){
			motor[test] = -127;
			motor[test2] = 127;
			}else{
			if(joystick.joy1_y2<20 && joystick.joy1_y2>-20){
				motor[test] = 0;
				ClearTimer(T2);
				}else{
				motor[test] = joystick.joy1_y2;
			}

			if(joystick.joy1_y2<20 && joystick.joy1_y2>-20){
				motor[test2] = 0;
				ClearTimer(T2);
				}else{
				motor[test2] = joystick.joy1_y2;
			}
		}

		if(joy1Btn(3)){
			motor[test] = 127;
			motor[test2] = -127;
			}else if(joy1Btn(4)){
			motor[test] = -127;
			motor[test2] = 127;
		}
	}
}
Ejemplo n.º 4
0
void turnPointLeft (float mRot, float mRotPerSec)
{
	nxtDisplayCenteredTextLine(5, "TPL(%.2f,%.2f)",
		mRot, mRotPerSec);

	if (moveModeType != MMAllMoveTypes
	      && moveModeType != MMTurnsOnly
	      && moveModeType != MMPointTurnsOnly) {
		return;
	}

	if (moveModeTiming == MMOneMoveAtATime) {
		waitForTouch();
	}


	if (stepThroughMode == stepThroughModeOn) {
		waitForTouch();
	}

	checkParameterRange(mRot, mRotPerSec);

	int leftWheelInitial = nMotorEncoder[leftWheelMotor];
	int rightWheelInitial = nMotorEncoder[rightWheelMotor];
	int leftWheelTarget = nMotorEncoder[leftWheelMotor] - 360 * mRot;
	int rightWheelTarget = nMotorEncoder[rightWheelMotor] + 360 * mRot;

	ClearTimer(T1);

	float motorPower = revolutionsPerSecondToMotorPower(mRotPerSec);
	motor[leftWheelMotor] = -1 * motorPower;
	motor[rightWheelMotor] = motorPower;

	while ((nMotorEncoder[leftWheelMotor] > leftWheelTarget)
		&& (nMotorEncoder[rightWheelMotor] < rightWheelTarget))
	{
		nxtDisplayCenteredTextLine(7, "%d", nMotorEncoder[leftWheelMotor]);
	}

	motor[leftWheelMotor] = 0;
	motor[rightWheelMotor] = 0;

	int leftWheelChange = nMotorEncoder[leftWheelMotor] - leftWheelInitial;
	int rightWheelChange = nMotorEncoder[rightWheelMotor] - rightWheelInitial;
	float revolutionsWheelsRotated =
	  ((float) ( abs(leftWheelChange) > abs(rightWheelChange) ?
	                leftWheelChange : rightWheelChange ))
	        / 360.0;

	nxtDisplayCenteredTextLine(2, "TPL(%.2f,%.2f)",
		mRot, mRotPerSec);
	nxtDisplayCenteredTextLine(3, "%.2frev %.2fsec",
	  (float) revolutionsWheelsRotated, (float) time10(T1) / 100);
	nxtDisplayCenteredTextLine(5, "");
	nxtDisplayCenteredTextLine(7, "");
}
Ejemplo n.º 5
0
void VariableMove(int leftpower, int rightpower, int duration)
{
  ClearTimer(T1);
  while(time10(T1) < duration)
  {
    motor[FrontLeft] = leftpower;
    motor[FrontRight] = -rightpower;
    motor[BackLeft] = leftpower;
    motor[BackRight] = -rightpower;

    if (stopEventFunction("none"))
      break;
  }
}
Ejemplo n.º 6
0
task main() {
  bool conveyoron=false;

  initializeRobot();

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

  StartTask(DriveUpdate,0);

  servo[SERVOGATE]  =  SERVOGATECLOSE;    B_gateOpen=false;

  ClearTimer(T1); //we use timer T1 for program time

  while(true){

	  getJoyValues();

	  if(B_gateOpen) servo[SERVOGATE]=SERVOGATEOPEN;

    //////////////////////////////////
    //arm control button processing
	  if(driveButton10) {
	    drivingreversed=!drivingreversed;
	    wait1Msec(200);
	  }
	  if(driveButton6){
	    if(servo[SERVOGATE]==SERVOGATECLOSE){
	      servo[SERVOGATE] = SERVOGATEOPEN;
	      B_gateOpen=true;
	      wait1Msec(200);
	     }
	  else {
	    servo[SERVOGATE] = SERVOGATECLOSE;
	    B_gateOpen=false;
	    wait1Msec(300);
	    }
	   }
	  if(driveButton7 || armButton7){
	    if(conveyoron){
	      motor[conveyor1]=0;
	      wait1Msec(300);
        conveyoron=false;
	     }
	  else {
	    motor(conveyor1) = 100;
	    wait1Msec(300);
	    conveyoron=true;
	    }
	  }
	  if(driveButton5 || armButton5){
	    motor[conveyor1]=-100;
	    wait1Msec(300);
	    motor[conveyor1]=0;
	    conveyoron=false;
	  }
	  /*if(armButton1){
	    servo[servoFlip]=SERVOARMDOWN;
	    wait1Msec(500);
	    driveNudge(-50, -50, 500);
	    servo[servoFlip]=SERVOARMUP;
	  }
	  if(armButton4){
	    servo[servoFlip]=SERVOARMUP;
	    wait1Msec(50);
	  }
    if(armButton2){
      servo[servoFlip]=SERVOARMDOWN;
      wait1Msec(50);
    }
    if(armTopHat>=0){
      if(armTopHat==0){
        servo[hopControl]=HOPCONTROLUP;
        wait1Msec(50);
      }
      if(armTopHat==4){
        servo[hopControl]=HOPCONTROLDOWN;
        wait1Msec(50);
      }
      if(armTopHat==2){
        for(int i=0; i<=2; i++){
          servo[hopControl]=HOPCONTROLP1;
          wait1Msec(300);
          servo[hopControl]=HOPCONTROLP2;
          wait1Msec(300);
        }
        servo[hopControl]=HOPCONTROLDOWN;
        wait1Msec(50);
      }
      if(armTopHat==6){
        servo[hopControl]=HOPCONTROLCLOSED;
        wait1Msec(50);
      }
    }*/


    //////////////////////////
    //DRIVING
    //////////////////////////

    //Nudging, at most 1 every 1/10 second
  	if(time10(T4)<0) ClearTimer(T4); //keep timer positive
	  if(time10(T4)>10){
		  if(driveTopHat>=0){
			if(driveTopHat==0) driveNudge( 50, 50,60); //forward
			if(driveTopHat==1) driveNudge(  0, 50,60);
			if(driveTopHat==2) driveNudge( -50,50,60); //right
			if(driveTopHat==3) driveNudge( -50, 0,60);
			if(driveTopHat==4) driveNudge(-50,-50,60); //back
			if(driveTopHat==5) driveNudge( 0, -50,60);
			if(driveTopHat==6) driveNudge( 50,-50,60); //left
			if(driveTopHat==7) driveNudge( 50,  0,60);
			ClearTimer(T4); //so delay at least 100 Msec before next nudge
		  }
	  }
    /*if(abs(armjy1)<18) armjy1=0;
      motor[flipper]=((100*armjy1)/700);*/
	  if(abs(drivejy1)<10) drivejy1=0;
      if(abs(drivejy2)<10) drivejy2=0;
	  //standard driving
    if(drivingreversed) standardDrive(-(100*drivejy2)/127,-(100*drivejy1)/127);
    else standardDrive((100*drivejy1)/127,(100*drivejy2)/127);

  }
////////// end of infinite loop

}
Ejemplo n.º 7
0
/**
* Arc turn has the center point to the left or right of the wheel in line
* with the axel.  The idea is that the Rotations Per Second difference is
* the ratio between the outside and inside circle circumference.
*
* The only reason we really ask for both wheel numbers is to facilitate
* the teaching of algebra concepts.  A more useful function would just
* use the robot dimensions and the number of degrees to turn.
*
* There isn't a left and right function because the only difference is which
* wheel has to travel the furthest distance.
*/
void turnArcBackwards (float mRotLeft, float mRotLeftPerSec, float mRotRight, float mRotRightPerSec) {

  nxtDisplayCenteredTextLine(5, "TAL(%.2f,%.2f)",mRotLeft, mRotLeftPerSec);
  nxtDisplayCenteredTextLine(6, "TAL(%.2f,%.2f)",mRotRight, mRotRightPerSec);
	 

	if (moveModeType != MMAllMoveTypes
	      && moveModeType != MMTurnsOnly
	      && moveModeType != MMArcTurnsOnly) {
		return;
	}

	if (moveModeTiming == MMOneMoveAtATime) {
		waitForTouch();
	}


	if (stepThroughMode == stepThroughModeOn) {
		waitForTouch();
	}

	checkParameterRange(mRotLeft, mRotLeftPerSec);
	checkParameterRange(mRotRight, mRotRightPerSec);

	int leftWheelInitial = nMotorEncoder[leftWheelMotor];
	int leftWheelTarget = nMotorEncoder[leftWheelMotor] - 360 * mRotLeft;
	int rightWheelInitial = nMotorEncoder[rightWheelMotor];
	int rightWheelTarget = nMotorEncoder[rightWheelMotor] - 360 * mRotRight;
	
	ClearTimer(T1);

	float leftMotorPower = revolutionsPerSecondToMotorPower(mRotLeftPerSec);
	float rightMotorPower = revolutionsPerSecondToMotorPower(mRotRightPerSec);
	
	motor[leftWheelMotor] = -1 * leftMotorPower;
	motor[rightWheelMotor] = -1 * rightMotorPower;

	while ( (nMotorEncoder[leftWheelMotor] > leftWheelTarget) &&
		(nMotorEncoder[rightWheelMotor] > rightWheelTarget) )
	{
		nxtDisplayCenteredTextLine(7, "%d", nMotorEncoder[leftWheelMotor]);
	}

	motor[leftWheelMotor] = 0;
	motor[rightWheelMotor] = 0;

	int leftWheelChange = nMotorEncoder[leftWheelMotor] - leftWheelInitial;
	float revolutionsWheelsRotatedLeft = ((float) leftWheelChange) / 360.0;
	int rightWheelChange = nMotorEncoder[rightWheelMotor] - rightWheelInitial;
	float revolutionsWheelsRotatedRight = ((float) rightWheelChange) / 360.0;
	
	nxtDisplayCenteredTextLine(2, "TAL(%.2f,%.2f)",
		mRotLeft, mRotLeftPerSec);
	nxtDisplayCenteredTextLine(3, "%.2frev %.2fsec",
	  (float) revolutionsWheelsRotatedLeft, (float) time10(T1) / 100);
	nxtDisplayCenteredTextLine(4, "TAL(%.2f,%.2f)",
		mRotRight, mRotRightPerSec);
	nxtDisplayCenteredTextLine(5, "%.2frev %.2fsec",
	  (float) revolutionsWheelsRotatedRight, (float) time10(T1) / 100);
	  nxtDisplayCenteredTextLine(6, "");
	nxtDisplayCenteredTextLine(7, "");

}