Ejemplo n.º 1
0
void reset_op()
{
	encoderReset(rf_encoder);
	encoderReset(lf_encoder);
	encoderReset(rb_encoder);
	encoderReset(lb_encoder);
}
Ejemplo n.º 2
0
int gofor(int llen, int rlen, int turn, int forward) {
	encoderReset(r_encoder);
	encoderReset(l_encoder);
	int ol=0;
	int or=0;
	dstat=0;
	printf("going for %d\t%d degrees with dir %d\t%d\n\r",llen,rlen,turn,forward);
#ifndef SIM
	while((abs(encoderGet(l_encoder))<llen &&abs(encoderGet(r_encoder))<rlen)){
		c_status("gofor",llen);
		controldrive(turn,forward);
		simtank(&ltank,encoderGet(l_encoder-ol)-ol,encoderGet(r_encoder)-or);
		ol=encoderGet(l_encoder);
		or=encoderGet(r_encoder);
		delay(20);
	}
#endif
	dstat=1;
	controldrive(0,0);
	printf("Motion complete, expected:");
	printpos(&ctank);
	printf("\tintegrated: ");
	printpos(&ltank);
	printf("\n\r");
	return encoderGet(l_encoder)>encoderGet(l_encoder) ? encoderGet(l_encoder) : encoderGet(r_encoder);
}
Ejemplo n.º 3
0
void driveForward(int encoder_value){
	while((encoderGet(frontLeftEncoder)+ encoderGet(frontRightEncoder))/2 < encoder_value){
		motorSet(front_left_drive,  -127);
		motorSet(front_right_drive,   127);
		motorSet(back_left_drive,   -127);
		motorSet(back_right_drive,   -127);
	}
	encoderReset(frontLeftEncoder);
	encoderReset(frontRightEncoder);
}
Ejemplo n.º 4
0
void turnLeft(int encoder_value){
	while(((-1*encoderGet(frontLeftEncoder)) +  encoderGet(frontRightEncoder))/2 > encoder_value){
		motorSet(front_left_drive,  127);
		motorSet(front_right_drive,   127);
		motorSet(back_left_drive,   127);
		motorSet(back_right_drive,  - 127);
	}
	encoderReset(frontLeftEncoder);
	encoderReset(frontRightEncoder);
}
Ejemplo n.º 5
0
void armbotRed()
{
	encoderReset(TOWER_ENCODER);
	printf("ENCODER %x reset\n\r",TOWER_ENCODER);

	int stackRot = 3360;
	int loadRot = 100;
	int loadHeight = 70;
	int loadHeightHigh = 376 +30; //30 is error
	int wallHeightU = loadHeightHigh; // idk
	int wallHeight = loadHeight; // idk
	int spike1 = -6; // heights for the individual spikes!
	int spike2 = 450; //n*(offset+ drift)
	//150 = 450-300 release height
	int spike3 = 800;
	int spike4 = 0;
	int spike5 = 0;
	int spike6 = 0;


	int pot = analogRead (8);
	int wall = 980; //pot value

	int stack = 3360; // pot value
	int center = (stack-wall)/2;

/////////////////////////////////////spike 1//////////////////////////////////////////

	armUpEnc(wallHeight);
	turnRightSlow(wall);
	intake(300); //clamp it!
	armUpEnc(wallHeightU); // pick it up
	turnLeftSlow(stack); // rotate to stack
	armDownEnc(spike1); //score the spike!
	outtake(300);
	armUpEnc(spike1 + 500);
	turnRightSlow(center);

	// and loop!/////////////////////spike 2//////////////////////////

	armUpEnc(wallHeightU);
	turnRightSlow(wall);
	intake(300); //clamp it!
	armUpEnc(spike2 + 100); // pick it up + go abit higher than spike height!
	turnLeftSlow(stack); // rotate to stack
	delay(500);  // wait for swing to stop!
	armDownEnc(spike1); //score the spike!
	outtake(300);
	armUpEnc(spike2 + 500);
	turnRightSlow(center);




	//end of routine
	stopAll () ;
	delay(60000);///////////////////////////////////////////////////////////////////////////////////


}
Ejemplo n.º 6
0
void test_init() {
	TEST_ASSERT_EQUAL(0, encoderState(encoder));
	TEST_ASSERT_EQUAL(0, getEncoderError(encoder).errors);
	TEST_ASSERT_EQUAL(0, getEncoderError(encoder).errorMask);
	encoderReset(encoder, 100);
	TEST_ASSERT_EQUAL(100, encoderState(encoder));
}
Ejemplo n.º 7
0
/*
 * Runs the user operator control code. This function will be started in its own task with the
 * default priority and stack size whenever the robot is enabled via the Field Management System
 * or the VEX Competition Switch in the operator control mode. If the robot is disabled or
 * communications is lost, the operator control task will be stopped by the kernel. Re-enabling
 * the robot will restart the task, not resume it from where it left off.
 *
 * If no VEX Competition Switch or Field Management system is plugged in, the VEX Cortex will
 * run the operator control task. Be warned that this will also occur if the VEX Cortex is
 * tethered directly to a computer via the USB A to A cable without any VEX Joystick attached.
 *
 * Code running in this task can take almost any action, as the VEX Joystick is available and
 * the scheduler is operational. However, proper use of delay() or taskDelayUntil() is highly
 * recommended to give other tasks (including system tasks such as updating LCDs) time to run.
 *
 * This task should never exit; it should end with some kind of infinite loop, even if empty.
 */
void operatorControl() {
	encoderReset(encoder);
	while (1) {
		int distance = encoderGet(encoder);
		printf("%d\n\r", distance);
		delay(20);
	}
}
Ejemplo n.º 8
0
void flywheelReset(Flywheel *flywheel)
{
	flywheel->derivative = 0.0f;
	flywheel->integral = 0.0f;
	flywheel->error = 0.0f;
	flywheel->action = 0.0f;
	flywheel->lastAction = 0.0f;
	flywheel->lastError = 0.0f;
	flywheel->firstCross = true;
	flywheel->reading = 0;
	encoderReset(flywheel->encoder);
}
Ejemplo n.º 9
0
void autonomous() {
	Encoder leftEnc; //Sets encoder variable
	Encoder rightEnc;

	leftEnc = encoderInit(1, 2, 0); //Points to encoders
	rightEnc= encoderInit(3, 4, 0);

	encoderReset(leftEnc); //Resets Encoder
	encoderReset(rightEnc);

	int leftVal; //Initializes value variable
	int rightVal;

	leftVal = encoderGet(leftEnc); //Gets initial value for encoder
	rightVal= encoderGet(rightEnc);

	motorSet(1, 127); //Motors Forward
	motorSet(2, 127);
	motorSet(9, -127);
	motorSet(10, 127);

	while(leftVal <= 360 && rightVal <= 360) {

		leftVal = encoderGet(leftEnc);  //Gets encoder value
		rightVal= encoderGet(rightEnc);

	}

	motorSet(1, 0); //Stops Motors
	motorSet(2, 0);
	motorSet(9, 0);
	motorSet(10, 0);






}
Ejemplo n.º 10
0
void calibrate(){

	if (bot == calib){
		wind(joystickGetAnalog(1, 3));

		if (joystickGetDigital(1, 7, JOY_RIGHT)){
			encoderReset(encode);
		}
	}

	int oldval = leftdraw;

	//raise the left shooting value
	while (joystickGetDigital(1, 7, JOY_UP)){
		leftdraw = oldval - 5;
		delay(25);
	}

	//lower left shooting value
	while (joystickGetDigital(1, 7, JOY_DOWN)){
		leftdraw = oldval + 5;
		delay(25);
	}

	oldval = rightdraw;

	//raise right shooting value
	while (joystickGetDigital(1, 8, JOY_UP)){
		rightdraw = oldval + 5;
		delay(25);
	}

	//lower left shooting value
	while (joystickGetDigital(1, 8, JOY_DOWN)){
		rightdraw = oldval - 5;
		delay(25);
	}

	char buf[16];
	sprintf(buf, "R: %d L: %d", rightdraw, leftdraw);
	lcdSetText(uart1, 2, buf);


}
Ejemplo n.º 11
0
void startSensor(Sensor *s) {
	switch (s->type) {
	case SHAFT_ENCODER:
		encoderReset(s->enc);
		if (DEBUG) {
			print("reset encoder");
		}
		break;
	case GYROSCOPE:
		gyroReset(s->gyr);
		break;
	case TIME:
		//resetTimer(s->port, true);
		startTimer(s->port, true);
		break;
	case IME:
		imeReset(s->port);
		break;
	default:
		break;
	}
}
Ejemplo n.º 12
0
void quadEncoderReset(QuadEncoder encoder) {
	encoderReset(encoder.encoder_data);
}
Ejemplo n.º 13
0
/*
 * Runs the user autonomous code. This function will be started in its own task with the default
 * priority and stack size whenever the robot is enabled via the Field Management System or the
 * VEX Competition Switch in the autonomous mode. If the robot is disabled or communications is
 * lost, the autonomous task will be stopped by the kernel. Re-enabling the robot will restart
 * the task, not re-start it from where it leftLine off.
 *
 * Code running in the autonomous task cannot access information from the VEX Joystick. However,
 * the autonomous function can be invoked from another task if a VEX Competition Switch is not
 * available, and it can access joystick information if called in this way.
 *
 * The autonomous task may exit, unlike operatorControl() which should never exit. If it does
 * so, the robot will await a switch to another mode or disable/enable cycle.
 */
void autonomous()
{
	// jumper in = 1
	int jumper1 = digitalRead (1);
	int jumper2 = digitalRead (2);
	int jumper3 = digitalRead (3);
	encoderReset(TOWER_ENCODER);
	if (jumper2 == 1) // this is gordons last sec brute force method cuz theres no time //jumper 2 in
	{// jumper 2 in
		armbotRed();
	}

	AutonomousMode mode = (jumper3) | (jumper2 << 1) | (jumper1 <<2);
	printf("Entered autonomous mode %d\n\r",mode);

	encoderReset(TOWER_ENCODER);
	printf("ENCODER %x reset\n\r",TOWER_ENCODER);
	//jumper 1 = MSB // jumper 3 = LSB
	armbotRed();
	//armTest();
	/*
	switch(mode)
	{
	case BLUE_RESERVED0:  // 0 0 0
		armbotRed();;
		break;

	case BLUE_RESERVED1:  // 0 0 1
		stackEm();
		break;

	case BLUE_RESERVED2:  // 0 1 0
		stackEm();
		break;

	case BLUE_RESERVED3:  // 0 1 0
		//mode3()
		break;
	case RED_RESERVED4:  // 1 0 0
			stackEm();
				break;
	case RED_RESERVED5:  // 1 0 1
			stackEm();
				break;
	case RED_RESERVED6:  // 1 1 0
		stackEm();
				break;

	default:
		printf("Jumpers are set to unkown configuration\n\r");
		stackEm();
		break;
	}
	*/


	// all jumper comands here:
	//1 = out
	// 0 = in

/*
	if ((jumper1 == 1) ) // jumper in 1 = red autonomous
	{
		armbotRed();
	}

	if (jumper2 == 0) // jumper 2 in = blue auto
	{
		//rejectionBlue();
	}
*/

	//allstop
	stopAll();
	delay(600000);



}
Ejemplo n.º 14
0
/*
 * Runs the user operator control code. This function will be started in its own task with the
 * default priority and stack size whenever the robot is enabled via the Field Management System
 * or the VEX Competition Switch in the operator control mode. If the robot is disabled or
 * communications is lost, the operator control task will be stopped by the kernel. Re-enabling
 * the robot will restart the task, not resume it from where it left off.
 *
 * If no VEX Competition Switch or Field Management system is plugged in, the VEX Cortex will
 * run the operator control task. Be warned that this will also occur if the VEX Cortex is
 * tethered directly to a computer via the USB A to A cable without any VEX Joystick attached.
 *
 * Code running in this task can take almost any action, as the VEX Joystick is available and
 * the scheduler is operational. However, proper use of delay() or taskDelayUntil() is highly
 * recommended to give other tasks (including system tasks such as updating LCDs) time to run.
 *
 * This task should never exit; it should end with some kind of infinite loop, even if empty.
 */
void operatorControl() {
	//Config variables 'n stuff
	bool autoM = false;
	bool first = true;
	bool reset = true;
	const int LFRONT=1;
	const int LBACK=2;
	const int RFRONT=3;
	const int RBACK=4;
	const int WINCH1=5;
	const int WINCH2=6;
	const int FLAGDWN=7;
	long startM;
	long endM;
	double circ;
	double MPH;
	//main loop
	while (1) {
		//motorSet(2,50);
		//auto code
		autoM = joystickGetDigital(1, 8, JOY_LEFT);
		if (autoM && first) {
			autonomous();
			first = false;
		}
		//delay(20);
		//Teleop code
		if (joystickGetDigital(1, 7, JOY_UP)) { //Drive straight
			int speed = joystickGetAnalog(1, 3);
			motorSet(LFRONT, speed);
			//delay(5);
			motorSet(LBACK, speed);
			//delay(5);
			motorSet(RFRONT, speed);
			//delay(5);
			motorSet(RBACK, speed);
			//delay(5);
		} else { //tank drive
			int LSpeed = joystickGetAnalog(1, 3);
			int RSpeed = joystickGetAnalog(1, 2);
			motorSet(LFRONT, LSpeed);
			motorSet(LBACK, LSpeed);
			motorSet(RFRONT, RSpeed);
			motorSet(RBACK, RSpeed);
		}
		//Winch 1
		if (joystickGetDigital(1, 5, JOY_UP)) {
			motorSet(WINCH1, 60);
		} else if (joystickGetDigital(1, 5, JOY_DOWN)) {
			motorSet(WINCH1, -60);
		}
		else motorSet(WINCH1, 0);
		//Winch 2
		if (joystickGetDigital(1, 6, JOY_UP)) {
			motorSet(WINCH2, 60);
		} else if (joystickGetDigital(1, 6, JOY_DOWN)) {
			motorSet(WINCH2, -60);
		}
		else motorSet(WINCH2, 0);
		//Flag
		if (joystickGetDigital(1, 8, JOY_DOWN)) {
			motorSet(FLAGDWN, 60);
		}
		else if (joystickGetDigital(1,8, JOY_UP)){
			motorSet(FLAGDWN, -60);
		}
		else motorSet(FLAGDWN, 0);
		//Anemometer
		if(reset){
			startM=micros();
			encoderReset(ameter);
			reset=false;
		}
		long reading = encoderGet(ameter);
		if(reading/360==4){
			endM=micros();
			double revPerSec = (4.0*pow(10.0, 6.0))/((double)(startM-endM));
			double revPerMin = revPerSec*60.0;
			MPH = circ/12.0/5280.0*revPerMin*60.0;
			reset=true;
		}
		//Flush Data

		delay(20);
	}
}