Example #1
0
task MapRoom()
{

	float botLastXCoordinate = 0.0;
	float botLastYCoordinate = 0.0;
	float botCurrentXCoordinate = 0.0;
	float botCurrentYCoordinate = 0.0;
	int wallXCoordinate = 0;
	int wallYCoordinate = 0;
	int xCoordinateDisplayOffset = 50;
	int yCoordinateDisplayOffset = 40;

	startGyro();
	resetGyro();
	eraseDisplay();

	while(true)
	{
		//Robot position
		botCurrentXCoordinate = botLastXCoordinate + EncoderDistance(ReadEncoderValue()) * sinDegrees(readGyro());
		botCurrentYCoordinate = botLastYCoordinate + EncoderDistance(ReadEncoderValue()) * cosDegrees(readGyro());

		//Wall mapping
		wallXCoordinate = botCurrentXCoordinate + ReadSonar(2) * CENTIMETERS_TO_INCHES * cosDegrees(readGyro());
		wallYCoordinate = botCurrentYCoordinate + ReadSonar(2) * CENTIMETERS_TO_INCHES * sinDegrees(readGyro());

		nxtSetPixel(wallXCoordinate + xCoordinateDisplayOffset, wallYCoordinate + yCoordinateDisplayOffset);
		ResetEncoderValue();
		botLastXCoordinate = botCurrentXCoordinate;
		botLastYCoordinate = botCurrentYCoordinate;
		wait1Msec(20);
	}
}
Example #2
0
int searchSpot() {
//menelusuri garis hitam sampai menemukan "color"
	int hue;
	int colortemp;
	int threshold = 65;
	resetGyro(gyroSensor);
	moveMotorTarget(leftMotor,360,100);
	moveMotorTarget(rightMotor,360,100);
	while(getMotorMoving(leftMotor)||getMotorMoving(rightMotor))
		sleep(1);
	while(1)
	{
		// sensor sees light:
		if(getColorReflected(colorSensor) < threshold)
		{
			// counter-steer right:
			motor[leftMotor]  = 55;
			motor[rightMotor] = 15;
		}
		// sensor sees dark:
		else
		{
			// counter-steer left:
			motor[leftMotor]  = 15;
			motor[rightMotor] = 55;
		}
		hue=getColorHue(colorSensor);
		if(hue==99 || hue==253) {
			colortemp=hue==99?green:red;
			break;
		}
	}
	return colortemp;
}
Example #3
0
task main() {
	initializeRobot();
	bool programRunning = false;
	while (true) {
		wait1Msec(5);
		if (programStarted() && !programRunning) {   // I.E. program JUST started
			wait1Msec(selection[DELAY_MENU]*1000);   // Wait for selected delay
			currentRoutine=selection[STARTPOS_MENU]; // Set routine
			if (gyroEnabled()) StartTask(gyroTask);  // Start gyro tracking
			resetGyro();          // Set gyro to 0
			resetDrive();         // Set encoders to 0
			programRunning=true;  // Set the running veriable so this doesn't execute again
		}

		if (programStarted()) {
if (DEBUG) nxtDisplayCenteredTextLine(4, "%i", currentRoutine);
			//Constantly update arm movements
			if (armState != 0) motor[BlockArm] = getTargetingPower(armState, nMotorEncoder[BlockArm], 0.04, 80);

			if (currentInc < 100 && !done) { // make sure step never wraps
				if (completed) {
					//Start sequences
					initRoutine(); currentInc++; // Increment step, make sure it doesn't ever wrap
				}
				runTargets();
			}
		} else {
			processMenuInput();
		}
	}
}
Example #4
0
void turnBack(){
	resetGyro(gyroSensor);
	moveMotorTarget(leftMotor,447,100);
	moveMotorTarget(rightMotor,-447,100);
	while ( getMotorMoving(leftMotor) || getMotorMoving(rightMotor) ) {
		sleep(1);
	}
}
Example #5
0
void turn(int dir,int deg,int lmot,int rmot) {
//berbelok ke arah "dir", sebesar "deg" derajat
	resetGyro(gyroSensor);
	setMotorSpeed(leftMotor,lmot);
	setMotorSpeed(rightMotor,rmot);
	while(getMotorMoving(leftMotor) || getMotorMoving(rightMotor)) {
		sleep(1);
		if((getGyroDegrees(gyroSensor)<deg && dir==left)
			||(getGyroDegrees(gyroSensor)>deg && dir==right))
		break;
	}
}
Example #6
0
// turn right
bool Motoruino2::turnRight (int angle)
{
	if (angle < -180 || angle > 180) return false;

	// First Reset Gyro
	resetGyro(true, false,false);

	// Prepare the Params
	motoruino2Comm.startNewComm(ADRRESS_SLAVE);

	motoruino2Comm.addLongData( angle );		// Angle

	// Send it and validate reply
	return motoruino2Comm.sendDataReplyBool(TTDG);
}
Example #7
0
int cekLine(int dir,int deg,int lmot,int rmot) {
//mengembalikan 1 jika menemukan garis
	int threshold = 65;
	resetGyro(gyroSensor);
	setMotorSpeed(leftMotor,lmot);
	setMotorSpeed(rightMotor,rmot);
	while(getMotorMoving(leftMotor) || getMotorMoving(rightMotor)) {
		sleep(1);
		if(getColorReflected(colorSensor) < threshold)
			return 1;
		else if((getGyroDegrees(gyroSensor)<deg && dir==left)
			||(getGyroDegrees(gyroSensor)>deg && dir==right))
			break;
	}
	return 0;
}
Example #8
0
//rotate the gyro clockwise is positive for this method
void rotate(int degrees) {
	resetGyro(GYRO);
	if(degrees < 0) {
		while(getGyroDegrees(GYRO) < -degrees) {
			setMotorSpeed(RIGHT_DRIVETRAIN_MOTOR, -100);
			setMotorSpeed(LEFT_DRIVETRAIN_MOTOR, -100);
		}
	} else {
		while(getGyroDegrees(GYRO) > -degrees) {
			setMotorSpeed(LEFT_DRIVETRAIN_MOTOR, 100);
			setMotorSpeed(RIGHT_DRIVETRAIN_MOTOR, 100);
		}
	}
	setMotorSpeed(LEFT_DRIVETRAIN_MOTOR, 0);
	setMotorSpeed(RIGHT_DRIVETRAIN_MOTOR, 0);
}
void turn( int deg, int power )
{
	resetGyro();
	if( deg < 0 )
		power = -1 * power;

	while( abs(gyroVal) < abs(deg) )
	{
		motor[right] = power;
		motor[left] = -1 * power;
		updateGyro();
	}

	motor[right] = 0;
	motor[left] = 0;
}
void turn( int deg, int power, bool ramp)
{
	resetGyro();
	float rampMult = 1.0;
	if( deg < 0 )
		power = -1 * power;

	while( abs(gyroVal) < abs(deg) )
	{
		if(ramp){rampMult = rampFunc(abs(gyroVal)/(float)abs(deg));}
		motor[right] = power * rampMult;
		motor[left] = -1 * power * rampMult;
		updateGyro();
	}

	motor[right] = 0;
	motor[left] = 0;
}
Example #11
0
void turn( int deg, int power )
{
	resetGyro();
	if( deg > 0 )
		power = -1 * power;

	while( abs(gyroVal) < abs(deg) )
	{
		motor[FL] = power;
		motor[BL] = power;
		motor[FR] = power;
		motor[BR] = power;
		updateGyro();
	}

	motor[FL] = 0;
	motor[BL] = 0;
	motor[FR] = 0;
	motor[BR] = 0;
}
Example #12
0
void seekBounds() {
	//Seek the left side of the line.
	faceHeading(0);
	setMotorSpeed(LeftMotor, -10);
	setMotorSpeed(RightMotor, 10);
	while(avgReflectedLight(5) < threshold || getGyroHeading(Gyro) > forwardsHeading*0.90) {
	}
	setMotorSpeed(LeftMotor, 0);
	setMotorSpeed(RightMotor, 0);
	resetGyro(Gyro);

	//Seek the right side of the line.

	setMotorSpeed(LeftMotor, 10);
	setMotorSpeed(RightMotor, -10);
	delay(500);
	while(avgReflectedLight(5) < threshold) {
		rightBound = getGyroHeading(Gyro);
	}

	forwardsHeading = rightBound/2;
}
Example #13
0
void turn( int deg, int power )
{
	resetGyro();
	if( deg > 0 )
		power = -1 * power;
	ClearTimer(T1);
	while( abs(gyroVal) < abs(deg)) // turn until the gyro is correct
	{
		if(time1[T1] > 3000) // timeout if stalled
			break;
		motor[FL] = power;
		motor[BL] = power;
		motor[FR] = power;
		motor[BR] = power;
		updateGyro();
	}

	motor[FL] = 0;
	motor[BL] = 0;
	motor[FR] = 0;
	motor[BR] = 0;
}
Example #14
0
/// rotate with counterclockwise (left) as positive
void rotate(int degrees, bool reset){
    if(reset)
        resetGyro(gyro);

    float kP = 1;

    if(degrees > 0){
        // counterclockwise turn
        while(getGyroHeading(gyro) < degrees){
            setMotorSpeed(leftMotor, -proportionalOutput(kP, degrees, getGyroDegreesFloat(gyro)));
            setMotorSpeed(rightMotor, proportionalOutput(kP, degrees, getGyroDegreesFloat(gyro)));
        }
    } else {
        while(getGyroHeading(gyro) > degrees){
            setMotorSpeed(leftMotor, -proportionalOutput(kP, degrees, getGyroDegreesFloat(gyro)));
            setMotorSpeed(rightMotor, proportionalOutput(kP, degrees, getGyroDegreesFloat(gyro)));
        }
    }

    setMotorSpeed(leftMotor, 0);
    setMotorSpeed(rightMotor, 0);
    sleep(200);
}
void turn( float deg, float power )
{
	resetGyro();
	if( deg > 0 )
		power = -1 * power;
	ClearTimer(T1);
	while( abs(gyroVal) < abs(deg)) // turn until the gyro is correct
	{
		if(time1[T1] > 3000) // timeout if stalled
			Stop(1);
		motor[FrontL] = -power;
		motor[BackL] = -power;
		motor[FrontR] = power;
		motor[BackR] = power;
		updateGyro();
		writeDebugStreamLine("gyro is reading %f" , gyroVal);
	}

	motor[FrontL] = 0;
	motor[BackL] = 0;
	motor[FrontR] = 0;
	motor[BackR] = 0;
}
void RobotBeta1::driveStrait(long maxTime) {
	int slowDownProccessing = 0;
	long cTime = 0;
	
	resetGyro();
	float angle = 0;
	angle = gyro->GetAngle(); // get heading
	GetWatchdog().SetEnabled(true);
	
	
	while ((IsAutonomous()) && (cTime <= maxTime))  { 
		GetWatchdog().Feed();
		float angle = gyro->GetAngle(); // get heading
		if ((slowDownProccessing % 250) == 0) {
			DBG("\n\t\tGyro Angle:  %f\t\t\tDrive Adj:  %f", gyro->GetAngle(), (angle * 0.03));
			cout << "\n\t\tTime:  "; cout << cTime; cout << "\t\t\tExit:  "; cout << maxTime; cout << "\n";
		}
		slowDownProccessing++; cTime++;
		robotDrive->Drive(-.5, (angle * 0.03));// turn to correct heading 
		Wait(0.004); 
	}
	robotDrive->Drive(0.0, 0.0);
}
Example #17
0
task main()
{
	manualCallibColor();
	resetGyro(Gyro);
	seekBounds();
	faceHeading(forwardsHeading);
	while(true) {

		if(isInsideLine()) {
			setMotorSpeed(LeftMotor, 30);
			setMotorSpeed(RightMotor, 30);
		}
		else {
			seekBounds();
			faceHeading(forwardsHeading);
			eraseDisplay();
			displayBigTextLine(0, "Head: %d", forwardsHeading);
		}


	}


}
Example #18
0
/// drive straight using the gyro
void gyroDriveInches(float inches, int driveSpeed) {
    inches = inches-2;
    float kP = 2.5;
    bool onTarget = false;
    resetMotorEncoder(leftMotor);
    resetMotorEncoder(rightMotor);
    resetGyro(gyro);

    while(!onTarget) {

        if(getMotorEncoder(leftMotor) < inches/7.8) {
            setMotorSpeed(leftMotor, driveSpeed + proportionalOutput(kP, 0, getGyroDegreesFloat(gyro)));
        }else {
            onTarget = true;
        }

        if(getMotorEncoder(rightMotor) < inches/7.8) {
            setMotorSpeed(rightMotor, driveSpeed - proportionalOutput(kP, 0, getGyroDegreesFloat(gyro)));
        }
    }
    setMotorSpeed(leftMotor, 0);
    setMotorSpeed(rightMotor, 0);
    sleep(200);
}
Example #19
0
task main() {

	int leftMotorSpeed, rightMotorSpeed, wristMotorSpeed, armMotorSpeed, stickAValue, stickBValue, stickCValue, stickDValue;

	resetMotorEncoder(armMotor);
	resetMotorEncoder(clawMotor);
	resetMotorEncoder(leftMotor);
	resetMotorEncoder(rightMotor);
	resetMotorEncoder(wristMotor);

	resetGyro(gyro);
	startGyroCalibration(gyro, gyroCalibrateSamples2048);
	eraseDisplay();
	getGyroCalibrationFlag(gyro);
	displayString(3, "calibrating gyro");
	wait1Msec(500);
	eraseDisplay();

	startTask(displayMyMotorPositions);


	while (true ){


		/***************************************************************************************************************/
		//Joystick Control:

		/***************************************************************************************************************/
		//Driving:

		if(getJoystickValue(BtnEUp) > 0)   {  //Drive Straight Forward
			driveStraightForward();
		}
		else if(getJoystickValue(BtnEDown) > 0)   {  //Drive Straight Backward
			driveStraightBackward();
		}
		else {
			driveForwardPressed = false;
			driveBackwardPressed = false;

			stickAValue  = getJoystickValue(ChA);
			if ((stickAValue <= 15) && (stickAValue >= -15) ) stickAValue = 0;

			stickBValue  = getJoystickValue(ChB);
			if ((stickBValue <= 15) && (stickBValue >= -15) ) stickBValue = 0;

			if (stickBValue >=0 ) 	 	stickBValue = (stickBValue / 10) * (stickBValue / 10) * 2;
			else stickBValue = 		- (stickBValue / 10) * (stickBValue / 10) * 2;



			leftMotorSpeed = stickAValue - stickBValue;
			rightMotorSpeed = stickAValue + stickBValue;

			if ( leftMotorSpeed > 100) leftMotorSpeed = 100;
			if ( leftMotorSpeed < -100) leftMotorSpeed = -100;

			if ( rightMotorSpeed > 100) rightMotorSpeed = 100;
			if ( rightMotorSpeed < -100) rightMotorSpeed = -100;

			if (leftMotorSpeed >=0 ) 	 	leftMotorSpeed = (leftMotorSpeed / 10) * (leftMotorSpeed / 10);
			else leftMotorSpeed = 		- (leftMotorSpeed / 10) * (leftMotorSpeed / 10);
			if (rightMotorSpeed >=0 ) 	rightMotorSpeed = (rightMotorSpeed / 10) * (rightMotorSpeed / 10);
			else rightMotorSpeed = 		- (rightMotorSpeed / 10) * (rightMotorSpeed / 10);

			setMotorSpeed(leftMotor, leftMotorSpeed);
			setMotorSpeed(rightMotor, rightMotorSpeed);
		}

		/***************************************************************************************************************/
		//WRIST MOTOR
		stickDValue		= getJoystickValue(ChD);
		if ((stickDValue <= 15) && (stickDValue >= -15) ) stickDValue = 0;

		wristMotorSpeed = stickDValue;

		if (wristMotorSpeed >=0 ) 	 	wristMotorSpeed = (wristMotorSpeed / 10) * (wristMotorSpeed / 10);
		else wristMotorSpeed = 		- (wristMotorSpeed / 10) * (wristMotorSpeed / 10);

		globalWristPosition = getMotorEncoder(wristMotor);

		if ((wristMotorSpeed > 0) && (globalWristPosition >= WRIST_UPPER_LIMIT)){
			setMotorTarget(wristMotor, WRIST_UPPER_LIMIT, 70);
		}
		else if ((wristMotorSpeed < 0) && (globalWristPosition <= WRIST_LOWER_LIMIT)) {
			setMotorTarget(wristMotor, WRIST_LOWER_LIMIT, 70);
		}
		else {
			//slow things down if we are near the limit of wrist movement
			if ( abs(globalWristPosition - WRIST_LOWER_LIMIT) < 10) {
				wristMotorSpeed = wristMotorSpeed / 4;
			}
			if ( abs(globalWristPosition - WRIST_UPPER_LIMIT) < 10) {
				wristMotorSpeed = wristMotorSpeed / 4;
			}
			setMotorSpeed(wristMotor, wristMotorSpeed );
		}

		/***************************************************************************************************************/
		//ARM MOTOR
		stickCValue		= getJoystickValue(ChC);
		if ((stickCValue <= 15) && (stickCValue >= -15) ) stickCValue = 0;

		armMotorSpeed = stickCValue;

		if (armMotorSpeed >=0 ) 	 	armMotorSpeed = (armMotorSpeed / 10) * (armMotorSpeed / 10);
		else armMotorSpeed = 		- (armMotorSpeed / 10) * (armMotorSpeed / 10);

		globalArmPosition = getMotorEncoder(armMotor);

		if ((armMotorSpeed > 0) && (globalArmPosition >= ARM_UPPER_LIMIT)){
			setMotorTarget(armMotor, ARM_UPPER_LIMIT, 70);
		}
		else if ((armMotorSpeed < 0) && (globalArmPosition <= ARM_LOWER_LIMIT)) {
			setMotorTarget(armMotor, ARM_LOWER_LIMIT, 70);
		}
		else {
			//slow things down if we are near the limit of arm movement
			if ( abs(globalArmPosition - ARM_LOWER_LIMIT) < 10){
				armMotorSpeed= armMotorSpeed/ 4;
			}
			if ( abs(globalArmPosition - ARM_UPPER_LIMIT) < 10){
				armMotorSpeed= armMotorSpeed/ 4;
			}
			setMotorSpeed(armMotor, armMotorSpeed);
		}


		/***************************************************************************************************************/
		//CLAW MOTOR
		if(getJoystickValue(BtnLUp) > 0)   {  //CLOSE
			setMotorSpeed(clawMotor, 70);
		}
		else if(getJoystickValue(BtnLDown) > 0)   {  //OPEN
			globalClawPosition = getMotorEncoder(clawMotor);
			if (globalClawPosition <= -87) {
				setMotorTarget(clawMotor, -87, 70);
			}
			else {
				setMotorSpeed(clawMotor, -70);
			}
		}
		else {
			globalClawPosition = getMotorEncoder(clawMotor);
			setMotorTarget(clawMotor, globalClawPosition, 90);
		}


		/***************************************************************************************************************/
		//Buttons to move our Arm for Us quickly to other positions
		if(getJoystickValue(BtnFUp) > 0)   {  //UP
			moveToAimingPosition();
		}
		if(getJoystickValue(BtnFDown) > 0)   {  //GRAB a Block
			moveToNeutralPosition();
		}

		/***************************************************************************************************************/
		//Block Stacking Positions
		//Buttons to move our Arm for Us quickly to other positions
		if(getJoystickValue(BtnRUp) > 0)   {  //Upper Stacking Position
			moveToUpperStackingPosition();
		}
		if(getJoystickValue(BtnRDown) > 0)   {  //Lower Stacking Position
			moveToLowerStackingPosition();
		}
		wait1Msec(50);   // Give actions time to make progress and prevent over-control from taking inputs too fast

	}

}
Example #20
0
void setTarget(int modeIn, int maxTimeIn, long leftTargetIn, long rightTargetIn, unsigned int leftSpeedIn, unsigned int rightSpeedIn) {
	// Note: If you're moving less than 1000 counts, set a lower speed than 100

	mode = modeIn; // Define global mode
	maxTime = maxTimeIn; //All modes accept maxTime

	leftSpeed = leftSpeedIn; // These are unused in most instances but set them anyway
	rightSpeed = rightSpeedIn;

	completed = false; // Tell loop to reset whatever neccesary

	if (!gyroEnabled() && mode==GYRO_DEGREES) mode=TURN_DEGREES; // If gyro is disabled switch to a raw turn
	if (!gyroEnabled() && mode==ARC_GYRO_DEGREES) mode=ARC_DEGREES; //ditto

	if (mode==RAW_MODE) {
		leftTarget = leftTargetIn;
		rightTarget = rightTargetIn;
	} else if (mode==DRIVE_INCHES) {
		leftTarget = leftTargetIn*INCH;     //Set position times inch constant
		rightTarget = rightTargetIn*INCH;
		mode = RAW_MODE;                    // Inches have been converted; treat as raw
	} else if (mode==TURN_DEGREES) {
		rightTarget = leftTargetIn*DEGREES;  // Also define approx motor counts incase fallback is needed
		leftTarget = rightTarget*-1;        // Negative power for other side
		leftSpeed = AUTON_TURN_SPEED;
		rightSpeed = AUTON_TURN_SPEED;
		mode = RAW_MODE;                    // Degrees have been converted; treat as raw
	} else if (mode==GYRO_DEGREES) {
		setGyroPos(false);
		resetGyro();gAngle=0;
		leftTarget = leftTargetIn;          // Assign degrees for gyro
	} else if (mode==RAISE_ARM) {
		armState = ARM_RAISED;               // Set arm position
	} else if (mode==LOWER_ARM) {
		armState = -100;
	} else if (mode==ELEVATE_ARM) {
		armState = ARM_ELEVATED;
	} else if (mode==OPEN_CLAMP) {
		clampState = false;         // Set clamp position
	} else if (mode==CLOSE_CLAMP) {
		clampState = true;
	} else if (mode==DRIVE_UP_RAMP) {
		int sel = selection[RAMP_MENU];
		if (sel==0 && gyroEnabled()) {  //Drive until level
			setGyroPos(true); // Run gyro-setting routine
			mode=INTERNAL_RAMP;
		} else {  //Time-based ramp
			leftTarget=-10000;
			rightTarget=-10000; //Unattainably far position for max power
			leftSpeed=100;
			rightSpeed=100;
			mode=RAW_MODE;
			switch(sel) {
				case 1: maxTime=1600; break; //Drive to hump
				case 2: maxTime=1300; break; //Drive to front
				case 3: maxTime=2000; break; //Drive to back
			}
		}
	} else if (mode==ARC_DEGREES) {
		mode = RAW_MODE;
		leftSpeed=100;
		rightSpeed=100;
		if (leftTargetIn != 0) {
			leftTarget=leftTargetIn*DEGREES_ARC;
			rightTarget=0;
		} else { // Right
			leftTarget=0;
			rightTarget=-rightTargetIn*DEGREES_ARC; //note the negative
		}
	} else if (mode==ARC_GYRO_DEGREES) {
		setGyroPos(false);
		resetGyro();
		if (leftTargetIn != 0) {
			leftSpeed=1;
			rightSpeed=0;
			leftTarget = leftTargetIn;
		} else { // Right
			leftSpeed=0;
			rightSpeed=1;
			leftTarget = rightTargetIn;
		}
	} else if (mode==SCOUT_RAMP) {
		// setTarget(SCOUT_RAMP, SCOUT_LEFT or SCOUT_RIGHT, distance behind ramp edge, 0,0,0)
		bool dir = (maxTime==SCOUT_RIGHT);

		int distTo1 = 2400;
		int distTo2 = 5000;

		int sel = selection[SONAR_MENU];
		if (fallbackMode==true) sel=3;

		if (sel==0) {
			findClearRamp((dir ? distanceRight : distanceLeft), leftTargetIn+distTo2);
		} else if (sel==1) {
			findLine(leftTargetIn+distTo2, 1);
		} else if (sel==2) {
			findLine(leftTargetIn+distTo2, 2);
		} else if (sel==3) { //Fallback mode
			mode=RAW_MODE;
			maxTime=3000;
			leftTarget =  -3200;
			rightTarget = -3200;
			leftSpeed = 100;
			rightSpeed = 100;
		}
	}

	time1[T2]=0; // Start clock
	return;
}
//Predefined construct
void pre_auton()
{
	bStopTasksBetweenModes = true;
	resetGyro();
}
Example #22
0
void Drive::resetDrive()
{
    resetDriveEncoders();
    resetGyro();
}
void pre_auton()
{
	setUp();
	resetGyro();
	resetDrive();
}