void positionArm(int position)
{// Positions arm to a specified place
	if (getArmPotentiometer() < position )
	{
		while(getArmPotentiometer() < position && vexRT[Btn8R] == 0)
		{
			raiseArm();
			driveWithJoysticks();
			intakeControl();
		}

		stopArm();
	}
	else if (getArmPotentiometer() > position)
	{
		while (getArmPotentiometer() > position && vexRT[Btn8R] == 0)
		{
			lowerArm();
			driveWithJoysticks();
			intakeControl();
		}

		stopArm();
	}
}
Beispiel #2
0
void armHelixUp (double spikeNumber)
{
	const Encoder TOWER_ENCODER = encoderInit (2,3,false);

	armUpDead(); // power all lift motors to max
	motorSet (SWING_MOTOR, SWING_MOTOR_SPEED);

	bool done =false, swingDone =false, heightDone =false;

	int towerCount=0, swingCount=0;

	while(!done)
	{
		swingCount = analogRead (SWING_POT_PIN); //4095 is 5 volts
		towerCount = encoderGet(TOWER_ENCODER);//documentation claims 360 ticks in 1 revolution

		if (towerCount > SPIKE_OFFSET * spikeNumber)
		{
			stopArm();
			printf("Tower raised, might fall, motor stopped\n");
			heightDone = true;
		}

		if (swingCount > SWING_ANGLE)
		{
			motorSet (SWING_MOTOR, 0);
			printf("Arm swung, may be wrong angle\n");
			swingDone=true;
		}
		done = swingDone && heightDone;
	}
}
Beispiel #3
0
///Move to loading height
void armHelixDown()
{
	motorSet (SWING_MOTOR, -SWING_MOTOR_SPEED);
	armDownDead(); // power all lift motors to max
	bool done =false, swingDone =false, heightDone =false;
	int towerCount=0, swingCount=0;
	const Encoder TOWER_ENCODER = encoderInit (TOWER_ENCODER_TOP_CHANNEL,TOWER_ENCODER_BOTTOM_CHANNEL,false);

	while(!done)
	{
		swingCount = analogRead (SWING_POT_PIN); //4095 is 5 volts
		towerCount = encoderGet(TOWER_ENCODER);//documentation claims 360 ticks in 1 revolution

		if (towerCount < SPIKE_OFFSET*1.25) //1.25 is an estimate for the load height
		{
			stopArm();
			printf("Tower lowered to load position\n");
			heightDone = true;
		}

		if (swingCount < 100) //TODo no measurements for pot yet
		{
			motorSet (SWING_POT_PIN, 0);
			printf("Arm swung, may be wrong angle\n");
			swingDone=true;
		}
		done = swingDone && heightDone;
	}
}
Beispiel #4
0
// Controls the algorithm in determining arm speed
void moveArm(int target) {
	if(target > 0) {
		int tempSpeed = armSpeedPrevious + ((127-armSpeedPrevious)/5);
		powerArm(tempSpeed);
		armSpeedPrevious = tempSpeed;
	} else if (target < 0) {
		int tempSpeed = armSpeedPrevious - ((127+armSpeedPrevious)/5);
		powerArm(tempSpeed);
		armSpeedPrevious = tempSpeed;
	} else {
		stopArm();
	}
}
Beispiel #5
0
// controls the algorithm in determining arm speed
void moveArm(int target) {
	if(target > 0 /*&& armRotation < 1250*/) {
		int tempSpeed = armSpeedPrevious + ((127-armSpeedPrevious)/5);
		powerArm(tempSpeed);
		armSpeedPrevious = tempSpeed;
	} else if (target < 0 && armRotation > 0) {
		int tempSpeed = armSpeedPrevious - ((127+armSpeedPrevious)/5);
		powerArm(tempSpeed);
		armSpeedPrevious = tempSpeed;
	} else {
		stopArm();
	}
}
Beispiel #6
0
void armDown(int x)
{
	int pot = analogRead (8);

		while (pot > x)
		{
			motorSet (motor3, 127) ; // arm down
			motorSet (motor4, -127) ;
			motorSet (motor7, 127) ;
			motorSet (motor8, -127) ;
			pot = analogRead (8);
		}

		stopArm();
}
/*void driveForward(int x) {
	int imeAccumulator = 0;
	imeReset(0); // rest rightLine I.M.E

	//Read decoder into counts
	imeGet(0, &imeAccumulator);

	//Move forward at max speed until desired x
	while (abs(imeAccumulator) < x) {
		motorSet(MOTOR_DRIVE_RIGHT_BACK, FORWARD_VELOCITY);
		motorSet(MOTOR_DRIVE_RIGHT_FRONT, FORWARD_VELOCITY);
		motorSet(MOTOR_ARM_LEFT_BACK, FORWARD_VELOCITY);
		motorSet(MOTOR_ARM_LEFT_FRONT, FORWARD_VELOCITY);

		imeGet(0, &imeAccumulator); // keep getting the value
	}

	//Cancel forward inertia
	motorSet(MOTOR_DRIVE_RIGHT_BACK, -INERTIA_CANCELLATION_FACTOR); // no inertia
	motorSet(MOTOR_DRIVE_RIGHT_FRONT, -INERTIA_CANCELLATION_FACTOR);
	motorSet(MOTOR_ARM_LEFT_BACK, -INERTIA_CANCELLATION_FACTOR);
	motorSet(MOTOR_ARM_LEFT_FRONT, -INERTIA_CANCELLATION_FACTOR);
	delay(65);

	motorSet(MOTOR_DRIVE_RIGHT_BACK, 0);
	motorSet(MOTOR_DRIVE_RIGHT_FRONT, 0);
	motorSet(MOTOR_ARM_LEFT_BACK, 0);
	motorSet(MOTOR_ARM_LEFT_FRONT, 0);
	delay(200);

}

void driveBack(int x) {
	int counts = 0;
	imeReset(0); // rest rightLine I.M.E
	imeGet(0, &counts);

	while (abs(counts) < x) {
		motorSet(MOTOR_DRIVE_RIGHT_BACK, REVERSE_VELOCITY);
		motorSet(MOTOR_DRIVE_RIGHT_FRONT, REVERSE_VELOCITY);
		motorSet(MOTOR_ARM_LEFT_BACK, REVERSE_VELOCITY);
		motorSet(MOTOR_ARM_LEFT_FRONT, REVERSE_VELOCITY);

		imeGet(0, &counts); // keep getting the value
	}

	motorSet(MOTOR_DRIVE_RIGHT_BACK, INERTIA_CANCELLATION_FACTOR); // no inertia
	motorSet(MOTOR_DRIVE_RIGHT_FRONT, INERTIA_CANCELLATION_FACTOR);
	motorSet(MOTOR_ARM_LEFT_BACK, INERTIA_CANCELLATION_FACTOR);
	motorSet(MOTOR_ARM_LEFT_FRONT, INERTIA_CANCELLATION_FACTOR);
	delay(65);

	motorStop(MOTOR_DRIVE_RIGHT_BACK);
	motorStop(MOTOR_DRIVE_RIGHT_FRONT);
	motorStop(MOTOR_ARM_LEFT_BACK);
	motorStop(MOTOR_ARM_LEFT_FRONT);
	delay(200);
}

void turnRight(int x) {
	int counts = 0;
	imeReset(0); // rest rightLine I.M.E
	imeGet(0, &counts);

	while (abs(counts) < x) {
		motorSet(MOTOR_DRIVE_RIGHT_BACK, -64);
		motorSet(MOTOR_DRIVE_RIGHT_FRONT, -64);
		motorSet(MOTOR_ARM_LEFT_BACK, 64);
		motorSet(MOTOR_ARM_LEFT_FRONT, 64);

		imeGet(0, &counts); // keep getting the value
	}

	motorSet(MOTOR_DRIVE_RIGHT_BACK, 10); // no inertia
	motorSet(MOTOR_DRIVE_RIGHT_FRONT, 10);
	motorSet(MOTOR_ARM_LEFT_BACK, -10);
	motorSet(MOTOR_ARM_LEFT_FRONT, -10);
	delay(45);

	motorStop(MOTOR_DRIVE_RIGHT_BACK);
	motorStop(MOTOR_DRIVE_RIGHT_FRONT);
	motorStop(MOTOR_ARM_LEFT_BACK);
	motorStop(MOTOR_ARM_LEFT_FRONT);
	delay(200);
}

void turnLeft(int x) {
	int counts = 0;
	imeReset(0); // rest rightLine I.M.E
	imeGet(0, &counts);

	while (abs(counts) < x) {
		motorSet(MOTOR_DRIVE_RIGHT_BACK, 64);
		motorSet(MOTOR_DRIVE_RIGHT_FRONT, 64);
		motorSet(MOTOR_ARM_LEFT_BACK, -64);
		motorSet(MOTOR_ARM_LEFT_FRONT, -64);

		imeGet(0, &counts); // keep getting the value
	}

	motorSet(MOTOR_DRIVE_RIGHT_BACK, -10); // no inertia
	motorSet(MOTOR_DRIVE_RIGHT_FRONT, -10);
	motorSet(MOTOR_ARM_LEFT_BACK, 10);
	motorSet(MOTOR_ARM_LEFT_FRONT, 10);
	delay(45);

	motorStop(MOTOR_DRIVE_RIGHT_BACK);
	motorStop(MOTOR_DRIVE_RIGHT_FRONT);
	motorStop(MOTOR_ARM_LEFT_BACK);
	motorStop(MOTOR_ARM_LEFT_FRONT);
	delay(200);
}

void turnRightSlowDead() {
	motorSet(MOTOR_DRIVE_RIGHT_BACK, -50);
	motorSet(MOTOR_DRIVE_RIGHT_FRONT, -50);
	motorSet(MOTOR_ARM_LEFT_BACK, 50);
	motorSet(MOTOR_ARM_LEFT_FRONT, 50);
}

void turnLeftSlowDead() {
	motorSet(MOTOR_DRIVE_RIGHT_BACK, 40);
	motorSet(MOTOR_DRIVE_RIGHT_FRONT, 40);
	motorSet(MOTOR_ARM_LEFT_BACK, -40);
	motorSet(MOTOR_ARM_LEFT_FRONT, -40);
}

void turnLeftArc(int x) {
	int counts = 0;
	imeReset(0); // rest rightLine I.M.E
	imeGet(0, &counts);

	while (abs(counts) < x) {
		motorSet(MOTOR_DRIVE_RIGHT_BACK, 127);
		motorSet(MOTOR_DRIVE_RIGHT_FRONT, 127);
		motorSet(MOTOR_ARM_LEFT_BACK, 0);
		motorSet(MOTOR_ARM_LEFT_FRONT, 0);

		imeGet(0, &counts); // keep getting the value
	}
	motorStop(MOTOR_DRIVE_RIGHT_BACK);
	motorStop(MOTOR_DRIVE_RIGHT_FRONT);
	motorStop(MOTOR_ARM_LEFT_BACK);
	motorStop(MOTOR_ARM_LEFT_FRONT);
	delay(200);
}

void turnRightArc(int x) {
	int counts = 1;
	imeReset(1); // rest rightLine I.M.E
	imeGet(1, &counts);

	while (abs(counts) < x) {
		motorSet(MOTOR_DRIVE_RIGHT_BACK, 0);
		motorSet(MOTOR_DRIVE_RIGHT_FRONT, 0);
		motorSet(MOTOR_ARM_LEFT_BACK, 127);
		motorSet(MOTOR_ARM_LEFT_FRONT, 127);

		imeGet(0, &counts); // keep getting the value
	}
	motorStop(MOTOR_DRIVE_RIGHT_BACK);
	motorStop(MOTOR_DRIVE_RIGHT_FRONT);
	motorStop(MOTOR_ARM_LEFT_BACK);
	motorStop(MOTOR_ARM_LEFT_FRONT);

}

void stopDrive() {
	motorStop(MOTOR_DRIVE_RIGHT_BACK);
	motorStop(MOTOR_DRIVE_RIGHT_FRONT);
	motorStop(MOTOR_ARM_LEFT_BACK);
	motorStop(MOTOR_ARM_LEFT_FRONT);
	delay(200);
}

void stopAll() {
	motorStop(MOTOR_DRIVE_RIGHT_BACK);
	motorStop(MOTOR_DRIVE_RIGHT_FRONT);
	motorStop(MOTOR_ARM_RIGHT_BOTTOM);
	motorStop(MOTOR_ARM_RIGHT_TOP);
	motorStop(MOTOR_ARM_RIGHT_MID);
	motorStop(MOTOR_ARM_LEFT_MID);
	motorStop(MOTOR_ARM_LEFT_TOP);
	motorStop(MOTOR_ARM_LEFT_BOTTOM);
	motorStop(MOTOR_ARM_LEFT_FRONT);
	motorStop(MOTOR_ARM_LEFT_BACK);
}

void stopIntake() {
	motorStop(MOTOR_ARM_RIGHT_MID);
	motorStop(MOTOR_ARM_LEFT_MID);
}

void driveForwardDead() {
	motorSet(MOTOR_DRIVE_RIGHT_BACK, 127);
	motorSet(MOTOR_DRIVE_RIGHT_FRONT, 127);
	motorSet(MOTOR_ARM_LEFT_BACK, 127);
	motorSet(MOTOR_ARM_LEFT_FRONT, 127);
}

void driveBackDead() {
	motorSet(MOTOR_DRIVE_RIGHT_BACK, -127);
	motorSet(MOTOR_DRIVE_RIGHT_FRONT, -127);
	motorSet(MOTOR_ARM_LEFT_BACK, -127);
	motorSet(MOTOR_ARM_LEFT_FRONT, -127);
}

void driveForwardSlow(int x) {
	int counts = 0;
	imeReset(0); // rest rightLine I.M.E
	imeGet(0, &counts);

	while (abs(counts) < x) {
		motorSet(MOTOR_DRIVE_RIGHT_BACK, 40);
		motorSet(MOTOR_DRIVE_RIGHT_FRONT, 40);
		motorSet(MOTOR_ARM_LEFT_BACK, 40);
		motorSet(MOTOR_ARM_LEFT_FRONT, 40);

		imeGet(0, &counts); // keep getting the value
	}

	motorSet(MOTOR_DRIVE_RIGHT_BACK, -10); // no inertia
	motorSet(MOTOR_DRIVE_RIGHT_FRONT, -10);
	motorSet(MOTOR_ARM_LEFT_BACK, -10);
	motorSet(MOTOR_ARM_LEFT_FRONT, -10);
	delay(45);

	motorSet(MOTOR_DRIVE_RIGHT_BACK, 0);
	motorSet(MOTOR_DRIVE_RIGHT_FRONT, 0);
	motorSet(MOTOR_ARM_LEFT_BACK, 0);
	motorSet(MOTOR_ARM_LEFT_FRONT, 0);
	delay(200);
}

void driveBackSlow(int x) {
	int counts = 0;
	imeReset(0); // rest rightLine I.M.E
	imeGet(0, &counts);

	while (abs(counts) < x) {
		motorSet(MOTOR_DRIVE_RIGHT_BACK, -40);
		motorSet(MOTOR_DRIVE_RIGHT_FRONT, -40);
		motorSet(MOTOR_ARM_LEFT_BACK, -40);
		motorSet(MOTOR_ARM_LEFT_FRONT, -40);

		imeGet(0, &counts); // keep getting the value
	}

	motorSet(MOTOR_DRIVE_RIGHT_BACK, 10); // no inertia
	motorSet(MOTOR_DRIVE_RIGHT_FRONT, 10);
	motorSet(MOTOR_ARM_LEFT_BACK, 10);
	motorSet(MOTOR_ARM_LEFT_FRONT, 10);
	delay(45);

	motorSet(MOTOR_DRIVE_RIGHT_BACK, 0);
	motorSet(MOTOR_DRIVE_RIGHT_FRONT, 0);
	motorSet(MOTOR_ARM_LEFT_BACK, 0);
	motorSet(MOTOR_ARM_LEFT_FRONT, 0);

	delay(200);
}

void driveForwardSlowDead() {

	motorSet(MOTOR_DRIVE_RIGHT_BACK, 50);
	motorSet(MOTOR_DRIVE_RIGHT_FRONT, 50);
	motorSet(MOTOR_ARM_LEFT_BACK, 50);
	motorSet(MOTOR_ARM_LEFT_FRONT, 50);

}

void driveBackSlowDead() {

	motorSet(MOTOR_DRIVE_RIGHT_BACK, -40);
	motorSet(MOTOR_DRIVE_RIGHT_FRONT, -40);
	motorSet(MOTOR_ARM_LEFT_BACK, -40);
	motorSet(MOTOR_ARM_LEFT_FRONT, -40);

}

void scrapeRight(int x) {
	int counts = 0;
	imeReset(1); // rest left I.M.E
	imeGet(1, &counts);

	while (abs(counts) < x) {
		motorSet(MOTOR_DRIVE_RIGHT_BACK, 0);
		motorSet(MOTOR_DRIVE_RIGHT_FRONT, 0);
		motorSet(MOTOR_ARM_LEFT_BACK, 90);
		motorSet(MOTOR_ARM_LEFT_FRONT, 90);

		imeGet(1, &counts); // keep getting the value
	}

	motorSet(MOTOR_DRIVE_RIGHT_BACK, 0);
	motorSet(MOTOR_DRIVE_RIGHT_FRONT, 0);
	motorSet(MOTOR_ARM_LEFT_BACK, -10);
	motorSet(MOTOR_ARM_LEFT_FRONT, -10);
	delay(55);

	motorSet(MOTOR_DRIVE_RIGHT_BACK, 0);
	motorSet(MOTOR_DRIVE_RIGHT_FRONT, 0);
	motorSet(MOTOR_ARM_LEFT_BACK, 0);
	motorSet(MOTOR_ARM_LEFT_FRONT, 0);
	delay(200);
}

void scrapeLeft(int x) {
	int counts = 0;
	imeReset(0); // rest rightLine I.M.E
	imeGet(0, &counts);

	while (abs(counts) < x) {
		motorSet(MOTOR_DRIVE_RIGHT_BACK, 90);
		motorSet(MOTOR_DRIVE_RIGHT_FRONT, 90);
		motorSet(MOTOR_ARM_LEFT_BACK, 0);
		motorSet(MOTOR_ARM_LEFT_FRONT, 0);

		imeGet(0, &counts); // keep getting the value
	}

	motorSet(MOTOR_DRIVE_RIGHT_BACK, -10); // no intertia
	motorSet(MOTOR_DRIVE_RIGHT_FRONT, -10);
	motorSet(MOTOR_ARM_LEFT_BACK, 0);
	motorSet(MOTOR_ARM_LEFT_FRONT, 0);
	delay(55);

	motorSet(MOTOR_DRIVE_RIGHT_BACK, 0);
	motorSet(MOTOR_DRIVE_RIGHT_FRONT, 0);
	motorSet(MOTOR_ARM_LEFT_BACK, 0);
	motorSet(MOTOR_ARM_LEFT_FRONT, 0);
	delay(200);
}

void scrapeLeftBack(int x) {
	int counts = 0;
	imeReset(0); // rest left I.M.E
	imeGet(0, &counts);

	while (abs(counts) < x) {
		motorSet(MOTOR_DRIVE_RIGHT_BACK, -90);
		motorSet(MOTOR_DRIVE_RIGHT_FRONT, -90);
		motorSet(MOTOR_ARM_LEFT_BACK, 0);
		motorSet(MOTOR_ARM_LEFT_FRONT, 0);

		imeGet(0, &counts); // keep getting the value
	}

	motorSet(MOTOR_DRIVE_RIGHT_BACK, 10); // no intertia
	motorSet(MOTOR_DRIVE_RIGHT_FRONT, 10);
	motorSet(MOTOR_ARM_LEFT_BACK, 0);
	motorSet(MOTOR_ARM_LEFT_FRONT, 0);
	delay(55);

	motorSet(MOTOR_DRIVE_RIGHT_BACK, 0);
	motorSet(MOTOR_DRIVE_RIGHT_FRONT, 0);
	motorSet(MOTOR_ARM_LEFT_BACK, 0);
	motorSet(MOTOR_ARM_LEFT_FRONT, 0);
	delay(200);


}

void scrapeRightBack(int x) {
	int counts = 0;
	imeReset(1); // rest rightLine I.M.E
	imeGet(1, &counts);

	while (abs(counts) < x) {
		motorSet(MOTOR_DRIVE_RIGHT_BACK, 0);
		motorSet(MOTOR_DRIVE_RIGHT_FRONT, 0);
		motorSet(MOTOR_ARM_LEFT_BACK, -90);
		motorSet(MOTOR_ARM_LEFT_FRONT, -90);

		imeGet(1, &counts); // keep getting the value
	}

	motorSet(MOTOR_DRIVE_RIGHT_BACK, 0); // no intertia
	motorSet(MOTOR_DRIVE_RIGHT_FRONT, 0);
	motorSet(MOTOR_ARM_LEFT_BACK, 10);
	motorSet(MOTOR_ARM_LEFT_FRONT, 10);
	delay(55);

	motorSet(MOTOR_DRIVE_RIGHT_BACK, 0);
	motorSet(MOTOR_DRIVE_RIGHT_FRONT, 0);
	motorSet(MOTOR_ARM_LEFT_BACK, 0);
	motorSet(MOTOR_ARM_LEFT_FRONT, 0);
	delay(200);

}

void forwardDetect() {
	int white = 1300;
	int leftLine = analogRead(1);
	int midLine = analogRead(2);
	int rightLine = analogRead(3);

	while (midLine > white) // drive till hit white line
	{
		motorSet(MOTOR_DRIVE_RIGHT_BACK, 60);
		motorSet(MOTOR_DRIVE_RIGHT_FRONT, 60);
		motorSet(MOTOR_ARM_LEFT_BACK, 60);
		motorSet(MOTOR_ARM_LEFT_FRONT, 60);

		midLine = analogRead(2);
	}

	motorSet(MOTOR_DRIVE_RIGHT_BACK, -10); // no intertia
	motorSet(MOTOR_DRIVE_RIGHT_FRONT, -10);
	motorSet(MOTOR_ARM_LEFT_BACK, -10);
	motorSet(MOTOR_ARM_LEFT_FRONT, -10);
	delay(55);

	motorSet(MOTOR_DRIVE_RIGHT_BACK, 0);
	motorSet(MOTOR_DRIVE_RIGHT_FRONT, 0);
	motorSet(MOTOR_ARM_LEFT_BACK, 0);
	motorSet(MOTOR_ARM_LEFT_FRONT, 0);
	delay(200);
}

void backDetect() {
	int white = 2000;
	int leftLine = analogRead(1);
	int midLine = analogRead(2);
	int rightLine = analogRead(3);

	while (midLine > white) // drive till hit white line
	{
		motorSet(MOTOR_DRIVE_RIGHT_BACK, -60);
		motorSet(MOTOR_DRIVE_RIGHT_FRONT, -60);
		motorSet(MOTOR_ARM_LEFT_BACK, -60);
		motorSet(MOTOR_ARM_LEFT_FRONT, -60);

		midLine = analogRead(2);
	}

	motorSet(MOTOR_DRIVE_RIGHT_BACK, 10); // no intertia
	motorSet(MOTOR_DRIVE_RIGHT_FRONT, 10);
	motorSet(MOTOR_ARM_LEFT_BACK, 10);
	motorSet(MOTOR_ARM_LEFT_FRONT, 10);
	delay(55);

	motorSet(MOTOR_DRIVE_RIGHT_BACK, 0);
	motorSet(MOTOR_DRIVE_RIGHT_FRONT, 0);
	motorSet(MOTOR_ARM_LEFT_BACK, 0);
	motorSet(MOTOR_ARM_LEFT_FRONT, 0);
	delay(200);

}
*/
void armUp(int x) {
	int pot = encoderGet(encoder2);

	while (pot < x) {
		motorSet(MOTOR_ARM_RIGHT_TOP, -127);
		motorSet(MOTOR_ARM_LEFT_TOP, 127);
		motorSet(MOTOR_ARM_RIGHT_BOTTOM, -127);
		motorSet(MOTOR_ARM_LEFT_BOTTOM, 127);
		motorSet(MOTOR_ARM_RIGHT_MID, -127);
		motorSet(MOTOR_ARM_LEFT_MID, 127);
		pot = encoderGet(encoder2);
	}
	stopArm();
	//armUpTrim();
	delay(300);
}
Beispiel #8
0
void armUp(int x)
{
	int pot = analogRead (8);

	while (pot < x)
	{
		motorSet (motor3, -127) ; // arm up
		motorSet (motor4, 127) ;
		motorSet (motor7, -127) ;
		motorSet (motor8, 127) ;
		pot = analogRead (8);
	}

	stopArm();
	delay (1000);
}
Beispiel #9
0
void armUpEnc(int x)
{

	int towerCount = encoderGet(TOWER_ENCODER);//documentation claims 360 ticks in 1 revolution
		printf("Height stop: %d > %d \n\r ",towerCount,x);

	while (towerCount > x)
	{
		motorSet(ARM_MOTOR3, MOTOR_MAX); // arm up
		motorSet(ARM_MOTOR4, MOTOR_MAX);
		motorSet(ARM_MOTOR7, MOTOR_MAX);
		motorSet(ARM_MOTOR8, MOTOR_MAX);

		towerCount = encoderGet(TOWER_ENCODER);
	}
	stopArm();

	armUpTrim();
	delay (300);
}
Beispiel #10
0
///Move to loading height
void armHelixDown()
{
	printf("Helix Down\r\n");
	motorSet (SWING_MOTOR, -SWING_MOTOR_SPEED);
	armDownDead(); // power all lift motors to max

	//Swingdone is true for testing, set it back for production
	bool done =false, swingDone =true, heightDone =false;
	int towerCount=0, swingCount=0;


	while(!done)
	{
		swingCount = analogRead (SWING_POT_PIN); //4095 is 5 volts
		towerCount = encoderGet(TOWER_ENCODER);//documentation claims 360 ticks in 1 revolution

		//Should not be quite zero
		int threshold = SPIKE_OFFSET*0.01;


		printf("Height stop: %d < %d \n\r ",towerCount,threshold);

		if (towerCount < threshold) //1.25 is an estimate for the load height
		{
			stopArm();
			printf("Tower lowered to load position\n\r");
			heightDone = true;
#if DEBUG
			delay(5000);
#endif
		}

//		if (swingCount < 100) //TODo no measurements for pot yet
//		{
//			motorSet (SWING_POT_PIN, 0);
//			printf("Arm swung, may be wrong angle\n");
//			swingDone=true;
//		}
		done = swingDone && heightDone;
	}
}
Beispiel #11
0
void armHelixUpEnc (int enc, int potTargetValue)
{
	//moved to init once
	//const Encoder TOWER_ENCODER = encoderInit (2,3,false);
	//TOWER_ENCODER = encoderInit (TOWER_ENCODER_TOP_CHANNEL,TOWER_ENCODER_BOTTOM_CHANNEL,false);
	armUpDead(); // power all lift motors to max
	motorSet (SWING_MOTOR, SWING_MOTOR_SPEED);

	//TODO hack, set wingDone true
	bool done =false, swingDone =true, heightDone =false;

	int towerCount=0, swingCount=0;
	printf("Arm Helix up \n\r");
	int threshold =enc;
	while(!done)
	{
		swingCount = analogRead (SWING_POT_PIN); //4095 is 5 volts
		towerCount = encoderGet(TOWER_ENCODER);//documentation claims 360 ticks in 1 revolution
		printf("Height stop: %d > %d \n\r ",towerCount,threshold);

		if (towerCount > threshold)
		{
			stopArm();
			printf("Tower raised, might fall, motor stopped\n\r");
			heightDone = true;
		}

//		if (swingCount > potTargetValue)
//		{
//			motorSet (SWING_MOTOR, 0);
//			printf("Arm swung, may be wrong angle\n");
//			swingDone=true;
//		}
		done = swingDone && heightDone;
	}
#if DEBUG
	delay(5000);
#endif
}
Beispiel #12
0
void lowerArm()
{
	// Arm lowers
	// Speed decreases as we near the bottom.
	if(getArmPotentiometer() > Arm_SlowedHeight)
	{
		motor[rightArm1] = -Arm_DownSpeed;
		motor[rightArm2] = -Arm_DownSpeed;
		motor[leftArm1]  = -Arm_DownSpeed;
		motor[leftArm2]  = -Arm_DownSpeed;
	}
	else if(getArmPotentiometer() > Arm_MinHeight)
	{
		motor[rightArm1] = -Arm_SlowedSpeed;
		motor[rightArm2] = -Arm_SlowedSpeed;
		motor[leftArm1]  = -Arm_SlowedSpeed;
		motor[leftArm2]  = -Arm_SlowedSpeed;
	}
	else
	{
		stopArm();
	}
}
Beispiel #13
0
void raiseArm()
{
	int error = getArmSeperation();

	if(getArmPotentiometer() < Arm_MaxHeight)
	{
		if(abs(error) > 0)
		{
			if(error > 0) // Left arm is higher than the right arm
			{
				motor[rightArm1] = Arm_NormalSpeed;
				motor[rightArm2] = Arm_NormalSpeed;
				motor[leftArm1]  = Arm_NormalSpeed - 20;
				motor[leftArm2]  = Arm_NormalSpeed - 20;
			}
			else if(error < 0) // Right arm is higher than the left arm
			{
				motor[rightArm1] = Arm_NormalSpeed - 20;
				motor[rightArm2] = Arm_NormalSpeed - 20;
				motor[leftArm1]  = Arm_NormalSpeed;
				motor[leftArm2]  = Arm_NormalSpeed;
			}
		}
		else // The difference between arm heights is negligible
		{
			motor[rightArm1] = Arm_NormalSpeed;
			motor[rightArm2] = Arm_NormalSpeed;
			motor[leftArm1]  = Arm_NormalSpeed;
			motor[leftArm2]  = Arm_NormalSpeed;
		}
	}
	else
	{
		stopArm();
	}
}
Beispiel #14
0
void kakitRed()
{

// variables
	int armMin = 300;
	int wallHeight = 1000;
	int goalHeight = 1650;

	int pot = analogRead(8);

	int encoder00 = 250; // back up abit to row
	int encoder0 = 950; // turn 360 degrees, knock off buckys, face line
	int encoder1 = 162; // rotate towards 2 buckys
	int encoder2 = 150; // drive towards buckys
	int encoder3 = 400; // back up to bump
	int encoder4 = 200; // back up towards bridge
	int encoder5 = 360; // rotate 180 degrees to the large ball
	int encoder6 = 900; // go under bridge and knock out large ball
	int encoder7 = 90; // turn to goal
	int encoder8 = 200; // drive to goal
	int encoder9 = 75;

	// begin routine
	intake(300);
	armDownTrim();

	driveForwardDead(); //ram big balls
	delay(1500);
	stopDrive();
	delay(500);

	driveBack(encoder00); // back up to row abit
	intakeDead();

	turnLeft(encoder0); // turn 360 degrees

	driveBackDead(); // drive back + wall align
	delay(1300);
	stopDrive();
	delay(500);

	turnRight(encoder1); // turn to two buckys
	intakeDead();

	followLine(300); // make sure ur straight

	driveForwardSlowDead(); // drive towards buckys
	delay(500);
	stopDrive();
	delay(600);

	driveForwardDead(); //get the 2nd ball
	delay(200);
	stopDrive();
	delay(600);

	stopIntake();

	driveBack(encoder3); // back up to bump

	armUpDead(); // armup
	delay(50);
	stopArm();
	stopIntake();

	driveBackSlowDead(); // allign the bump
	delay(300);
	stopDrive();
	delay(500);

	driveBackDead();  // over the bump
	delay(1000);
	stopDrive();
	delay(500);

	driveForwardSlowDead(); // alighn to bump
	delay(800);
	stopDrive();
	delay(500);

	driveBackSlow(encoder4); // back up towards bridge
	turnLeft(encoder5); // rotate 180 degrees to the large ball
	armDown(armMin);
	armDownTrim();

	driveForward(encoder6); // go under the bridge + knock large ball
	armUp(goalHeight);

	turnRight(encoder7); // turn to goal

	findLineRight();
	followLine(300);

	driveForwardSlowDead(); // drive to goal
	delay(700);
	stopDrive();
	outtake(7000); // outtake 3

	stopAll();

}
Beispiel #15
0
task usercontrol()
{
	while(true)
	{
		driveWithJoysticks();
		intakeControl();

		if(vexRT[Btn6U]== 1 && vexRT[Btn6D] == 0 && vexRT[Btn8U]== 0 && vexRT[Btn8D]== 0)
		{
			raiseArm();
		}
		else if(vexRT[Btn6U] == 0 && vexRT[Btn6D] == 1 && vexRT[Btn8U] == 0 && vexRT[Btn8D] == 0 && vexRT[Btn8R] == 0)
		{
			lowerArm();
		}
		else if(vexRT[Btn6U] == 0 && vexRT[Btn6D] == 0 && vexRT[Btn8U] == 1 && vexRT[Btn8D] == 0 && vexRT[Btn8R] == 0)
		{
			positionArm(Arm_ScoreHeight);
		}
		else if(vexRT[Btn6U] == 0 && vexRT[Btn6D] == 0 && vexRT[Btn8U] == 0 && vexRT[Btn8D] == 1 && vexRT[Btn8R] == 0)
		{
			positionArm(Arm_MinHeight);
		}
		else
		{
			stopArm();
		}

		if(vexRT[Btn7U] == 1)
		{
			if(btn7UPressed != 1)
			{
				currentIntakeSpeedStep++;

				if(currentIntakeSpeedStep > 2)
				{
					currentIntakeSpeedStep = 2;
				}

				btn7DPressed = true;
			}
		}
		else
		{
			btn7UPressed = false;
		}

		if(vexRT[Btn7D] == 1)
		{
			if(btn7DPressed != 1)
			{
				currentIntakeSpeedStep--;

				if(currentIntakeSpeedStep < 1)
				{
					currentIntakeSpeedStep = 1;
				}

				btn7DPressed = true;
			}
		}
		else
		{
			btn7DPressed = false;
		}

		switch(currentIntakeSpeedStep)
		{
		case 1:
			currentIntakeSpeed = IntakeSlow;
			break;

		case 2:
			currentIntakeSpeed = IntakeFast;
			break;

		default:
			currentIntakeSpeed = IntakeFast;
			break;
		}
	}

	while ((vexRT[Btn7D] == 1) || (vexRT[Btn7U] == 1)	|| (vexRT[Btn7R] == 1)	|| (vexRT[Btn7L] == 1))
	{
		// Wait until button is released to go back to single joystick control.
		driveWithJoysticks();
	}
}
Beispiel #16
0
task autonomous()
{
	switch(selectedAuton)
	{
		case Autonomous_LeftScoreMatchLoads:
			{
				clearEncoders();

				motor[intakeTread]  = -127;
				motor[intakeWheel] = -127;

				wait1Msec(700);

				stopIntake();

				positionArm(Arm_ScoreHeight);

				stopArm();

				wait1Msec(1000);

				int leftDest = SensorValue[encoder_Left] + 2200;

				while(SensorValue[encoder_Left] < leftDest)
				{
					driveAtSpeed(60);
				}

				stopDrive();

				motor[intakeTread]  = 127;
				motor[intakeWheel] = 127;

				wait10Msec(200);
			}
			break;

		case Autonomous_RightPickupScoreYellow:
			{
				clearEncoders();

				int leftDest = SensorValue[encoder_Left] + 1850;

				while(SensorValue[encoder_Left] < leftDest)
				{
					driveAtSpeed(50);
				}

				stopDrive();

				motor[intakeTread]  = -127;
				motor[intakeWheel] = -127;

				wait1Msec(1000);

				stopIntake();

				positionArm(Arm_ScoreHeight);

				int rightDest = SensorValue[encoder_Right] - 730;

				while(SensorValue[encoder_Right] > rightDest)
				{
					motor[driveLeftBack] = 38;
					motor[driveRightBack] = 0;
					motor[driveLeftFront] = 38;
					motor[driveRightFront] = 0;
				}

				leftDest = SensorValue[encoder_Left] + 650;

				while(SensorValue[encoder_Left] < leftDest)
				{
					motor[driveLeftBack] = 38;
					motor[driveRightBack] = 40;
					motor[driveLeftFront] = 38;
					motor[driveRightFront] = 40;
				}

				stopDrive();

				motor[intakeTread]  = 127;
				motor[intakeWheel] = 127;

				wait1Msec(1500);

				stopIntake();
			}
			break;

			case Autonomous_ProgrammingSkills:
				{
					clearEncoders();

					int leftDest = SensorValue[encoder_Left] + 1850;

					while(SensorValue[encoder_Left] < leftDest)
					{
						driveAtSpeed(50);
					}

					stopDrive();

					motor[intakeTread]  = -127;
					motor[intakeWheel] = -127;

					wait1Msec(1000);

					stopIntake();

					positionArm(Arm_ScoreHeight);

					int rightDest = SensorValue[encoder_Right] - 730;

					while(SensorValue[encoder_Right] > rightDest)
					{
						motor[driveLeftBack] = 38;
						motor[driveRightBack] = 0;
						motor[driveLeftFront] = 38;
						motor[driveRightFront] = 0;
					}

					leftDest = SensorValue[encoder_Left] + 650;

					while(SensorValue[encoder_Left] < leftDest)
					{
						driveAtSpeed(40);
					}

					stopDrive();

					motor[intakeTread] = 127;
					motor[intakeWheel] = 127;

					wait1Msec(500);

					stopIntake();

					leftDest = SensorValue[encoder_Left] - 390;

					while(SensorValue[encoder_Left] > leftDest)
					{
						driveAtSpeed(-30);
					}

					stopDrive();

					positionArm(990);

					stopArm();

					rightDest = SensorValue[encoder_Right] + 660;

					while(SensorValue[encoder_Right] < rightDest)
					{
						motor[driveLeftBack] = -30;
						motor[driveLeftFront] = -30;
					}

					stopDrive();

					leftDest = SensorValue[encoder_Left] - 2050;

					while(SensorValue[encoder_Left] > leftDest)
					{
						driveAtSpeed(-50);
					}

					stopDrive();
				}
				break;

			case Autonomous_None:
			default:
				{
				}
				break;
	}
}