예제 #1
0
task main() {
	int speed;
	int sonar_value;
	int distance = 25;
	while (true) {
		sonar_value = SensorValue(sonarSensor);
		displayLCDPos(0,0);
		displayNextLCDString("Sonar: ");
		displayNextLCDNumber(sonar_value);

		if (sonar_value < 0) {
			speed = 127;
			} else {
			speed = (sonar_value - distance)^2;
		}

		clearLCDLine(1);
		displayLCDPos(1,0);
		displayNextLCDString("Speed: ");
		displayNextLCDNumber(speed);

		motor[frontLeftMotor] = speed;
		motor[frontRightMotor] = speed;
		motor[backLeftMotor] = speed;
		motor[backRightMotor] = speed;

		wait1Msec(100);
	}
}
  void LCD(){
    {

    clearLCDLine(0);
    clearLCDLine(1);                                      // Clear LCD Bottom Line
    displayLCDPos(1,0);                                   // Set the cursor to bottom line, first position
    displayNextLCDString("Arm: ");                   // Print "Potentio: " starting at the cursor position
    displayNextLCDNumber(SensorValue(ncoderarm));    // Display the reading of the Potentiometer to current cursor position
    displayLCDPos(0,5);
    displayNextLCDString("Touch: ");
    displayNextLCDNumber(SensorValue(Touch));

    }
  }
예제 #3
0
task changeBallCount() {
	bool button8DPressed = false,
			 button8LPressed = false;
	while (1) {
		if (vexRT[Btn8D] && !button8DPressed) {
			ballsInIntake--;
			button8DPressed = true;
		} else if (vexRT[Btn8L] && !button8LPressed) {
			ballsInIntake++;
			button8LPressed = true;
		}

		if (!vexRT[Btn8D]) {
			button8DPressed = false;
		}
		if (!vexRT[Btn8L]) {
			button8LPressed = false;
		}

		if (ballsInIntake > 4) {
			ballsInIntake = 4;
		} else if (ballsInIntake < 0) {
			ballsInIntake = 0;
		}

		clearLCDLine(0);
		displayLCDPos(0,0);
		displayNextLCDNumber(ballsInIntake);

		wait1Msec(100);
	}
}
task main()
{
  int nBiasValues[3];
  int X_Accel;
  int Y_Accel;
  int Z_Accel;

  clearLCDLine(0);
  clearLCDLine(1);
  bLCDBacklight = true;
  displayLCDCenteredString(0, "Accel Test");
  wait1Msec(500);  // Bias values are being calculated
  PlaySound(soundBeepBeep);

  // Store the bias values in a variable so that they can be easily displayed in the ROBOTC debugger
  // global variables window

  nBiasValues[0] = SensorBias[xAxis];
  nBiasValues[1] = SensorBias[yAxis];
  nBiasValues[2] = SensorBias[zAxis];

  while (true)
  {
    // Use the VEX LCD to display the accelerometer values

    clearLCDLine(0);
    clearLCDLine(1);

    displayLCDPos(0, 0);
    displayNextLCDString(".X.. ..Y.. ..Z..");
    displayLCDPos(1, 0);
    displayNextLCDNumber(SensorValue[xAxis], 4);
    displayLCDPos(1, 5);
    displayNextLCDNumber(SensorValue[yAxis], 5);
    displayLCDPos(1, 12);
    displayNextLCDNumber(SensorValue[zAxis], 4);

    // Also store values in variables so that they can be easily displayed in "Globals" debugger window

    X_Accel = SensorValue[xAxis];
    Y_Accel = SensorValue[yAxis];
    Z_Accel = SensorValue[zAxis];

    wait1Msec(100);
  }
}
예제 #5
0
task main()
{
  do
  {
    derivitive;
    displayNextLCDNumber(DPS);
  }while(1<2)
}
예제 #6
0
static void displayStatusAndTime()
{
  displayLCDPos(1, 0);
	if (bIfiRobotDisabled)
	  displayNextLCDString("Disable ");
	else
	{
	  if (bIfiAutonomousMode)
	    displayNextLCDString("Auton  ");
	  else
	    displayNextLCDString("Driver ");
	}
	displayNextLCDNumber(nTimeXX / 600, 2);
	displayNextLCDChar(':');
	displayNextLCDNumber((nTimeXX / 10) % 60, -2);
	displayNextLCDChar('.');
	displayNextLCDNumber(nTimeXX % 10, 1);
}
예제 #7
0
void screenrefresh()
{
  clearLCDLine(1);
  displayLCDPos(1,0);
  if (redteam == true)
  {
    displayNextLCDString("RED ");
  }
  if (redteam == false)
  {
    displayNextLCDString("BlUE");
  }
  displayNextLCDString("    ");
  displayNextLCDNumber(programselect);
  displayNextLCDString("     OK");
}
task usercontrol()
{
	// User control code here, inside the loop

	int potThreshold = 1616;
	int flipperVal = 0;
	//clearDebugStream;
	//ClearTimer(T1);

	wait1Msec(2000);                // wait 2 seconds before exectuing following code
	bMotorReflected[port2] = true;

	while (true)
	{
		// This is the main execution loop for the user control program. Each time through the loop
		// your program should update motor + servo values based on feedback from the joysticks.

		// .....................................................................................
		// Insert user code here. This is where you use the joystick values to update your motors, etc.
		// .....................................................................................

		int ch2 = vexRT[Ch2];
		int ch3 = vexRT[Ch3];
		int secondch2 = vexRT[Ch2Xmtr2];
		int secondch3 = vexRT[Ch3Xmtr2];
		int pot = SensorValue[potentiometer];

		//motor[motor1] = motor[motor2] = motor[motor3] = motor[motor10] = ch2;
		// Flipper on Controller 2
		/*motor[flip1] = secondch2;
		motor[flip2] = secondch2;
		motor[flip3] = secondch2;
		motor[flip4] = secondch2;*/

		//Flipper button control
		if(vexRT[Btn6UXmtr2])
		{
			motor[servo] = -125;
			wait1Msec(500);
			motor[servo] = 95;
		}

		motor[flip1] = flipperVal;
		motor[flip2] = flipperVal;
		motor[flip3] = flipperVal;
		motor[flip4] = flipperVal;


		if(vexRT[Btn8DXmtr2] && pot < potThreshold) {
			flipperVal = -127;
			} else if (vexRT[Btn8UXmtr2]) {
			flipperVal = 127;
			}else if (pot > potThreshold) {
			flipperVal = -20;
			}  else {
			flipperVal = 0;
		}

		// doesn't work yet

		/*if(vexRT[Btn8DXmtr2] && pot < potThreshold) {
		flipperVal = -127;
		} else if (vexRT[Btn8DXmtr2] && pot > potThreshold) {
		flipperVal = -20;
		} else if (pot > potThreshold && !vexRT[Btn8DXmtr2]) {
		if (pot > 750) {
		flipperVal = 127;
		} else {
		flipperVal = 0;
		}}*/

		// Drive On Controller 1
		motor[backLeft] = ch3;
		motor[frontLeft] = ch3;
		motor[backRight] = ch2;
		motor[frontRight] = ch2;

		if(vexRT[Btn7UXmtr2])
		{
			motor[intake] = -127;
		}
		if(vexRT(Btn7DXmtr2))
		{
			motor[intake] = 0;
		}
		if(vexRT[Btn5U]) {
			motor[backLeft] = 127;
			motor[frontLeft] = -127;
			motor[backRight] = -127;
			motor[frontRight] = 127;
		}
		if(vexRT[Btn6U]) {
			motor[backLeft] = -127;
			motor[frontLeft] = 127;
			motor[backRight] = 127;
			motor[frontRight] = -127;
		}

		//int pot = SensorValue[potentiometer];
		//sfoo = time100[T1]/10;
		writeDebugStreamLine("The potentiometer is reading %d", SensorValue[potentiometer]);
		//int shoot vexRT[Btn8D];

		clearLCDLine(0);                                  // clear the top VEX LCD line
		clearLCDLine(1);                                  // clear the bottom VEX LCD line

		setLCDPosition(0,0);                              // set the VEX LCD cursor the first line, first space
		displayNextLCDString("Potentiometer:");           // display "Potentiometer:" on the top line

		setLCDPosition(1,0);                              // set the VEX LCD cursor the second line, first space
		displayNextLCDNumber(SensorValue(potentiometer));







	}
}
예제 #9
0
task countBallsInIntake() {
	int numConsecZeros = 0, //number of consecutive 0s return by the limit switch
			numConsecOnes = 0; //number of consecutive 1s return by the limit switch
	while(1) {

		if (ballsInIntake < 3 && rollerState == 1) { //if there are 2 or fewer balls in the intake, and intaking balls in
			while(!SensorValue[intakeLimit]) { //limit switch indicates that a ball is not at the bottom of the dangle
				wait1Msec(25); //wait until a ball approaches the dangle
			}
			while(numConsecZeros < 2) { //condition: SensorValue[intakeLimit]; wait until the ball has triggered the limit switch
				if (!SensorValue[intakeLimit]) {
					numConsecZeros++;
				} else {
					numConsecZeros = 0;
				}
				wait1Msec(25);
			}
		} else if (ballsInIntake <= 3 && rollerState == -1) { //if there are 3 or fewer balls in the intake and we are outtaking via the roller
			while(!SensorValue[intakeLimit]) { //limit switch indicates that a ball is not at the bottom of the dangle
				wait1Msec(25); //wait until a ball approaches the dangle
			}
			while(numConsecZeros < 2) { //condition: SensorValue[intakeLimit]; wait until the ball has triggered the limit switch
				if (!SensorValue[intakeLimit]) {
					numConsecZeros++;
				} else {
					numConsecZeros = 0;
				}
				wait1Msec(25);
			}
		} else if (ballsInIntake >= 3 && rollerState == 1) { //limit switch is pressed when there are 4 balls in the intake - do this when the third ball is in the intake, so ballsInIntake = 3
			while(numConsecOnes < 2) { //condition: !SensorValue[intakeLimit]; only do this when going forward - this means we are intaking
				if (SensorValue[intakeLimit]) {
					numConsecOnes++;
				} else {
					numConsecOnes = 0;
				}
				wait1Msec(25);
			}
		} else if (ballsInIntake > 3 && rollerState == -1) { //limit switch is pressed when there are 4 balls in the intake - do this when the fourth ball is in the intake, so ballsInIntake = 3
				while(numConsecZeros < 2) { //condition: SensorValue[intakeLimit]; only do this when going backward - this means we are releasing balls from the lower end of the intake; the 4th ball keeps the limt switch pressed
 					if (!SensorValue[intakeLimit]) {
						numConsecZeros++;
					} else {
						numConsecZeros = 0;
					}
					wait1Msec(25);
 				}
		}

		numConsecZeros = 0; //reset to zero once limit we move on
		numConsecOnes = 0;

		//NOTE: this doesn't account for balls leaving the intake via the flywheel
		//reach this point once the intake limit switch has been pressed and then released (so balls are counted after they are done passing the dangle)

		if (rollerState == 1) { //if the roller is moving forward
			ballsInIntake++; //increment the number of balls in the intake
			} else if (rollerState == -1) { //if the roller is moving backwards (indicated by a rollerState value of -1)
			ballsInIntake--; //decrement the number of balls in the intake
		}

		//limit the ballsinIntake variable to 0-4 (inclusive) only
		if (ballsInIntake > 4) {
			ballsInIntake = 4;
		} else if (ballsInIntake < 0) {
			ballsInIntake = 0;
		}

		/*
		if (ballsInIntake < 4) {
			overrideAutoOuttakeOnly = false;
		}

		if (flywheelMode == 3 || flywheelMode == 4) {
				outtakeOnly = false;
		} else {
			if (ballsInIntake == 4) {
				if (vexRT[Btn6U] && !overrideAutoOuttakeOnly) {
					outtakeOnly = true;
				} else {
					outtakeOnly = false;
					overrideAutoOuttakeOnly = true;
				}
			} else if (ballsInIntake < 4 && outtakeOnly) { //check if we can disable outttake only mode
				outtakeOnly = false;
			}
		}*/

		clearLCDLine(0);
		displayLCDPos(0,0);
		displayNextLCDNumber(ballsInIntake);

	}

}
task usercontrol()
{
	while (true)
	{
		motor[rightfrontdrive]=vexRT[Ch2];
		motor[rightmiddledrive]=vexRT[Ch2];
		motor[rightreardrive]=vexRT[Ch2];
		motor[leftfrontdrive]=vexRT[Ch3];
		motor[leftmiddledrive]=vexRT[Ch3];
		motor[leftreardrive]=vexRT[Ch3];
		if (1 == 1)
		{
		clearLCDLine(0);											// Clear line 1 (0) of the LCD
		clearLCDLine(1);											// Clear line 2 (1) of the LCD

		//Display the Primary Robot battery voltage
		displayLCDString(0, 0, "Primary: ");
		sprintf(mainBattery, "%1.2f%c", nImmediateBatteryLevel/1000.0,'V'); //Build the value to be displayed
		displayNextLCDString(mainBattery);
    displayLCDPos(1,0);
    displayNextLCDNumber(SensorValue(pot));

		//Short delay for the LCD refresh rate
		wait1Msec(100);
		}
		if(vexRT[Btn5U]==1)
		{
			if(SensorValue(pot) < 1700){
				motor[leftlift] = 127;
				motor[rightlift] = 127;
			}
			else if(SensorValue(pot) > 1700){
				motor[leftlift] = 70;
				motor[rightlift] = 70;
			}
		}
		else if(vexRT[Btn5D]==1)
		{
			if(SensorValue(pot) < 1700){
				motor[leftlift] = -127;
				motor[rightlift] = -127;
			}
			else if(SensorValue(pot) > 1700){
				motor[leftlift] = -70;
				motor[rightlift] = -70;
			}
		}
		else
		{
			motor[leftlift] = 10;
			motor[rightlift] = 10;
		}
		if (vexRT[Btn6U]==1)
		{
			motor[leftintake]=-127;
			motor[rightintake]=-127;
		}
		else if (vexRT[Btn6D]==1)
		{
			motor[leftintake]=127;
			motor[rightintake]=127;
		}
		else
		{
			motor[leftintake]=0;
			motor[rightintake]=0;
		}
		if (vexRT[Btn7U]==1)
		{
			SensorValue[cat1]=0;
			SensorValue[cat2]=0;
		}
		else if (vexRT[Btn7D]==1)
		{
			SensorValue[cat2]=1;
			SensorValue[cat1]=1;
		}
		if (vexRT[Btn8U]==1)
		{
			SensorValue[hang2]=1;
			SensorValue[hang1]=1;
		}
		else if (vexRT[Btn8D]==1)
		{
			SensorValue[hang1]=0;
			SensorValue[hang2]=0;
		}
		while(vexRT[Btn7L]==1)
		{
			if(SensorValue(pot) < 1450){
				motor[leftlift] = 127;
				motor[rightlift] = 127;
			}
			else if(SensorValue(pot) > 1500){
				motor[leftlift] = -127;
				motor[rightlift] = -127;
			}
			else
			{
				motor[leftlift] = 10;
				motor[rightlift] = 10;
			}
		}
		if(vexRT[Btn7R]==1)
		{
			if(SensorValue(pot) < 1900){
				motor[leftlift] = 127;
				motor[rightlift] = 127;
			}
			else if(SensorValue(pot) > 2025){
				motor[leftlift] = -127;
				motor[rightlift] = -127;
			}
			else
			{
				motor[leftlift] = 10;
				motor[rightlift] = 10;
			}
		}
		if(vexRT[Btn8L]==1)
		{
			if(SensorValue(pot) < 700){
				motor[leftlift] = 50;
				motor[rightlift] = 50;
			}
			else if(SensorValue(pot) > 710){
				motor[leftlift] = -50;
				motor[rightlift] = -50;
			}
			else if(SensorValue(pot) < 703){
				motor[leftlift] = 35;
				motor[rightlift] = 35;
			}
			else if(SensorValue(pot) > 707){
				motor[leftlift] = -35;
				motor[rightlift] = -35;
			}
			else
			{
				motor[leftlift] = 10;
				motor[rightlift] = 10;
			}
		}
	}
}
예제 #11
0
task main()
{
  int i;
  const int kNumbOfMainSamples = 4000;

  bLCDBacklight = true;
  displayLCDPos(0, 0);
  displayNextLCDString("Gyro Noise Test ");
  memset(nSampleHisogram[0], 0, sizeof(nSampleHisogram));
  wait1Msec(1000);

  // Calculate a rough mezsure of 'bias' sum that fits in 16-bit signed.
  nSampleSum = 0;

  const int kRoughBiasCounts = 30;
  for (i = 0; i < kRoughBiasCounts; ++i)
  {
    nSampleSum += SensorValue[gyro];
    wait1Msec(1);
  }
  nBiasRough = nSampleSum / kRoughBiasCounts;

  nSampleSum = 0;
  for (i = 0; i < kNumbOfMainSamples; ++i)
  {
    nOffset = SensorValue[gyro] - nBiasRough;
    nSampleSum += nOffset;
    wait1Msec(1);
  }
  nBias = nBiasRough + nSampleSum / kNumbOfMainSamples;
  nBiasSmall = nSampleSum / (kNumbOfMainSamples / 100)   - (nSampleSum / kNumbOfMainSamples) * 100;
  nBiasSmaller = nSampleSum / (kNumbOfMainSamples / 1000) - nBiasSmall * 10;

  StartTask(testMotorNoise);
  nLastCycles = -1;

  bool bDisplayIt;
  bool bOverflow = false;
  for (nCycles = 0; true; ++nCycles)
  {
    //ASSERT(nCycles == (nLastCycles + 1));
    nLastCycles = nCycles;
    nTimeStamp = nPgmTime;
    nOffset = SensorValue[gyro] - nBias;
    nDrift += nOffset;
    if (nCycles >= 0)
    {
	    if ((nCycles % 100) == 50)
	      nDrift -= nBiasSmall;
	    if ((nCycles % 1000) == 500)
	    {
	      nDrift -= nBiasSmaller;
	      bDisplayIt = true;
	    }
	    else
	      bDisplayIt = false;
    }
    else
    {
	    if ((nCycles % 1000) == -50)
	      nDrift -= nBiasSmall;
	    if ((nCycles % 1000) == -500)
	    {
	      nDrift -= nBiasSmaller;
	      bDisplayIt = true;
	    }
	    else
	      bDisplayIt = false;
    }

    if (nOffset < -32)
      nOffset = -32;
    else if (nOffset > 32)
      nOffset = 32;

    nCount = nSampleHisogram[nOffset + 32];
    if (nCount >= 32767)
      bOverflow = true;
    if (!bOverflow)
      ++nSampleHisogram[nOffset + 32];
    if (true)
    {
      nPlusMinusWeighted += nOffset;
	    if (nOffset < 0)
	      --nPlusMinus;
	    else if (nOffset > 0)
	      ++nPlusMinus;
	  }
    if (bDisplayIt)
    {
      ++nSeconds;
      displayLCDPos(1, 0);
		  displayNextLCDNumber(nSeconds, 3);
	    displayNextLCDString("   : ");
	    if (nDrift > 0)
	    {
		    displayNextLCDNumber(nDrift / 1000);
		    displayNextLCDChar('.');
	      displayNextLCDNumber(nDrift % 1000, -3);
		  }
		  else
	    {
		    displayNextLCDChar('-');
		    displayNextLCDNumber(- nDrift / 1000);
		    displayNextLCDChar('.');
	      displayNextLCDNumber(- nDrift % 1000, -3);
	    }
	    displayNextLCDChar(' ');
	    displayNextLCDChar(' ');
	  }

    while (nTimeStamp == nPgmTime)
    {}
  }
  StopTask(testMotorNoise);
}