/*-----------------------------------------------------------------------------*/
task
iqMenuTask()
{
    static  int select = 0;
            bool update = true;

    // This should allow using the exit button but it doesn't seem to work
    bUserControlsLCDButtons = true;

    // Run menu forever
    while( true ) {
        if( update ) {
            // Update main menu, only three items
            iqDisplayLine( LCD_ROW1, select == 0, "Run" );
            iqDisplayLine( LCD_ROW2, select == 1, "Configure" );
            iqDisplayLine( LCD_ROW3, select == 2, "Reset to Default" );
        update = false;
        }

        // Check LCD buttons
        if( nLCDButtons != kButtonNone ) {
            if( nLCDButtons == kButtonDown  )
                if( select++ == 2 ) select = 2;

            if( nLCDButtons == kButtonUp  )
                if( select-- == 0 ) select = 0;

            if( nLCDButtons == kButtonSelect  ) {
                // reset
                if( select == 2 ) iqResetToDefaults();

                // Enter configure menu
                if( select == 1 ) iqMenuConfiguration();

                // Enter run mode
                if( select == 0 ) {
                    // Set everything as enabled
                    robotRunning = true;
                    robotEnabled = true;

                    // Start the control task
                    startTask( iqRunRobotTask, kDefaultTaskPriority + 1 );

                    // and display information on the LCD while running...
                    iqMenuRun();

                    // When we return to this point user has indicated program should stop

                    // force task to stop motors and quit
                    robotRunning = false;
                    wait1Msec(100);
                    // task should be done
                }
            }

        // Wait for LCD button release
        while( nLCDButtons != kButtonNone )
          wait1Msec(10);

        // update display
        update = true;
        }

    // Don't hog cpu
    wait1Msec(25);
    }
}
示例#2
0
int sucker(int speed, int duration) { //positive numbers for out
  setSuckSpeed(speed);
  wait1Msec(duration);
  setSuckSpeed(0);
  return 1;
}
示例#3
0
文件: E1.c 项目: jimmycode/INB860
void PIDDriver(short target){
  float e, e_dot, old_e = threshold - SensorValue[Light], E = 0;
  short u;

  time1[T1] = 0;
  time1[T2] = 0;
  time1[T4] = 0;
  float pre_compass = compass();
  bool greyPatch = false;
  while(true){
    if(time1[T1] < timeout){ // T1 for timeout
        e = threshold - SensorValue[Light];
        e_dot = e - old_e;
        E += e;
        u = (short)(KP * e + KD * e_dot + KI * E);
        old_e = e;
        motor[Left] = cruise_speed - u;
        motor[Right] = cruise_speed + u;

        if(SensorValue[Light] < target){ //target can be make more conservative than threshold
          time1[T1] = 0;
        }

        // When grey patch detected
        if(SensorValue[Light] > 38 && SensorValue[Light] < 43){
            if(time1[T4] > 300){
                greyPatch = true;
                stop();

                // beep three times
                PlayTone(1500, 40);
                wait1Msec(800);
                PlayTone(1500, 40);
                wait1Msec(800);
                PlayTone(1500, 40);
                wait1Msec(800);

                control(rotateSpeed, -rotateSpeed, 800); // 800 is for getting rid of the grey patch
                while(SensorValue[Light] > threshold){}
                stop();

                time1[T1] = 0;
                time1[T2] = 0;
                time1[T3] = 0;
                nMotorEncoder[Left]=0;
                nMotorEncoder[Right]=0;
                pre_compass = compass();
                continue;
            }
        }
        else{
            time1[T4] = 0;
        }

        if(time1[T2] > sampleFreq && SensorValue[Light] < target){ //the light sensor should be on the tape when it beeps
            float cur_compass = compass();
            if(time1[T3] > beepInterval && ((cur_compass - pre_compass) > beepThreshold || (pre_compass - cur_compass) > beepThreshold)){
                PlayTone(1175, 20);
                time1[T3] = 0; // T3 for beepInterval
            }

            pre_compass = cur_compass;
            time1[T2] = 0; // T2 for sampling frequency

            if(greyPatch){ // log the compass value from grey patch to the other endpoint
              float tmp = signedCompass();
              nxtDisplayStringAt(0,31,"%f", tmp);
              logCompass(tmp);
            }
        }
    }
    else{
        if(greyPatch){
          if(!turnOnSpot(target, greyPatch)) return; // reach the endpoint other than grey patch, return
        }
        else{
          turnOnSpot(target, greyPatch);
        }
    }
  }
}
task main ()
{
  ubyte counter = 0;
  ubyte oldCounter = 0;
  ubyte refSignal = 0;
  ubyte oldRefSignal = 0;
  ubyte txType = 0;
  long rawValue = 0;

  nxtDisplayCenteredTextLine(0, "Mindsensors");
  nxtDisplayCenteredBigTextLine(1, "PSP-Nx");
  nxtDisplayCenteredTextLine(3, "Test 2");
  wait1Msec(2000);

  PlaySound(soundBeepBeep);
  while(bSoundActive) EndTimeSlice();

  eraseDisplay();
  while (true)
  {
    // Get the current counter value, wraps at 255
    counter = PSPV4readRefSignalCount(PSPNXV4);

    if (oldCounter != counter)
    {
      // This is the command sent by the remote control, can be:
      // PSPV4_SIGNAL_FAST_REWIND
      // PSPV4_SIGNAL_FAST_FORWARD
      // PSPV4_SIGNAL_PLAY
      // PSPV4_SIGNAL_STOP
      refSignal =  PSPV4readRefSignal(PSPNXV4);

      // Get the transmitted type.  Not very useful in most cases unless
      // you need to know what type of transmitter sent the command
			txType = PSPV4readRefTXType(PSPNXV4);

			// Raw value will also "see" commands from the remote
			// that are not recognised as "play", "stop", "forward" or "rewind".
			// You could use this to control additional aspects of your robot!
			rawValue = PSPV4readRawRefTXValue(PSPNXV4);

      if (oldRefSignal != refSignal)
      {
	      PlaySound(soundShortBlip);
		    switch(refSignal)
		    {
		      case PSPV4_SIGNAL_FAST_REWIND:   nxtDisplayCenteredBigTextLine(3,"REWIND"); break;
		      case PSPV4_SIGNAL_FAST_FORWARD:  nxtDisplayCenteredBigTextLine(3,"FORWARD"); break;
		      case PSPV4_SIGNAL_PLAY:          nxtDisplayCenteredBigTextLine(3,"PLAY"); break;
		      case PSPV4_SIGNAL_STOP:          nxtDisplayCenteredBigTextLine(3,"STOP"); break;
		    }
		  }

		  // Always display the raw value, even if it's not one of the four
		  // referee signals.  Handy if you want to add more commands to your robot!
	    nxtDisplayTextLine(6, "Type:  0x%02X", txType);
	    nxtDisplayTextLine(7, "Raw:  0x%03X", rawValue);

	    // Update the counters and signals
	    oldCounter = counter;
	    oldRefSignal = refSignal;
	  }
    wait1Msec(50);
  }
}
void Ultra_Seek( int Min, int Max )
{
	int state = 2;
  int time;
  int time_old = 0;
  int delta_time;
  float maxVelocity = 180.0; // max angular velocity in deg/sec
  float GyroOld;
  float GyroNew;
  int rotDirection = 1;
  int motorSpeed = 0;
  float DegreeGain = 5.0;
  float degreesToGo;
  int MAX_GYRO_SPEED  = 85; //maximum motor speed allowed in turns
  int MIN_GYRO_SPEED  = 40; //minimum motor speed allowed in turns
  float Angle;

  Angle = 0.0;
	degreesToGo = 0.0;
	time_old = nPgmTime;
	GyroOld = 0.0;
	wait1Msec(100);

	while(SensorValue[SONAR_1] > Min)
		{
			while (SensorValue[SONAR_1] > Min && SensorValue[SONAR_1] < Max)
			{
				motor[rightDrive] = 20;
				motor[leftDrive] = 20;
			}
				motor[rightDrive] = 0;
				motor[leftDrive] = 0;
				wait10Msec(55);

				time100[T1] = 0;

				time100[T2] = 0;

			while(SensorValue[SONAR_1] > Max)
			{

				if(time100[T1] < 30)
				{
				 motor[rightDrive] = 20;
				 motor[leftDrive] = -20;
		  	}


		  	if(time100[T2] > 30)
				{
				 motor[rightDrive] = -20;
				 motor[leftDrive] = 20;
		  	}

			}
			ClearTimer(T3);

			time =  nPgmTime; // this timer wraps around
    delta_time = abs(time - time_old); // delta time in ms

    if (delta_time < 1) // protect against divide by zero
    {
      delta_time = 1;
    }
    // read the gyro sensor minus the bias offset. GyroBias must be declared and
    // computed in the calling program.
    GyroNew = -((float)SensorValue[Gyro] - GyroBias);

    // limit the gyro to the max achievable by the bot to minimize data spikes.
    if (abs(GyroNew) > maxVelocity) GyroNew = sgn(GyroNew)*maxVelocity;

    // deadband for the gyro to eliminate drift due to noise
    if (abs(GyroNew) <= 0.2) GyroNew = 0.0;

    // compute the integral of the angular rate using a trapazoidal approximation
    // http://en.wikipedia.org/wiki/Numerical_integration
    Angle = Angle + 0.001 * (float)delta_time * 0.5 *(GyroNew + GyroOld);

    // update the old values for the next time through the loop
    time_old = time;
    GyroOld = GyroNew;
		}
} // End of Move()
void deploy()
{
	intake(true);
	wait1Msec(300);
	stopIntake();
}
示例#7
0
/**Time-Based Intake**/
void intake(int power, int time){
	motor[intakeMotor] = power;
	wait1Msec(time);		//Time intake runs
	motor[intakeMotor] = 0;
}
示例#8
0
////// PID drive //////
void Drive(int DistanceIN) {
	long error = 99999;
	float KP = 0.35;
	float KD = 2.1;
	float KI = 1;
	long Velocity = 0;
	long Integral = 0;
	long CurrentDistance;
	long PreviousDistance = 0;
	SensorValue(LeftEncoder) = 0;
	SensorValue(RightEncoder) = 0;
	SensorValue(TurnGyro) = 0;
	int Distance;
	int SpeedCorrect = 0;
	int MotorSpeed = 0;
	int cycle = 0;

	Distance = DistanceIN / 0.024;

	while (abs(error) > 5) {
		CurrentDistance = ((SensorValue(RightEncoder)-SensorValue(LeftEncoder))/2);

		if (Velocity < 10){
			if(error > 0){
				Integral++;
			}
			else {
				Integral = 0;
			}
		}
		else {
			Integral = 0;
		}


		// P = calculate the error
		error = Distance - CurrentDistance;

		// D = Calculate the speed
		Velocity = CurrentDistance - PreviousDistance;

		if (SensorValue(TurnGyro) < -2){
			// Edit how much is added to change how fast it reacts
			SpeedCorrect = SpeedCorrect - 0.7;
		}
		else if (SensorValue(TurnGyro) > 2){
			SpeedCorrect = SpeedCorrect + 0.7;
		}

		// Calculates how fast to move the robot
		MotorSpeed = (error * KP) + (Integral * KI) - (Velocity * KD);

		BaseSpeed(MotorSpeed - SpeedCorrect, MotorSpeed + SpeedCorrect);

		writeDebugStreamLine("Cycle: %d", cycle);

		// Save the distance for the next cycle
		PreviousDistance = CurrentDistance;

		wait1Msec(25);
	}
	BaseSpeed(0,0);
}
task main()
{
	int elevatorHeightTicks;

	// Set the following variables to false to hide the standard NXT LCD information
	bDisplayDiagnostics = false;
	bNxtLCDStatusDisplay = false;

	////////// INITIALIZATIONS //////////
	initializeRobot();	// Execute robot initialization routine

	///////////////  Variables to be used later  /////////////////
	//int maxArmHeightTicks = inchesToTicks(maxArmHeight);
	//int minRingPickupHeightTicks = inchesToTicks(minRingPickupHeight);
	//int WAMservoStep = 3;	//Amount to inc1rement the WAM servo position
	//int WAMservoStep12 = 17;	// Move a servo 15 degrees
	//float maxArmHeight = 45.25;	// Maximum Safe Arm Height used during manual control of the arm
	//float minRingPickupHeight = 8.0;

	// User selected Autonomous information
	{
		selectStartLocation();	// Get the starting location of the robot
		selectAutoActions(); 		// Get Autononmous actions
		switch(AutoActions)
		{
		case 0:	// No Autonomous
			break;
		case 1:	// IR Beacon
			selectRow();  // runs the select row function
			selectAutoStartDelay();	// select the autonomous start delay
			break;
		case 2:	// Left Colmnn
			selectRow();        // runs the select row function
			selectAutoStartDelay();	// select the autonomous start delay
			break;
		case 3:	// Center Colmnn
			selectRow();        // runs the select row function
			selectAutoStartDelay();	// select the autonomous start delay
			break;
		case 4:	// Right Colmnn
			selectRow();        // runs the select row function
			selectAutoStartDelay();	// select the autonomous start delay
			break;
		case 5:	// Play Defense
			selectAutoStartDelay();	// select the autonomous start delay
			break;
		default:	// Not a valid selection
			eraseDisplay();
			nxtDisplayCenteredTextLine(1,"DITU SAYS");
			nxtDisplayCenteredTextLine(2,"INPUT ERROR");
			nxtDisplayCenteredTextLine(4,"TRY AGAIN");
			wait1Msec(6000);
			break;
		}
	}
	eraseDisplay();
	// Set the following variables to false to hide the standard NXT LCD information
	// Show the default FTC Display Information
	bDisplayDiagnostics = true;
	bNxtLCDStatusDisplay = true;
	//bDisplayDiagnostics = false;
	//bNxtLCDStatusDisplay = false;

	// Determine the autonomous start delay based on delayAutoStart
	switch(delayAutoStart)
	{
	case 0:	// delay start = 0 seconds
		delayAutoStartValue = 0;
		break;
	case 1:	// delay start = 1 second
		delayAutoStartValue = 1000;
		break;
	case 2:	// delay start = 5 seconds
		delayAutoStartValue = 5000;
		break;
	case 3:	// delay start = 10 seconds
		delayAutoStartValue = 10000;
		break;
	case 5:	// delay start = 15 seconds
		delayAutoStartValue = 15000;
		break;
	default:	// delay start = 0 seconds
		delayAutoStartValue = 0;
		break;
	}

	// Process robot starting location selection
	switch(robotStartLocation)
	{
	case 0:	// Left Wall
		leftWall = true;
		break;
	case 1:	// Corner
		corner = true;
		break;
	case 2:	// Right Wall
		rightWall = true;
		break;
	default:	// Not a valid starting location
		break;
	}

	// Process robot starting location selection
	switch(Row)
	{
	case 0:	// Bottom Row
		pegHeightCmd = 1;
		break;
	case 1:	// Middle Row
		pegHeightCmd = 2;
		break;
	case 2:	// Top Row
		pegHeightCmd = 3;
		break;
	default:	// Not a valid starting location
		break;
	}

	waitForStart();	// Wait for the signal to start from the Field Control System

	//reads the protoboard to see which switches are pressed. Since they are globals, nothing is returned. We may want to make this it's own task eventually
	ProcessProto();

	switch(AutoActions)
	{
	case 0:	// No Autonomous
		break;
	case 1:	// Score Ring on the column designated by the IR Beacon
		wait1Msec(delayAutoStartValue);

		// INSERT CODE TO IDENTIFY THE COLUMN CONTAINING THE IR BEACON HERE

		elevatorHeightTicks = selectLocation(5);	// Determine the number of encoder ticks to raise the elevator to the drive height
		elevatorGoToHeight(elevatorHeightTicks);	// Raise elevator to the commanded height
		wait1Msec(500);

		// Insert robot move commands here

		// Now that the robot is in the scoring location, raise the elevator to the commanded row
		elevatorHeightTicks = selectLocation(pegHeightCmd);	// Determine the number of encoder ticks based on the commanded elevator height
		elevatorGoToHeight(elevatorHeightTicks);	// Raise elevator to the commanded height to score a ring
		wait1Msec(500);
		elevatorHeightTicks = selectLocation(4);	// Determine the number of encoder ticks to lower the elevator and score the ring
		elevatorGoToHeight(elevatorHeightTicks);	// Score Ring
		break;
	case 2:	// Score Ring on Left Column
		wait1Msec(delayAutoStartValue);
		elevatorHeightTicks = selectLocation(5);	// Determine the number of encoder ticks to raise the elevator to the drive height
		elevatorGoToHeight(elevatorHeightTicks);	// Raise elevator to the commanded height
		wait1Msec(500);

		// Insert robot move commands here

		// Now that the robot is in the scoring location, raise the elevator to the commanded row
		elevatorHeightTicks = selectLocation(pegHeightCmd);	// Determine the number of encoder ticks based on the commanded elevator height
		elevatorGoToHeight(elevatorHeightTicks);	// Raise elevator to the commanded height to score a ring
		wait1Msec(500);
		elevatorHeightTicks = selectLocation(4);	// Determine the number of encoder ticks to lower the elevator and score the ring
		elevatorGoToHeight(elevatorHeightTicks);	// Score Ring
		break;
	case 3:	// Score Ring on Center Column
		wait1Msec(delayAutoStartValue);
		elevatorHeightTicks = selectLocation(5);	// Determine the number of encoder ticks to raise the elevator to the drive height
		tripped = elevatorGoToHeightFailSafe(elevatorHeightTicks, 	10000);	// Raise elevator to the commanded height
		if (tripped) //If the elevator trips the fail safe, break
			break;

		wait1Msec(500);

		move_forward(60-5, 5000, 90, 90);

		/////The alignment from the wall that could work.../////
		//move_forward(24, 4000, 80, 80);
		//turngyro_left(45, 60, 60);
		//move_forward(4, 2000, 80, 80);

		RAM_down();

		// Drive forward until the RAM limit switch is activated or time expires
		ClearTimer(T1);
		while(ramLimit==0 && time1[T1] < 2500)
		{
			// Try to follow the white navigation tape
			if(SensorValue(colorSensorVal) >= 15)
			{
				// The color sensor sees the black platform, command the robot to drift to the right
				motor[Left1] = 35;
				motor[Left2] = 35;
				motor[Right1] = 15;
				motor[Right2] = 15;
			}
			else
			{	// The color sensor sees the white navigation tape, command the robot to drift to the left
				motor[Left1] = 15;
				motor[Left2] = 15;
				motor[Right1] = 35;
				motor[Right2] = 35;
			}
			ProcessProto();
		}
		ClearTimer(T1);
		while(time1[T1] < 1500)
		{
			motor[Left1] = -25;
			motor[Left2] = -25;
			motor[Right1] = 25;
			motor[Right2] = 25;
			motor[RAMright] = 95;
			motor[RAMleft] = 95;
		}

		ClearTimer(T1);
		while(time1[T1] < 1500)
		{
			motor[Left1] = 20;
			motor[Left2] = 20;
			motor[Right1] = -20;
			motor[Right2] = -20;
			motor[RAMright] = 95;
			motor[RAMleft] = 95;
		}

		ClearTimer(T1);
		while(time1[T1] < 1500)
		{
			motor[Left1] = 25;
			motor[Left2] = 25;
			motor[Right1] = 25;
			motor[Right2] = 25;
			motor[RAMright] = 95;
			motor[RAMleft] = 95;
		}

		/*ClearTimer(T1);
		while(time1[T1] < 200)
		{
			motor[Left1] = -20;
			motor[Left2] = -20;
			motor[Right1] = -20;
			motor[Right2] = -20;
			motor[RAMright] = 95;
			motor[RAMleft] = 95;
		}*/

		motor[RAMright] = 0;
		motor[RAMleft] = 0;
		motor[Left1] = 0;
		motor[Left2] = 0;
		motor[Right1] = 0;
		motor[Right2] = 0;

		// Now that the robot is in the scoring location, raise the elevator to the commanded row
		elevatorHeightTicks = selectLocation(pegHeightCmd);	// Determine the number of encoder ticks based on the commanded elevator height
		elevatorGoToHeightFailSafe(elevatorHeightTicks, 7000);	// Raise elevator to the commanded height to score a ring
		if (tripped) //If the elevator trips the fail safe, break
			break;
		wait1Msec(500);
		elevatorHeightTicks = selectLocation(4);	// Determine the number of encoder ticks to lower the elevator and score the ring
		elevatorGoToHeightFailSafe(elevatorHeightTicks, 7000);	// Score Ring
		if (tripped) //If the elevator trips the fail safe, break
			break;
		break;
	case 4:	// Score Ring on Right Column
		wait1Msec(delayAutoStartValue);
		elevatorHeightTicks = selectLocation(5);	// Determine the number of encoder ticks to raise the elevator to the drive height
		elevatorGoToHeight(elevatorHeightTicks);	// Raise elevator to the commanded height
		wait1Msec(500);

		// Insert robot move commands here
		//asdfjkl;

		wait1Msec(500);

		move_forward(12, 5000, 90, 90);
		turngyro_right(45, 100);
		move_forward(20, 7000, 80, 80);
		//RAM_down();

		motor[Right1] = 0;
		motor[Right2] = 0;
		motor[Left1] = 0;
		motor[Left2] = 0;

		/*
		// Drive forward until the RAM limit switch is activated or time expires
		ClearTimer(T1);
		while(ramLimit==0 && time1[T1] < 2500)
		{
			// Try to follow the white navigation tape
			if(SensorValue(colorSensorVal) >= 15)
			{
				// The color sensor sees the black platform, command the robot to drift to the right
				motor[Left1] = 35;
				motor[Left2] = 35;
				motor[Right1] = 15;
				motor[Right2] = 15;
			}
			else
			{	// The color sensor sees the white navigation tape, command the robot to drift to the left
				motor[Left1] = 15;
				motor[Left2] = 15;
				motor[Right1] = 35;
				motor[Right2] = 35;
			}
			ProcessProto();
		}
		ClearTimer(T1);
		while(time1[T1] < 1500)
		{
			motor[Left1] = -25;
			motor[Left2] = -25;
			motor[Right1] = 25;
			motor[Right2] = 25;
			motor[RAMright] = 95;
			motor[RAMleft] = 95;
		}

		ClearTimer(T1);
		while(time1[T1] < 1500)
		{
			motor[Left1] = 20;
			motor[Left2] = 20;
			motor[Right1] = -20;
			motor[Right2] = -20;
			motor[RAMright] = 95;
			motor[RAMleft] = 95;
		}

		ClearTimer(T1);
		while(time1[T1] < 1500)
		{
			motor[Left1] = 25;
			motor[Left2] = 25;
			motor[Right1] = 25;
			motor[Right2] = 25;
			motor[RAMright] = 95;
			motor[RAMleft] = 95;
		}

		/*ClearTimer(T1);
		while(time1[T1] < 200)
		{
			motor[Left1] = -20;
			motor[Left2] = -20;
			motor[Right1] = -20;
			motor[Right2] = -20;
			motor[RAMright] = 95;
			motor[RAMleft] = 95;
		}*/

		/*
		motor[RAMright] = 0;
		motor[RAMleft] = 0;
		motor[Left1] = 0;
		motor[Left2] = 0;
		motor[Right1] = 0;
		motor[Right2] = 0;

		// Now that the robot is in the scoring location, raise the elevator to the commanded row
		elevatorHeightTicks = selectLocation(pegHeightCmd);	// Determine the number of encoder ticks based on the commanded elevator height
		elevatorGoToHeightFailSafe(elevatorHeightTicks, 7000);	// Raise elevator to the commanded height to score a ring
		if (tripped) //If the elevator trips the fail safe, break
			break;
		wait1Msec(500);
		elevatorHeightTicks = selectLocation(4);	// Determine the number of encoder ticks to lower the elevator and score the ring
		elevatorGoToHeightFailSafe(elevatorHeightTicks, 7000);	// Score Ring
		if (tripped) //If the elevator trips the fail safe, break
			break;
		break;
		// Now that the robot is in the scoring location, raise the elevator to the commanded row
		/*elevatorHeightTicks = selectLocation(pegHeightCmd);	// Determine the number of encoder ticks based on the commanded elevator height
		elevatorGoToHeight(elevatorHeightTicks);	// Raise elevator to the commanded height to score a ring
		wait1Msec(500);
		elevatorHeightTicks = selectLocation(4);	// Determine the number of encoder ticks to lower the elevator and score the ring
		elevatorGoToHeight(elevatorHeightTicks);	// Score Ring
		break;*/
	case 5:	// Play Defense
		wait1Msec(delayAutoStartValue);
		elevatorHeightTicks = selectLocation(5);	// Determine the number of encoder ticks to raise the elevator to the drive height
		elevatorGoToHeight(elevatorHeightTicks);	// Raise elevator to the commanded height
		//wait1Msec(500);

		move_backwards(84.0, 10000, 100, 100);

		break;
	default:	// Not a valid autonomous action
		break;
	}

	servo[colorSensorServo] = colorServoDown;
	// All Done, time to stop all tasks
	StopAllTasks();
}	// End Main Bracket
示例#10
0
task main()
{
	motor[Motor] = 100;
	wait1Msec(1000);
	motor[Motor] = 0;
}
示例#11
0
void moveIntakeTop(int time, int power) {
	motor[intakeTop] = power;
	wait1Msec(time);
	motor[intakeTop] = 0;
}
示例#12
0
task main()
{

  initializeRobot();



	int countline = 0;
  waitForStart(); // Wait for the beginning of autonomous phase.
  motor[slide] = 20;
	wait1Msec(750);
	motor[slide] = 0;
	ClearTimer(T1);
	SensorValue[irSeek] = 0;
	motor[leftDrive]  = 70;
	motor[rightDrive] = 70;
	servo[leftgrab] = grabDownPosition;

  while(time1[T1] < 9000)//stays in loop while less than 9 seconds
	{
		if(atLine())
		{
			countline++; // if at line check and see if we are at the IR beacon if break out of loop and go to next loop

			if(SensorValue[irSeek] >= 5 && SensorValue[irSeek] <= 6)
			{
				// at Correct Line
				PlayTone(100,100);

				break;

			}
			else if(countline==1)
			{
				motor[rightDrive]= 5;//if it isnt at the beacon and its the first line turn to the right slightly
				motor[leftDrive] = 0;
				wait1Msec(500);
			}
			else if(countline == 2)/*may need to change*/{
				motor[rightDrive] = 4;//if it isn't at the beacon and is at line 2
				motor[leftDrive] = 0;
				wait1Msec(500);
			}
			motor[leftDrive]  = 75;// drive with 75% power
			motor[rightDrive] = 75;

			wait1Msec(500);

		}
	}
	if(time1[T1] < 9000)//while less than 9 seconds and boolean atgood is true
	{
		nMotorEncoder[leftDrive]  = 0;
		nMotorEncoder[rightDrive] = 0;
		if(countline == 1)
		{//if it is at the beacon and it is at the first line

			forward(6); //Distance after line till stop
			stopDrive();

			wait1Msec(500);

			servo[leftgrab] = grabReleasePositionpeg1;//put grabber at position
			wait1Msec(500);
			motor[slide] = -50;
			wait1Msec(1000);
			motor[slide] = 0;
			wait1Msec(1000);
	    servo[leftgrab] = grabReleasePositionpeg1 - 15;// move slightly to possible help
	    wait1Msec(1000);
	    motor[leftDrive] = -30;
	    motor[rightDrive] = -30;
	    wait1Msec(1000);
			motor[leftDrive] = 0;
			motor[rightDrive] = 0;
		}
		else if(countline == 2)//
		{//if at line 2 and at the beacon
			forward(7);//Distance after line till stop
			stopDrive();
			wait1Msec(500);

			servo[leftgrab] = grabReleasePositionpeg2;//move to position for the 2nd peg
			wait1Msec(500);
			motor[slide] = -50;//slide over to put the ring on
			wait1Msec(1000);
			motor[slide] = 0;
			wait1Msec(1000);
		    servo[leftgrab] = grabReleasePositionpeg2 + 10;//move slightly to help put the ring on
		    wait1Msec(1000);
		    motor[leftDrive] = -50;
		    motor[rightDrive] = -50;
		    wait1Msec(2000);
			motor[leftDrive] = 0;
			motor[rightDrive] = 0;


		}
		else if(countline ==3)
		{//if at line 3 and at beacon
			forward(6.2);//Distance after line till stop
			stopDrive();
			wait1Msec(500);

			servo[leftgrab] = grabReleasePositionpeg3;//put grabber to position for 3rd peg
			wait1Msec(500);
			motor[slide] = -50;//move to slide the ring on

			wait1Msec(1000);
			motor[slide] = 0;
			wait1Msec(1000);
			servo[leftgrab] = grabReleasePositionpeg3 + 25;



			//move slightly to help put ring on
			wait1Msec(1000);
			motor[leftDrive] = -50;
			motor[rightDrive] = -50;
			wait1Msec(6400);

			forward(7);
		}
	}
	else
		stopDrive();//if not below 9 seconds stop
	nMotorEncoder[leftDrive]  = 0;
	nMotorEncoder[rightDrive] = 0;//clear encoders
  while (true) {}
}
示例#13
0
void StopMotors()
{
	driveMotors(0,0);
	wait1Msec(20);
}
示例#14
0
文件: Teleop12002.c 项目: balty/ftc
//--------------------------------------------------------------------------------------------------------------
task main()
{																																// Start Main Loop
	initializeRobot(); 																						// Get ready! see void initalizerobot() above
	waitForStart();   																						// wait for start of tele-op phase From FCS
	// Start the loop ----------------------------------------------------------------------------------------------
	while(true)
	{																															// Start While loop
		// Right and Left Joysticks-------------------------------------------------------------------------------------
		getJoystickSettings(joystick);  															// Update Buttons and Joysticks
		motor[motorLEFT]  = scaleForMotor(joystick.joy2_y1);
		motor[motorRIGHT] =-scaleForMotor(joystick.joy2_y2);
		// Buttons------------------------------------------------------------------------------------------------------
		if(joy1Btn (1)== 1)  		    																// If Joy1-Button (1 blue X) is pressed:
		{																													//1open bracket
			ENCODER_1_TRGT = ARM_1_COUNT_1 - nMotorEncoder[motorARM1];
			ENCODER_2_TRGT = ARM_2_COUNT_1 - nMotorEncoder[motorARM2];
			ARM_2_STATE = 1;																			// Trigger for Stage 2
			//---------
			if (ENCODER_1_TRGT > DEAD_ARM_1)
			{																											//2open bracket
				PlaySound(soundShortBlip);
				wait1Msec(500);
				nMotorEncoderTarget[motorARM1] = ENCODER_1_TRGT;			// set the  target for Motor Encoder of Motor ARM

				motor[motorARM1] = POWER_1;               						// motor ARM is run at high power level
				while(nMotorRunState[motorARM1] != runStateIdle)  		// while Motor ARM hasn't reached target yet:
				{																										//3open - wait for motor to catch up
					servo[clawservo1] = SERVO_ANGLE_1;									// move servo to new position
					servo[clawservo2] = SERVO_ANGLE_1;									// move servo to new position
					motor[motorLEFT]  = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves
					motor[motorRIGHT] = -scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves
				}																										//3close
				ENCODER_1_TRGT = 0;
				motor[motorARM1] = 0;              									// stop motor and hold position
			}
			if (ENCODER_2_TRGT > DEAD_ARM_2)
			{
				nMotorEncoderTarget[motorARM2] = ENCODER_2_TRGT;			// set the  target for Motor Encoder of Motor ARM
				motor[motorARM2] = POWER_2;               						// motor ARM is run at high power level
				while(nMotorRunState[motorARM2] != runStateIdle)  		// while Motor ARM hasn't reached target yet:
				{																										// wait for motor to catch up
					motor[motorLEFT]  = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves
					motor[motorRIGHT] =-scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves
				}
				servo[clawservo1] = SERVO_ANGLE_1;									// move servo to new position
				servo[clawservo2] = SERVO_ANGLE_1;									// move servo to new position
				ENCODER_2_TRGT = 0;
			}																											// 2close
			//---------
			if (ENCODER_2_TRGT <  -DEAD_ARM_2)
			{
				nMotorEncoderTarget[motorARM2] = ENCODER_2_TRGT;			// set the  target for Motor Encoder of Motor ARM
				motor[motorARM2] = -POWER_2;               						// motor ARM is run at high power level
				while(nMotorRunState[motorARM2] != runStateIdle)  		// while Motor ARM hasn't reached target yet:
				{																										// wait for motor to catch up
					//						servo[clawservo1] = SERVO_ANGLE_1;									// move servo to new position
					//						servo[clawservo2] = SERVO_ANGLE_1;									// move servo to new position
					motor[motorLEFT]  = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves
					motor[motorRIGHT] =-scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves
				}
				servo[clawservo1] = SERVO_ANGLE_1;									// move servo to new position
				servo[clawservo2] = SERVO_ANGLE_1;									// move servo to new position
				ENCODER_2_TRGT = 0;
			}

			else if (ENCODER_1_TRGT <  -DEAD_ARM_1)
			{																											//4open
				nMotorEncoderTarget[motorARM1] = ENCODER_1_TRGT;			// set the  target for Motor Encoder of Motor ARM
				motor[motorARM1] = -POWER_1;               						// motor ARM is run at high power level
				while(nMotorRunState[motorARM1] != runStateIdle)  		// while Motor ARM hasn't reached target yet:
				{																										//5open - wait for motor to catch up
					servo[clawservo1] = SERVO_ANGLE_1;									// move servo to new position
					servo[clawservo2] = SERVO_ANGLE_1;									// move servo to new position
					motor[motorLEFT]  = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves
					motor[motorRIGHT] =-scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves
				}																										//5close
				motor[motorARM1] = 0;              									// stop motor and hold position
				ENCODER_1_TRGT = 0;
			}																											//4close
			//---------

			motor[motorARM1] = 0;              									// stop motor and hold position
			motor[motorARM2] = 0;              									// stop motor and hold position
		} 																														//1close
		// Button 2 Stage 1----------------------------------------------------------------------------------------------
		else if(joy1Btn (2)== 1)																				// If Joy1-Button (2 green B) is pressed:
		{																													//1open bracket
			ENCODER_1_TRGT = ARM_1_COUNT_2 - nMotorEncoder[motorARM1];
			ENCODER_2_TRGT = ARM_2_COUNT_2 - nMotorEncoder[motorARM2];
			//---------
			if (ENCODER_1_TRGT > DEAD_ARM_1)
			{while (ENCODER_2_TRGT > DEAD_ARM_2)
				{
					nMotorEncoderTarget[motorARM1] = ENCODER_1_TRGT;			// set the  target for Motor Encoder of Motor ARM
					motor[motorARM1] = POWER_1;               						// motor ARM is run at high power level
					while(nMotorRunState[motorARM1] != runStateIdle)  		// while Motor ARM hasn't reached target yet:
					{																										// wait for motor to catch up
						servo[clawservo1] = SERVO_ANGLE_2;									// move servo to new position
						servo[clawservo2] = SERVO_ANGLE_2;									// move servo to new position
						motor[motorLEFT]  = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves
						motor[motorRIGHT] =-scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves
					}

					nMotorEncoderTarget[motorARM2] = ENCODER_2_TRGT;			// set the  target for Motor Encoder of Motor ARM
					motor[motorARM2] = POWER_2;               						// motor ARM is run at high power level
					while(nMotorRunState[motorARM2] != runStateIdle)  		// while Motor ARM hasn't reached target yet:
					{																										// wait for motor to catch up
						servo[clawservo1] = SERVO_ANGLE_2;									// move servo to new position
						servo[clawservo2] = SERVO_ANGLE_2;									// move servo to new position
						motor[motorLEFT]  = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves
						motor[motorRIGHT] =-scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves
					}
					ENCODER_1_TRGT = 0;
					ENCODER_2_TRGT = 0;
					motor[motorARM1] = 0;              									// stop motor and hold position
			}}
			//---------
			else if (ENCODER_2_TRGT <  -DEAD_ARM_2)
			{
				while  (ENCODER_1_TRGT <  -DEAD_ARM_1)
				{
					nMotorEncoderTarget[motorARM2] = ENCODER_2_TRGT;			// set the  target for Motor Encoder of Motor ARM
					motor[motorARM2] = -POWER_2;               						// motor ARM is run at high power level
					while(nMotorRunState[motorARM2] != runStateIdle)  		// while Motor ARM hasn't reached target yet:
					{																										// wait for motor to catch up
						servo[clawservo1] = SERVO_ANGLE_2;									// move servo to new position
						servo[clawservo2] = SERVO_ANGLE_2;									// move servo to new position
						motor[motorLEFT]  = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves
						motor[motorRIGHT] =-scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves
					}
				}

				nMotorEncoderTarget[motorARM1] = ENCODER_1_TRGT;			// set the  target for Motor Encoder of Motor ARM
				motor[motorARM1] = -POWER_1;               						// motor ARM is run at high power level
				while(nMotorRunState[motorARM1] != runStateIdle)  		// while Motor ARM hasn't reached target yet:
				{																										//5open - wait for motor to catch up
					servo[clawservo1] = SERVO_ANGLE_2;									// move servo to new position
					servo[clawservo2] = SERVO_ANGLE_2;									// move servo to new position
					motor[motorLEFT]  = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves
					motor[motorRIGHT] =-scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves
			}}
			//---------
			ENCODER_1_TRGT = 0;
			ENCODER_2_TRGT = 0;

			motor[motorARM1] = 0;              									// stop motor and hold position
			motor[motorARM2] = 0;              									// stop motor and hold position
		}
		//--------------------------------------------------------------------------------------------------------------
		else if(joy1Btn (3)== 1)																	// If Joy1-Button (3 red B) is pressed:
		{																													//1open bracket
			ENCODER_1_TRGT=ARM_1_COUNT_3-nMotorEncoder[motorARM1];
			ARM_2_STATE = 3;																			// Trigger for Stage 2
			//---------
			if (ENCODER_1_TRGT > DEAD_ARM_1)
			{																											//2open bracket
				nMotorEncoderTarget[motorARM1] = ENCODER_1_TRGT;			// set the  target for Motor Encoder of Motor ARM
				motor[motorARM1] = POWER_1;               						// motor ARM is run at high power level
				while(nMotorRunState[motorARM1] != runStateIdle)  		// while Motor ARM hasn't reached target yet:
				{																										//3open - wait for motor to catch up
					servo[clawservo1] = SERVO_ANGLE_3;									// move servo to new position
					servo[clawservo2] = SERVO_ANGLE_3;									// move servo to new position
					motor[motorLEFT]  = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves
					motor[motorRIGHT] =-scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves
				}																										//3close
				ENCODER_1_TRGT = 0;
				motor[motorARM1] = 0;              									// stop motor and hold position
			}																											// 2close
			//---------
			else if (ENCODER_1_TRGT <  -DEAD_ARM_1)
			{																											//4open
				nMotorEncoderTarget[motorARM1] = ENCODER_1_TRGT;			// set the  target for Motor Encoder of Motor ARM
				motor[motorARM1] = -POWER_1;               						// motor ARM is run at high power level
				while(nMotorRunState[motorARM1] != runStateIdle)  		// while Motor ARM hasn't reached target yet:
				{																										//5open - wait for motor to catch up
					servo[clawservo1] = SERVO_ANGLE_3;									// move servo to new position
					servo[clawservo2] = SERVO_ANGLE_3;									// move servo to new position
					motor[motorLEFT]  = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves
					motor[motorRIGHT] =-scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves
				}																										//5close
				motor[motorARM1] = 0;              									// stop motor and hold position
				ENCODER_1_TRGT = 0;
			}																											//4close
			//---------
			else if	(-DEAD_ARM_1 < ENCODER_1_TRGT < DEAD_ARM_1)
			{
				motor[motorARM1] = 0;              									// stop motor and hold position
			}
			//---------
			if (ARM_2_STATE == 3)      															// If Joy1-Button (1 blue X) is pressed:
			{
				ENCODER_2_TRGT = ARM_2_COUNT_3 - nMotorEncoder[motorARM2];
				if (ENCODER_2_TRGT > DEAD_ARM_2)
				{
					nMotorEncoderTarget[motorARM2] = ENCODER_2_TRGT;			// set the  target for Motor Encoder of Motor ARM
					motor[motorARM2] = POWER_2;               						// motor ARM is run at high power level
					//---------
					while(nMotorRunState[motorARM2] != runStateIdle)  		// while Motor ARM hasn't reached target yet:
					{																										// wait for motor to catch up
						servo[clawservo1] = SERVO_ANGLE_3;									// move servo to new position
						servo[clawservo2] = SERVO_ANGLE_3;									// move servo to new position
						motor[motorLEFT]  = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves
						motor[motorRIGHT] =-scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves
					}
					ENCODER_2_TRGT = 0;
				}
				//---------
				else if (ENCODER_2_TRGT <  -DEAD_ARM_2)
				{
					nMotorEncoderTarget[motorARM2] = ENCODER_2_TRGT;			// set the  target for Motor Encoder of Motor ARM
					motor[motorARM2] = -POWER_2;               						// motor ARM is run at high power level
					while(nMotorRunState[motorARM2] != runStateIdle)  		// while Motor ARM hasn't reached target yet:
					{																										// wait for motor to catch up
						servo[clawservo1] = SERVO_ANGLE_3;									// move servo to new position
						servo[clawservo2] = SERVO_ANGLE_3;									// move servo to new position
						motor[motorLEFT]  = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves
						motor[motorRIGHT] =-scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves
					}
					ENCODER_2_TRGT = 0;
				}
				//---------
				motor[motorARM2] = 0;              									// stop motor and hold position
				ARM_2_STATE = 0;
			}
		} 																													//1close
		//--------------------------------------------------------------------------------------------------------------
		else if(joy1Btn (4)== 1)																		// If Joy1-Button (4 yellow Y) is pressed:
		{																													//1open bracket
			ENCODER_1_TRGT=ARM_1_COUNT_4-nMotorEncoder[motorARM1];
			ARM_2_STATE = 4;																			// Trigger for Stage 2
			//---------
			if (ENCODER_1_TRGT > DEAD_ARM_1)
			{																											//2open bracket
				nMotorEncoderTarget[motorARM1] = ENCODER_1_TRGT;			// set the  target for Motor Encoder of Motor ARM
				motor[motorARM1] = POWER_1;               						// motor ARM is run at high power level
				while(nMotorRunState[motorARM1] != runStateIdle)  		// while Motor ARM hasn't reached target yet:
				{																										//3open - wait for motor to catch up
					//						servo[clawservo1] = SERVO_ANGLE_1;									// move servo to new position
					//						servo[clawservo2] = SERVO_ANGLE_1;									// move servo to new position
					motor[motorLEFT]  = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves
					motor[motorRIGHT] =-scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves
				}																										//3close
				ENCODER_1_TRGT = 0;
				motor[motorARM1] = 0;              									// stop motor and hold position
			}																											// 2close
			//---------
			else if (ENCODER_1_TRGT <  -DEAD_ARM_1)
			{																											//4open
				nMotorEncoderTarget[motorARM1] = ENCODER_1_TRGT;			// set the  target for Motor Encoder of Motor ARM
				motor[motorARM1] = -POWER_1;               						// motor ARM is run at high power level
				while(nMotorRunState[motorARM1] != runStateIdle)  		// while Motor ARM hasn't reached target yet:
				{																										//5open - wait for motor to catch up
					servo[clawservo1] = SERVO_ANGLE_4;									// move servo to new position
					servo[clawservo2] = SERVO_ANGLE_4;									// move servo to new position
					motor[motorLEFT]  = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves
					motor[motorRIGHT] =-scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves
				}																										//5close
				motor[motorARM1] = 0;              									// stop motor and hold position
				ENCODER_1_TRGT = 0;
			}																											//4close
			//---------
			else if	(-DEAD_ARM_1 < ENCODER_1_TRGT < DEAD_ARM_1)
			{
				motor[motorARM1] = 0;              									// stop motor and hold position
			}
			//---------
			if (ARM_2_STATE == 4)      															// If Joy1-Button (1 blue X) is pressed:
			{
				ENCODER_2_TRGT = ARM_2_COUNT_4 - nMotorEncoder[motorARM2];
				if (ENCODER_2_TRGT > DEAD_ARM_2)
				{
					nMotorEncoderTarget[motorARM2] = ENCODER_2_TRGT;			// set the  target for Motor Encoder of Motor ARM
					motor[motorARM2] = POWER_2;               						// motor ARM is run at high power level
					//---------
					while(nMotorRunState[motorARM2] != runStateIdle)  		// while Motor ARM hasn't reached target yet:
					{																										// wait for motor to catch up
						servo[clawservo1] = SERVO_ANGLE_4;									// move servo to new position
						servo[clawservo2] = SERVO_ANGLE_4;									// move servo to new position
						motor[motorLEFT]  = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves
						motor[motorRIGHT] =-scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves
					}
					ENCODER_2_TRGT = 0;
				}
				//---------
				else if (ENCODER_2_TRGT <  -DEAD_ARM_2)
				{
					nMotorEncoderTarget[motorARM2] = ENCODER_2_TRGT;			// set the  target for Motor Encoder of Motor ARM
					motor[motorARM2] = -POWER_2;               						// motor ARM is run at high power level
					while(nMotorRunState[motorARM2] != runStateIdle)  		// while Motor ARM hasn't reached target yet:
					{																										// wait for motor to catch up
						servo[clawservo1] = SERVO_ANGLE_4;									// move servo to new position
						servo[clawservo2] = SERVO_ANGLE_4;									// move servo to new position
						motor[motorLEFT]  = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves
						motor[motorRIGHT] =-scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves
					}
					ENCODER_2_TRGT = 0;
				}
				//---------
				motor[motorARM2] = 0;              									// stop motor and hold position
				ARM_2_STATE = 0;
			}
		} 																													//1close
		//--------------------------------------------------------------------------------------------------------------
		if(joy1Btn (5)== 1)      														// If Joy1-Left Button is pressed:
		{
			motor[motorARM1] = POWER_1;         								// Motor is run at high power level
			wait1Msec(10);																			// Wait for motor to move
		}
		//--------------------------------------------------------------------------------------------------------------
		else if(joy1Btn (6)== 1)      												// If Joy1- Right Button is pressed:
		{
			POS_CLAW -= DELTA_POS;													// move servo -1 step
			if (POS_CLAW < MIN_POS_CLAW)										// if servo is at minimum...
			{
				POS_CLAW = MIN_POS_CLAW;										// reset minimum position
			}
			servo[servo1] = POS_CLAW;												// move servo to new position
			servo[servo2] = POS_CLAW;												// move servo to new position
			wait1Msec(10);																	// wait for servo to move
		}

		//--------------------------------------------------------------------------------------------------------------
		else if(joy1Btn (7)== 1)      												// If Joy1- Left Trigger is pressed:
		{
			motor[motorARM1] = -POWER_1;   									// Motor is run at -high power level
			wait1Msec(10);																	// Wait for motor to start
		}
		//--------------------------------------------------------------------------------------------------------------
		else if(joy1Btn (8)== 1)      												// If Joy1- Right Trigger is pressed:
		{
			POS_CLAW += DELTA_POS;													// move servo 1 step
			if (POS_CLAW > MAX_POS_CLAW)										// if servo is at maximum...
			{
				POS_CLAW = MAX_POS_CLAW;										// reset maximum position
			}
			servo[servo1] = POS_CLAW;												// move servo to new position
			servo[servo2] = POS_CLAW;												// move servo to new position
			wait1Msec(10);																	// wait for servo to move
		}
		//--------------------------------------------------------------------------------------------------------------
		else if(joystick.joy1_TopHat == 0)    								// If 0 button on joy1's D-Pad (up) is pressed:
		{
			motor[motorARM2] = POWER_2;     								// Motor 2 power level.
			wait1Msec(10);																	// Wait for motor 2 to start
		}
		//--------------------------------------------------------------------------------------------------------------
		else if(joystick.joy1_TopHat == 1)    								// If 1 button on joy1's D-Pad (up right) is pressed:
		{
			motor[motorARM2] = POWER_2;     								// Motor 2 power level.
			wait1Msec(10);																	// Wait for motor 2 to start
		}
		//--------------------------------------------------------------------------------------------------------------
		else if(joystick.joy1_TopHat == 2)    								// If 2 button on joy1's D-Pad (right) is pressed:
		{
			PlaySound(soundShortBlip);  											// play the sound, 'soundshortblip'
		}
		//--------------------------------------------------------------------------------------------------------------
		else if(joystick.joy1_TopHat == 3)    								// If 3 button on joy1's D-Pad (rt dn) is pressed:
		{
			motor[motorARM2] = -POWER_2;         						// Motor 2 power level
			wait1Msec(10);																	// Wait for motor to start
		}
		//-------------------------------------------------------------------------------------------------------------
		else if(joystick.joy1_TopHat == 4)    								// If 4 button on joy1's D-Pad (down) is pressed:
		{
			motor[motorARM2] = -POWER_2;    									// Motor 2 power level
			wait1Msec(10);																	// Wait for motor to start
		}
		//-------------------------------------------------------------------------------------------------------------
		else if(joystick.joy1_TopHat == 5)    								// If 5 button on joy1's D-Pad (lt dn) is pressed:
		{
			motor[motorARM2] = -POWER_2;         						// Motor 2 power level
			wait1Msec(10);																	// Wait for motor to start
		}
		//------------------------------------------------------------------------------------------------------------
		else if(joystick.joy1_TopHat == 6)    								// If 6 button on joy1's D-Pad (left) is pressed:
		{																													//1open bracket
			ENCODER_1_TRGT = ARM_1_COUNT_6	-	nMotorEncoder[motorARM1];
			ENCODER_2_TRGT = ARM_2_COUNT_6 	- nMotorEncoder[motorARM2];

			while (ENCODER_2_TRGT <  -DEAD_ARM_2)
			{
				nMotorEncoderTarget[motorARM2] = ENCODER_2_TRGT;			// set the  target for Motor Encoder of Motor ARM
				motor[motorARM2] = -POWER_2;               						// motor ARM is run at high power level
				while(nMotorRunState[motorARM2] != runStateIdle)  		// while Motor ARM hasn't reached target yet:
				{																										// wait for motor to catch up

					motor[motorLEFT]  = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves
					motor[motorRIGHT] =-scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves
				}
				ENCODER_2_TRGT = 0;
			}

			while (ENCODER_1_TRGT <  -DEAD_ARM_1)
			{																											//4open
				nMotorEncoderTarget[motorARM1] = ENCODER_1_TRGT;			// set the  target for Motor Encoder of Motor ARM
				motor[motorARM1] = -POWER_1;               						// motor ARM is run at high power level
				while(nMotorRunState[motorARM1] != runStateIdle)  		// while Motor ARM hasn't reached target yet:
				{																										//5open - wait for motor to catch up
					servo[clawservo1] = SERVO_ANGLE_6;									// move servo to new position
					servo[clawservo2] = SERVO_ANGLE_6;									// move servo to new position
					motor[motorLEFT]  = scaleForMotor(joystick.joy1_y1);// allow robot to drive while arm moves
					motor[motorRIGHT] =-scaleForMotor(joystick.joy1_y2);// allow robot to drive while arm moves
				}																										//5close
				ENCODER_1_TRGT = 0;
			}
			motor[motorARM1] = 0;              									// stop motor and hold position
			motor[motorARM2] = 0;              									// stop motor and hold position
		}
		//------------------------------------------------------------------------------------------------------------
		else if(joystick.joy1_TopHat == 7)    								// If 7 button on joy1's D-Pad (lt up) is pressed:
		{
			motor[motorARM2] = POWER_2;          					// Motor 2 power level
			wait1Msec(10);																	// Wait for motor to start
		}
		//------------------------------------------------------------------------------------------------------------
		else                            											// If Joy1-Buttons are NOT pressed:
		{
			motor[motorARM1] = 0;															// Turn Arm Motor 1 Off
			motor[motorARM2] = 0;															// Turn Arm Motor 2 Off
		}
		//------------------------------------------------------------------------------------------------------------
	}//2 End While Loop ----------------------------------------------------------------------------------------
}//1   End Task Main -----------------------------------------------------------------------------------------
示例#15
0
sub MTASK_DOTASK(int MTASK_ID){
  switch (MTASK_ID)
  {
    //********
    case MT_DEFAULT:
	    wait1Msec(1);
	    break;

    //********
    case MT_BEEP:
	    PlayTone(200, 12);
	    wait10Msec(120);
	    break;

    //********
	  case MT_STOP_BUTTON:
	    if(nNxtButtonPressed==BT_ENTER)
	    {
	      int static TimeDif;
	      TimeDif=time10[T4];
	      while(nNxtButtonPressed==BT_ENTER){
	        if(time10[T4]-TimeDif>50)
	        {
    	      MV_StopMotors();
    	      ClearSounds();
	          PlaySound(soundBlip);
	          ESTADO_SET_TARGET(ST_WAIT);
	          break;
	        }
	      }
	    }
	    break;

	  //********
    case MT_ACCEL:
      static int LastTime;

      if(time10[T3]-LastTime >= 30)
      {
        HTACreadAllAxes(PORT_ACC,ACCEL[0],ACCEL[1],ACCEL[2]);
        if(abs(ACCEL[ACCEL_AXIS_RAMPA]-ACCEL_Offset)>ACCEL_DELTA){
          if (ACCEL_Filter++ > 2){
            ACCEL_Rampa = true;
            ACCEL_Filter = 3;
          }
        }else{
          if (ACCEL_Filter-- < 2){
            ACCEL_Filter = 0;
            ACCEL_Rampa = false;
          }
        }
        LastTime=time10[T3];
      }

	    break;

	  //********
    case MT_TOQUE:
	    wait1Msec(1);
	    break;

	  //********
    case MT_LL:

	    LL_Avr = LLreadAverage(PORT_LL);
	    LL_IO  = LLreadResult(PORT_LL);

	    break;

	  //********
    case MT_US:
	    wait1Msec(30);
	    ReadAllUS_short(PORT_ARD,USwall);
	    break;

  }
}
示例#16
0
task main()
{
	init();
	waitForStart();
	driveSonar(1,25,80);
	allStop();
	wait1Msec(100);
	backward(20);
	wait1Msec(700);
	sticksDown();
	wait1Msec(250);
	allStop();
	wait1Msec(100);
  	raiseLift(100);
  	time1[T1]=0;
  	while (nMotorEncoder[intake] < 410 && time1[T1] < 2500) //while the encoder wheel turns one revolution
  	{
  		times = time1[T1];
  	}
	allStop();
	wait1Msec(500);
	//if(time1[T1]>2500)
	//	while(true){
	//		nxtDisplayCenteredBigTextLine(1,":(");
	//	}
	releaseBalls();
	allStop();
	wait1Msec(3000);
	retainBalls();
	allStop();
	wait1Msec(750);
  	lowerLift(80);
  	while (nMotorEncoder[intake] > 220) //while the encoder wheel turns one revolution
  	{
  	}
	allStop();
	wait1Msec(500);
	releaseAutoBall();
	wait1Msec(500);
	motor[intake] = 100;
	wait1Msec(1000);
	motor[intake] = 0;
	turn(0,180,100);
	allStop();
	wait1Msec(100);
	drive(1,1,75);
	sticksUp();
	drive(0,1,75);
	time1[T1]=0;
	while(SensorValue[sonarSensor]>30||time1[T1]<1200){
		left(50);
	}
	while(SensorValue[sonarSensor]<200){
		left(50);
	}
	allStop();
	wait1Msec(100);
	turn(0,25,70);
	allStop();
	wait1Msec(100);
	driveSonar(1,25,50);
	backward(30);
	wait1Msec(700);
	sticksDown();
	wait1Msec(250);
	allStop();
	raiseLift(100);
  	while (nMotorEncoder[intake] < 1000) //while the encoder wheel turns one revolution
  	{
  	}
	allStop();
	wait1Msec(500);
	releaseBalls();
	allStop();
	wait1Msec(3000);
	retainBalls();
	allStop();
	wait1Msec(750);
  	lowerLift(80);
  	while (nMotorEncoder[intake] > 740) //while the encoder wheel turns one revolution
  	{
  	}
	allStop();
	wait1Msec(500);
}
task main() {
	RobotState state;
	initialize(&state);
	short leftSpeed, rightSpeed;
	short prevState = INITIALSTATE;
	bool distLess50;
	bool irDetected = false;
	bool sawRedBlue = false;
	bool goBackward = false;
	float startAngle = 0;

	//waitForStart();
	wait1Msec(state.delayTime*1000);
	INITIALDRIVE();
	while(true){
		//if state changes: stop motors, play tone, reset timers, gyro and lights
		if (prevState != state.currentState){
			if (prevState != PARK_DRIVE2 && prevState != CHECKEND){
				stopMotors();
				leftSpeed = 0;
				rightSpeed = 0;
			}
			AUDIO_DEBUG(500, 10);
			time1[T1] = 0;
			resetGyroAccel(&state);
			LEDController(0x00);
			distLess50 = false;
			startAngle = state.degrees;
		}

		getSensors(&state);
		prevState = state.currentState;

		if(state.currentState == FINDLINE_TURN){
			drive(0, TURNSPEED);
			if(state.color2 == RED || state.color2 == BLUE){
				sawRedBlue = true;
			}
			if (sawRedBlue && state.color2 == BLACK){
				state.currentState = LINEFOLLOW;
			}
			if(abs(state.degrees) > 10){
				state.currentState = LINEFOLLOW;
			}
		/* state FINDLINE_DRIVE seems unnecessary
		} else if (state.currentState == FINDLINE_DRIVE) {
			drive(DRIVESPEED, DRIVESPEED*.95);
			if (state.irDir == 5 && irDetected == false) {
				state.currentState = SCOREBLOCK;
			} else if(state.color2 == RED || state.color2 == BLUE){
				state.currentState = LINEFOLLOW;
			} */
		} else if (state.currentState == LINEFOLLOW) {
			if (state.dist < 50) {
				distLess50 = true;
			}
			if (state.irDir == 5 && irDetected == false) {
				state.currentState = SCOREBLOCK;
			} else if (state.dist > 50 && distLess50 && irDetected == true) {
				state.currentState = GOTOEND;
			} else if (goBackward){
				if (state.color == RED || state.color == BLUE) {
					leftSpeed = -DRIVESPEED*LINEFOLLOWRATIO;
					rightSpeed = -DRIVESPEED;
				}else {
					leftSpeed = -DRIVESPEED;
					rightSpeed = -DRIVESPEED*LINEFOLLOWRATIO;
				}
			} else {
				if (state.color2 == RED || state.color2 == BLUE) {
					leftSpeed = DRIVESPEED*LINEFOLLOWRATIO;
					rightSpeed = DRIVESPEED;
				} else {
					leftSpeed = DRIVESPEED;
					rightSpeed = DRIVESPEED*LINEFOLLOWRATIO;
				}
			}
			drive(leftSpeed, rightSpeed);
		} else if (state.currentState == SCOREBLOCK) {
			irDetected = true;
			goBackward = true;
			LEDController(B2);
			blockScorer();
			state.currentState = LINEFOLLOW;
		} else if (state.currentState == GOTOEND) {
			drive(-DRIVESPEED, -DRIVESPEED);
			if(time1[T1] >= 300) {
				state.currentState = PARK_TURN1;
			}
		} else if (state.currentState == PARK_TURN1) {
			drive(-TURNSPEED, TURNSPEED);
			if(abs(state.degrees) >= 15){
				state.currentState = PARK_DRIVE1;
			}
		} else if (state.currentState == PARK_DRIVE1) {
			drive(-DRIVESPEED, -DRIVESPEED);
			if(state.color == RED || state.color == BLUE){
				state.currentState = PARK_TURN2;
			}
		} else if (state.currentState == PARK_TURN2) {
			DRIVESPECIAL(-TURNSPEED, TURNSPEED);
			if(abs(state.degrees) >= 22.5){
				state.currentState = PARK_DRIVE2; //skip state HARVEST
			}
		} else if (state.currentState == HARVEST) {
			motor[harvester] = 100;
			DRIVESPECIAL(2*DRIVESPEED, 2*DRIVESPEED);
			if (time1[T1] >= 500){
				leftSpeed = startAngle/abs(startAngle)*TURNSPEED;
				rightSpeed = -startAngle/abs(startAngle)*TURNSPEED;
				drive(leftSpeed, rightSpeed);
				if (abs(startAngle-state.degrees) <= 0.5){
					state.currentState = PARK_DRIVE2;
				}
			}
		} else if (state.currentState == PARK_DRIVE2) {
			DRIVESPECIAL(-2*DRIVESPEED, -2*DRIVESPEED);
			if(abs(state.x_accel) > 35 && state.x_accel < 100){
				wait1Msec(20);
				state.currentState = CHECKEND;
			}
		} else if (state.currentState == CHECKEND) {
			if(abs(state.x_accel) > 35 && abs(state.x_accel) < 100){
				state.currentState = END; //if it's > 30 after 1 sec of pushing, you're done
			} else {
				state.currentState = PARK_DRIVE2; //else you need to do more pushing
			}
		} else if (state.currentState == END){
			DRIVESPECIAL(-2*DRIVESPEED, -2*DRIVESPEED);
			if(time1[T1] >= 1000){
				break;
			}
		} else {
			break;
		}
	}
}
/////////////////////////////////////////////////////////////////////////////////////////////////////
//                                         Main Task
// The following is the main code for the autonomous robot operation.
// Sequence of Operations:
// 1. Detect the IR beam
// 2. Drop the block
// 3. Backup, Turn, Go Back, Turn
// 4. Climb the Ramp
// At the end of the autonomous period, the FMS will autonmatically abort execution of the program.
/////////////////////////////////////////////////////////////////////////////////////////////////////
task main()
{

	//initialize the servos
  initializeRobot();

  bFloatDuringInactiveMotorPWM = true;

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

  servo[servoIRDrop]= 240;

	//raise the rack so that the robot can move easily
  RaiseRack();

  //move the arm up so that the robot can move easily
  motor[motorArm] = 65;
  wait1Msec(1000);
  motor[motorArm] = 0;

 	//detect the IR beam, we get out when 5 is detected
 	//returns the number of ticks moved
 	int currentTicks = MoveUntilIR();

 	//move up a bit
 	Move(6, FORWARD, 35);
	wait1Msec(30);

	//drop the block
 	DropBlockIR();

  //Bring the drop block mechanism higher,
 	//so that its not in the way of the center part of the beam
  servo[servoIRDrop] = 255 ;
	wait1Msec(70);

	//backup
  //dont go to the start position, start a bit before, so that wall will not be touched
  nMotorEncoder[motorLeft] = 0;
  nMotorEncoderTarget[motorLeft] =  (currentTicks * -1)  + 550;

  //power  the right motor earlier than the left motor
  motor[motorRight] = -55;
  wait1Msec(50);

  //power the left motor
  motor[motorLeft] = -55;

  //wait for it go back
  while ( nMotorRunState[motorLeft] != runStateIdle )
  {
  }

  //stop the motors
  motor[motorLeft] = 0;
  motor[motorRight] = 0;
	wait1Msec(50);

	//turnright towards the ramp
	PointTurnRight(1500);
	wait1Msec(30);

	//go back a bit for more clearance
	Move(50, BACKWARD, 55);
	wait1Msec(30);

	PointTurnRight(1900);
	wait1Msec(30);

	Move(40, BACKWARD, 55);
	wait1Msec(30);

	//brake instead of coast
	bFloatDuringInactiveMotorPWM = false;

	//check for other robots
	if(SensorValue[sonarSensor] <= 75) //if obstruction is present, max power
		Move(18, BACKWARD, 90);
	else if(SensorValue[sonarSensor] > 75)
		Move(18, BACKWARD, 60);

	//bring it down so it is not in the way of the rack motor
	servo[servoIRDrop] = 200;
	wait1Msec(70);

	// We are done
  while (true)
	{

	}

}
示例#19
0
/**Time-Based Tread**/
void tread(int power, int time){
	motor[treadMotor] = power;
	wait1Msec(time);		//Time intake runs
	motor[treadMotor] = 0;
}
void DropBlockIR()
{
	servo[servoIRDrop]=75;
	wait1Msec(700);
}
示例#21
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 specMotorSpeed = 0;
    int totalMessages = 0;
    int topSpeed = MOTOR_POWER_DOWN_MAX;
    int armTopSpeed = 65;
    //int specTopSpeed = 20;
    int scoopTopSpeed = 40;
    int scoopMotorSpeed = 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_y1 / 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_y2 / 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

                scoopMotorSpeed = joystick.joy2_y1  / JOYSTICK_Y1_MAX * scoopTopSpeed; // Map the teleMotorSpeed variable to joystick 2_y1
                if (abs(scoopMotorSpeed) < JOYSTICK_DEAD_ZONE) scoopMotorSpeed = 0; // Make sure that the joystick isn't inside dead zone

                //specMotorSpeed = joystick.joy1_x1  / JOYSTICK_Y1_MAX * specTopSpeed; // Map the teleMotorSpeed variable to joystick 2_y1
                //if (abs(specMotorSpeed) < JOYSTICK_DEAD_ZONE) specMotorSpeed = 0;

                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[scoopMotor] = scoopMotorSpeed;
                //motor[specMotor] = specMotorSpeed;

                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(0) == 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);
                }


                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()
{
	// Gekregen code voor BlueTooth
	string sig = "";
	TFileIOResult nBTCmdRdErrorStatus;
	int nSizeOfMessage;
	ubyte nRcvBuffer[kMaxSizeOfMessage];
	// Einde gekregen code voor BlueTooth

  while(true) // Main loop
  {
  	/*
		 * Gekregen code voor BlueTooth
		 * leest berichten die BlueTooth stuurt uit en stopt die in een string.
		 */
    nSizeOfMessage = cCmdMessageGetSize(INBOX);
    if (nSizeOfMessage > kMaxSizeOfMessage)
      nSizeOfMessage = kMaxSizeOfMessage;
    if (nSizeOfMessage > 0){
    	nBTCmdRdErrorStatus = cCmdMessageRead(nRcvBuffer, nSizeOfMessage, INBOX);
    	nRcvBuffer[nSizeOfMessage] = '\0';
    	string s = "";
    	stringFromChars(s, (char *) nRcvBuffer);
    	displayCenteredBigTextLine(4, s);
    /*
		 * Einde gekregen code voor BlueTooth
		 */

    	sig = nRcvBuffer;// zet bluetoothsignaal in variabele
    	//writeDebugStream(sig); // DEBUGCODE - check of het signaal juist is weggeschreven

    	// naar voren rijden zonder lijndetectie
    	if(sig == "U"){
   			setMotor(motorA, 30);
   			setMotor(motorB, 30);
   			wait1Msec(10);
   		}
   		// naar achter rijden zonder lijndetectie
			if(sig=="D"){
				setMotor(motorA, -30);
				setMotor(motorB, -30);
				wait1Msec(10);
			}
			// maak een kwartslag naar links en zet lijndetectie weer aan (voor kruispunten)
			if(sig=="L"){
				setMotorTarget(motorA, 0, 0);
				setMotorTarget(motorB, 360, 30);
				waitUntilMotorStop(motorB);
				startTask(Rijden);
				k=1; // zet anti-lijnhump op 1
				l=1; // zet anti-lijnhump op 1
			}
    	// maak een kwartslag naar rechts en zet lijndetectie weer aan (voor kruispunten)
  		if(sig=="R"){
  		  setMotorTarget(motorA, 360, 30);
			  setMotor(motorB, 0);
			  waitUntilMotorStop(motorA);
			  startTask(Rijden);
				k=1; // zet anti-lijnhump op 1
				l=1; // zet anti-lijnhump op 1
			}
			// rij een beetje naar voren en zet lijndetectie weer aan (voor kruispunten)
			if(sig=="F"){
				setMotorTarget(motorA, 180, 30);
				setMotorTarget(motorB, 180, 30);
				waitUntilMotorStop(motorA);
				waitUntilMotorStop(motorB);
				k=1; // zet anti-lijnhump op 1
				startTask(Rijden);
			}
			// Stoppen
			if(sig=="A"){
				setMultipleMotors(motorA, motorB, 0);
				stopTask(Rijden);
				clearSounds();

			}
			// Zet lijndetectie aan
			if(sig=="C"){
				k=1; // zet anti-lijnhump op 1
				l=1; // zet anti-lijnhump op 1
				startTask(Rijden);
			}

			/*
			 * nog 1 knop ongebruikt (B).
			 */

			// Na elk commando, wacht 0,1s.
    	wait1Msec(100);
    }
	}
}
task main()
{
	//wait
	servo[topServo] = 235;
	initializeRobot();
	StartTask(getHeading);
	wait1Msec(400);
  forwardInc(48, 30);
  //Calculates the positioning of the center goal after 10 position tics at 200msec intervals
	float a = 0;
	float b = 0;
	float c = 0;
  wait1Msec(400);
  StartTask(infared);
	wait1Msec(200);
	int ir2 = acS1a+ acS2a+acS3a+acS4a+acS5a;
	int ir1 = acS1b+acS2b+acS3b+acS4b+acS5b;
	for(float i = 0; i <= 10; i++){
      if(ir1 < ir2-10){
      	PlayImmediateTone(440, 10);
        a += 1;
      }else if(abs(ir1-ir2) <= 18){ //ir1-4 > ir2 && ir1+4 < ir2
      	PlayImmediateTone(1024, 10);
      	b += 1;
      }else if(ir1 > ir2+10){
      	PlayImmediateTone(2024, 10);
				c += 1;
      }else{
      	PlayImmediateTone(1024, 10);
      	//b += 1;
      }
      wait1Msec(200);
  }
  ClearSounds();
  //code for each of the three center positions a=1 b=2 c=3
  StopTask(getHeading);
  wait10Msec(50);
  StartTask(getHeading);
	wait1Msec(200);
  if(a > (b+c)/2){
  	//PlayImmediateTone(440, 10);
  	currHeading = 0;
		turnDeg(-20, 25);
		wait10Msec(50);
  	forwardInc(74, 30);
  	wait10Msec(50);
  	StopTask(getHeading);
  	currHeading = 0;
  	wait1Msec(200);
  	StartTask(getHeading);
  	wait1Msec(100);
  	turnDeg(-52, 20);//sdfhjkjhgftrhjkl;kjrtykl;kjgfdsghj.,mhfdsfghjkfdsgtgr5gtgr
  	wait10Msec(25);
  	forwardInc(-22, 25);
  	liftPos(3);
  	servo[topServo] = 210;//original 150
  	wait10Msec(25);
  	servo[topServo] = 235;
  	wait10Msec(500);
  	liftPos(0);
  	wait10Msec(500);
  }else if(b > (a+c)/2){
  	PlayImmediateTone(1024, 10);
  	StopTask(getHeading);
  	currHeading = 0;
  	wait1Msec(200);
  	StartTask(getHeading);
  	wait1Msec(100);
		turnDeg(180, 25); //do a one 80
		StopTask(getHeading);
  	currHeading = 0;
  	wait1Msec(200);
  	StartTask(getHeading);
		wait10Msec(70);
  	forwardInc(-36, 30);//go back __ inches
  	//StartTask(getHeading);
  	//wait1Msec(100);
		//turnDeg(20, 25);
  	wait10Msec(200);
  	liftPos(3);
  	servo[topServo] = 210;//original 150
  	wait10Msec(25);
  	servo[topServo] = 235;
  	wait10Msec(100);
  	liftPos(0);
  	wait10Msec(500);
  }else if(c > (a+b)/2){
  	PlayImmediateTone(2024, 10);
  	StopTask(getHeading);
  	currHeading = 0;
  	wait1Msec(200);
  	StartTask(getHeading);
  	wait1Msec(100);
		turnDeg(-120, 25);
		StopTask(getHeading);
  	currHeading = 0;
  	wait1Msec(200);
  	StartTask(getHeading);
		wait10Msec(70);
  	forwardInc(-35, 30);
  	StartTask(getHeading);
  	wait1Msec(100);
		turnDeg(-32, 25);
		forwardInc(-4, 25);
  	wait10Msec(50);
  	liftPos(3);
  	servo[topServo] = 210;//original 150
  	wait10Msec(25);
  	servo[topServo] = 235;
  	wait10Msec(100);
  	liftPos(0);
  	wait10Msec(500);
  }
}
task Rijden()
{
	 // Geluid dat de robot maakt tijdens het rijden, in loop.
	k = 0; // Reset kruispuntboolean naar 0
	//l = 0;
	int var = 0; // variabele die naar een waarde wordt geset afhankelijk van welke sensor een signaal geeft.

	while(true){
		playSoundFile("hehe.rso");
		// Linker sensor: Trigger wanneer de zwart-wit sensor (links) te weinig wit registreert en dus te veel op de lijn komt. Voer case 1 uit: stuur naar links.
		if(SensorValue[WIT] < 55)
			var=1;
		// Rechter sensor: Trigger wanneer de kleurensensor (rechts) zwart registreert en dus te veel op de lijn komt. Voer case 1 uit: stuur naar rechts.
		if((SensorValue[ZWART]==blackcolor)||(SensorValue[ZWART]==greencolor))
			var=2;
		// kruispuntherkenning: Trigger wanneer zowel de kleurensensor als de zwartwit sensor getriggerd worden. Voer case 3 uit: stop, rij achteruit, wacht op commando.
		if(((SensorValue[ZWART]==blackcolor)||(SensorValue[ZWART]==greencolor))&&((SensorValue[WIT] <55)))
			var=3;
		// Tegenovergestelde van kruispuntherkenning: Trigger als de sensoren alleen wit registreren. Voer case 4 uit: rij naar voren.
		if(((SensorValue[ZWART]!=blackcolor)&&(SensorValue[ZWART]!=greencolor))&&((SensorValue[WIT] >55)))
			var=4;
		// Obstakelsensor: Trigger wanneer de sonar een obstakel registreert. Voer case 5 uit: geef geluid en stop.
		if (SensorValue[sonar] <30)
			var=5;

		switch(var)
		{
			case 1: //stuur naar links
			setMotor(motorB,30);
			setMotor(motorA,-5);
			var=0;
			k=0;
			break;

			case 2: //stuur naar rechts
			setMotor(motorB,-5);
			setMotor(motorA,30);
			var=0;
			k=0;
			break;

			case 3: //stoppen, ga achteruit, wacht op BlueTooth signaal
			setMotor(motorB,0);
			setMotor(motorA,0);
			wait1Msec(100);
			clearSounds();
			setMotorTarget(motorA,85,-30); // ga achteruit
			setMotorTarget(motorB,85,-30);
			waitUntilMotorStop(motorA);
			waitUntilMotorStop(motorB);
			waitUntil(k==1); // wacht op een BlueTooth signaal dat links, rechts of vooruit zegt en k=1 triggert.
			var=0;
			k=0;
			break;

			case 4: // rij naar voren
			setMotor(motorB,30);
			setMotor(motorA,30);
			var=0;
			k=0;
			break;

			case 5: // als sonar wat ziet: schreeuw, rem rustig af en roteer 180*.
			clearSounds();
			playSoundFile("scream4.rso");
			for (int i=3000;i>0 ; i--)
			{
				setMotor(motorA,i/100);
				setMotor(motorB,i/100);
			}
			//waitUntil(l==1);
			//nxtDisplayTextLine(1,"%d",i);
			waitUntilMotorStop(motorA);
			waitUntilMotorStop(motorB);

			// oorspronkelijk hadden we code om een object te ontwijken, zowel linksom als rechtsom,
			// met als alternatief om 180* te draaien. Dit kregen we echter niet op tijd volledig
			// aan de praat, waardoor we hebben gekozen om hardcoded 180* te draaien.
			// zie hiervoor de AvoidObject.c en AvoidObject.h files.
			//
			//AvoidMain();
			//waitUntil(avoidDone==1);
			//waitUntil(l==1);
			//avoidDone = 0;
			//
			// einde ontwijkcode

			// hardcoded rotate robot 180*
			//setMotorTarget(motorA, 360, 30); // rechtsom draaien
			//setMotorTarget(motorB, 360, -30);
			//waitUntilMotorStop(motorA);
			//waitUntilMotorStop(motorB);
			// end hardcoded rotate robot 180*
			setMotorTarget(motorA, 360, 30);
			waitUntilMotorStop(motorA);
			setMotorTarget(motorA, 135, 30);
			setMotorTarget(motorB, 135, 30);
			waitUntilMotorStop(motorA);
			setMotorTarget(motorB, 360,30);
			waitUntilMotorStop(motorB);
			setMotorTarget(motorA, 540, 30);
			setMotorTarget(motorB, 540, 30);
			waitUntilMotorStop(motorA);
			setMotorTarget(motorB, 360,30);
			waitUntilMotorStop(motorB);
			setMotorTarget(motorA, 135, 30);
			setMotorTarget(motorB, 135, 30);
			waitUntilMotorStop(motorA);

			var=0;
			l=0;
			break;
		}
	}
}
示例#25
0
void lock_msec(int speed, int duration){
  setArmSpeed(speed);
  wait1Msec(duration);
  setArmSpeed(0);
}
示例#26
0
void turnRobot(float degrees, int direction, int turntype) {
	float arc = degrees / 360;
	float circumference, inches;
	int ticks;
	static int deltaLeft = 0, deltaRight = 0;

	if (TIGHT_TURNS) {
		circumference = PI * WHEEL_BASE;
	}
	else {
		circumference = 2 * PI * WHEEL_BASE;
	}

	inches = circumference * arc;
	ticks = inchesToTicks(inches);
	nMotorEncoder[drive1] = 0;
	nMotorEncoder[drive2] = 0;

	if (PID_CONTROL == false) {
		if (turntype == RIGHT) {
			motor[drive2] = TURN_POWERL * direction;
			if (TIGHT_TURNS) motor[drive1] = TURN_POWERR * direction * REVERSE;
		}
		else {
			motor[drive1] = TURN_POWERR * direction;
			if (TIGHT_TURNS) motor[drive2] = TURN_POWERL * direction * REVERSE;
		}
		while (true) {
			if (abs(nMotorEncoder[drive1]) >= ticks) break;
			if (abs(nMotorEncoder[drive2]) >= ticks) break;
		}
	}
	else {
		if (turntype == RIGHT) {
			nMotorEncoderTarget[drive2] = ticks * direction;
			if (TIGHT_TURNS) nMotorEncoderTarget[drive1] = ticks * direction * REVERSE;
			motor[drive2] = TURN_POWERL * direction;
			if (TIGHT_TURNS) motor[drive1] = TURN_POWERR * direction * REVERSE;
		}
		else {
			nMotorEncoderTarget[drive1] = ticks * direction;
			if (TIGHT_TURNS) nMotorEncoderTarget[drive2] = ticks * direction * REVERSE;
			motor[drive1] = TURN_POWERL * direction;
			if (TIGHT_TURNS) motor[drive2] = TURN_POWERR * direction * REVERSE;
		}
		while (true) {
			TNxtRunState state1, state2;
			// 0 = runStateIdle
			// 1 = runStateHoldPosition
			// 16 = runStateRampUp
			// 32 = runStateRunning
			// 64 = runStateRampDown
			state1 = nMotorRunState[drive1];
			state2 = nMotorRunState[drive2];
			if (state1 == runStateIdle && state2 == runStateIdle) break;
		}
	}

	motor[drive1] = 0;
	motor[drive2] = 0;
	metricsDisplay(ticks, ticks);
	deltaLeft = nMotorEncoder[drive2] - ticks;
	deltaRight = nMotorEncoder[drive1] - ticks;
	nMotorEncoder[drive1] = 0;
	nMotorEncoder[drive2] = 0;
	if (PLAY_SOUNDS == true) {
		PlaySound(soundBeepBeep);
	}
	wait1Msec(DRIVE_DELAY);
	return;
}
示例#27
0
void drive_msec(int speed, int duration) {
  setDriveSpeed(speed);
  wait1Msec(duration);
  killdrive;
}
示例#28
0
task main () {
  bool selected_50hz = true;

  nxtDisplayCenteredTextLine(0, "HiTechnic");
  nxtDisplayCenteredBigTextLine(1, "Color V2");
  nxtDisplayCenteredTextLine(4, "Config operating");
  nxtDisplayCenteredTextLine(5, "frequency to");
  nxtDisplayCenteredTextLine(6, "50 or 60 Hz");
  wait1Msec(2000);

  eraseDisplay();
  nxtDisplayCenteredTextLine(0, "Use arrow keys");
  nxtDisplayCenteredTextLine(1, "to select a");
  nxtDisplayCenteredTextLine(2, "frequency");
  nxtDisplayCenteredBigTextLine(4, "50 60");
  nxtDisplayCenteredTextLine(6, "[enter] to set");
  nxtDisplayCenteredTextLine(7, "[exit] to cancel");

  nxtDrawRect(19, 34, 44, 16);

  while (true) {
    // Do nothing while no buttons are pressed
    while (nNxtButtonPressed == kNoButton) {
      wait1Msec(1);
    }

    switch (nNxtButtonPressed) {
      // if the left button is pressed, set the sensor for 50Hz
      case kLeftButton:
            if (selected_50hz) {
              PlaySound(soundBlip);
              while(bSoundActive) {wait1Msec(1);}
            } else {
              selected_50hz = true;
              nxtEraseRect(55, 34, 80, 16);
              nxtDisplayCenteredBigTextLine(4, "50 60");
              nxtDrawRect(19, 34, 44, 16);
            }
            break;

       // if the right button is pressed, set the sensor for 60Hz
       case kRightButton:
						if (!selected_50hz) {
						  PlaySound(soundBlip);
						  while(bSoundActive) {wait1Msec(1);}
						} else {
						  selected_50hz = false;
						  nxtEraseRect(19, 34, 44, 16);
						  nxtDisplayCenteredBigTextLine(4, "50 60");
						  nxtDrawRect(55, 34, 80, 16);
						}
            break;

        // Make the setting permanent by saving it to the sensor
        case kEnterButton:
            eraseDisplay();
            nxtDisplayCenteredTextLine(2, "The Sensor is");
            nxtDisplayCenteredTextLine(3, "configured for");
            if (selected_50hz) {
              _HTCSsendCommand(HTCS2, 0x35);
              nxtDisplayCenteredTextLine(4, "50 Hz operating");
            } else {
              _HTCSsendCommand(HTCS2, 0x36);
              nxtDisplayCenteredTextLine(4, "60 Hz operating");
            }
            nxtDisplayCenteredTextLine(5, "frequency");
            for (int i = 5; i > 0; i--) {
              nxtDisplayCenteredTextLine(7, "Exiting in %d sec", i);
              wait1Msec(1000);
            }
            StopAllTasks();
            break;
    }

    // Debounce the button
    while (nNxtButtonPressed != kNoButton) {
      wait1Msec(1);
    }

  }
}
示例#29
0
文件: E1.c 项目: jimmycode/INB860
// update motor speed and wait for duration
void control(int left, int right, int duration = 0){
    motor[Left] = left;
    motor[Right] = right;
    if(duration)
        wait1Msec(duration);
}
/*-----------------------------------------------------------------------------*/
void
iqMenuConfiguration()
{
    static  tConfigSelect configSelect = kConfig_exit;
    static int offset = 0;
    bool done   = false;
    bool update = true;

    // Wait for LCD button release
    while( nLCDButtons != kButtonNone )
    wait1Msec(10);

    eraseDisplay();

    while( !done ) {
        // Display menu
        if( update ) {
            iqMenuConfigurationDisplay( configSelect, offset );
            update = false;
        }

        // Check LCD buttons
        if( nLCDButtons != kButtonNone ) {
            if( nLCDButtons == kButtonDown  ) {
                if( configSelect++ == kConfig_6 )
                    configSelect = kConfig_6;
                if( configSelect > kConfig_3 && offset == 0 )
                    offset = 3;
            }

            if( nLCDButtons == kButtonUp  ) {
                if( configSelect-- == kConfig_exit )
                    configSelect = kConfig_exit;
                if( configSelect < kConfig_2 && offset == 3 )
                    offset = 0;
            }

            if( nLCDButtons == kButtonSelect  ) {
                if( iqMenuConfigurationSelect( configSelect ) )
                    done = true;
            }

            // Exit doesn't really work
            if( nLCDButtons == kButtonExit  ) {
                done = true;
            }

            // Wait for LCD button release
            while( nLCDButtons != kButtonNone )
                wait1Msec(10);

            // update display
            update = true;
        }

    wait1Msec(25);
    }

    // erase before we leave this sub menu
    eraseDisplay();
}