Пример #1
0
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(MOTOR1, FORWARD_VELOCITY);
		motorSet(MOTOR2, FORWARD_VELOCITY);
		motorSet(MOTOR10, FORWARD_VELOCITY);
		motorSet(MOTOR9, FORWARD_VELOCITY);

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

	//Cancel forward inertia
	motorSet(MOTOR1, -INERTIA_CANCELLATION_FACTOR); // no inertia
	motorSet(MOTOR2, -INERTIA_CANCELLATION_FACTOR);
	motorSet(MOTOR10, -INERTIA_CANCELLATION_FACTOR);
	motorSet(MOTOR9, -INERTIA_CANCELLATION_FACTOR);
	delay(65);

	motorSet(MOTOR1, 0);
	motorSet(MOTOR2, 0);
	motorSet(MOTOR10, 0);
	motorSet(MOTOR9, 0);
	delay(200);

}
Пример #2
0
void turnLeft(int x)
{
	int counts = 0;
	imeReset(0); // rest rightLine ime
	imeGet(0, &counts);

	while (abs(counts) < x)
	{
		motorSet (MOTOR_DRIVE_RIGHT_LINE_BACK, MOTOR_TURN_SPEED) ;
		motorSet (MOTOR_DRIVE_RIGHT_LINE_FRONT, MOTOR_TURN_SPEED) ;
		motorSet (MOTOR_DRIVE_LEFT_LINE_BACK, -MOTOR_TURN_SPEED) ;
		motorSet (MOTOR_DRIVE_LEFT_LINE_FRONT, -MOTOR_TURN_SPEED) ;

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

	motorSet (MOTOR_DRIVE_RIGHT_LINE_BACK, -10)	; // no inertia
	motorSet (MOTOR_DRIVE_RIGHT_LINE_FRONT, -10)	;
	motorSet (MOTOR_DRIVE_LEFT_LINE_BACK, 10)	;
	motorSet (MOTOR_DRIVE_LEFT_LINE_FRONT, 10)	;
	delay (45);

	motorStop (MOTOR_DRIVE_RIGHT_LINE_BACK);
	motorStop (MOTOR_DRIVE_RIGHT_LINE_FRONT);
	motorStop (MOTOR_DRIVE_LEFT_LINE_BACK);
	motorStop (MOTOR_DRIVE_LEFT_LINE_FRONT);
	delay (200);
}
Пример #3
0
void driveBack(int x)
{
	int counts = 0;
	imeReset(0); // rest rightLine ime
	imeGet(0, &counts);

	while (abs(counts) < x)
	{
		motorSet (MOTOR_DRIVE_RIGHT_LINE_BACK, REVERSE_DRIVE_SPEED) ;
		motorSet (MOTOR_DRIVE_RIGHT_LINE_FRONT, REVERSE_DRIVE_SPEED) ;
		motorSet (MOTOR_DRIVE_LEFT_LINE_BACK, REVERSE_DRIVE_SPEED) ;
		motorSet (MOTOR_DRIVE_LEFT_LINE_FRONT, REVERSE_DRIVE_SPEED) ;

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

	motorSet (MOTOR_DRIVE_RIGHT_LINE_BACK, 7)	; // no inertia
	motorSet (MOTOR_DRIVE_RIGHT_LINE_FRONT, 7)	;
	motorSet (MOTOR_DRIVE_LEFT_LINE_BACK, 7)	;
	motorSet (MOTOR_DRIVE_LEFT_LINE_FRONT, 7)	;
	delay (65);


	motorStop (MOTOR_DRIVE_RIGHT_LINE_BACK);
	motorStop (MOTOR_DRIVE_RIGHT_LINE_FRONT);
	motorStop (MOTOR_DRIVE_LEFT_LINE_BACK);
	motorStop (MOTOR_DRIVE_LEFT_LINE_FRONT);
	delay (200);
}
Пример #4
0
void scrapeRightBack(int x) {
	int counts = 0;
	imeReset(1); // rest rightLine I.M.E
	imeGet(1, &counts);

	while (abs(counts) < x) {
		motorSet(MOTOR1, 0);
		motorSet(MOTOR2, 0);
		motorSet(MOTOR10, -90);
		motorSet(MOTOR9, -90);

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

	motorSet(MOTOR1, 0); // no intertia
	motorSet(MOTOR2, 0);
	motorSet(MOTOR10, 10);
	motorSet(MOTOR9, 10);
	delay(55);

	motorSet(MOTOR1, 0);
	motorSet(MOTOR2, 0);
	motorSet(MOTOR10, 0);
	motorSet(MOTOR9, 0);
	delay(200);

}
Пример #5
0
void scrapeLeft(int x)
{
	int counts = 0;
	imeReset(0); // rest rightLine ime
	imeGet(0, &counts);

		while (abs(counts) < x)
		    {
		      motorSet (motor1, 90)	;
		      motorSet (motor2,90)	;
		      motorSet (motor10, 0);
		      motorSet (motor9, 0)	;

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

		motorSet (motor1, -10)	; // no intertia
		motorSet (motor2, -10)	;
		motorSet (motor10, 0);
		motorSet (motor9, 0)	;
		delay (55);


		motorSet (motor1, 0)	;
		motorSet (motor2, 0)	;
		motorSet (motor10, 0);
		motorSet (motor9, 0)	;
		delay (200);
}
Пример #6
0
void turnRight(int x) {
	int counts = 0;
	imeReset(0); // rest rightLine I.M.E
	imeGet(0, &counts);

	while (abs(counts) < x) {
		motorSet(MOTOR1, -64);
		motorSet(MOTOR2, -64);
		motorSet(MOTOR10, 64);
		motorSet(MOTOR9, 64);

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

	motorSet(MOTOR1, 10); // no inertia
	motorSet(MOTOR2, 10);
	motorSet(MOTOR10, -10);
	motorSet(MOTOR9, -10);
	delay(45);

	motorStop(MOTOR1);
	motorStop(MOTOR2);
	motorStop(MOTOR10);
	motorStop(MOTOR9);
	delay(200);
}
Пример #7
0
void vTaskPIController() {
	//initialize values
	mutexTake(piSetpointMutex, -1);
	leftMotor.position=0; leftMotor.setpoint=0; leftMotor.output = 0; leftMotor.i_accum = 0;
	rightMotor.position=0; rightMotor.setpoint=0; rightMotor.output = 0; rightMotor.i_accum = 0;
	mutexGive(piSetpointMutex);

	unsigned long taskWakeTime;

	//update PI control every 25ms
	while(1) {
		taskWakeTime = millis();
		// take the mutex
		mutexTake(piSetpointMutex, -1);

		// update odometry
		imeGet(LEFT_IME_ADDR, &(leftMotor.position));
		imeGet(RIGHT_IME_ADDR, &(rightMotor.position));

		// compute control laws
		pi_update(&leftMotor);
		pi_update(&rightMotor);

		// set motor outputs
		motorSet(LEFT_MOTOR_CHANNEL, leftMotor.output);
		motorSet(RIGHT_MOTOR_CHANNEL, rightMotor.output);

		// release mutex
		mutexGive(piSetpointMutex);

		// sleep for 25 milliseconds
		taskDelayUntil(&taskWakeTime, 25);
	}
}
Пример #8
0
void driveBack(int x) {
	int counts = 0;
	imeReset(0); // rest rightLine I.M.E
	imeGet(0, &counts);

	while (abs(counts) < x) {
		motorSet(MOTOR1, REVERSE_VELOCITY);
		motorSet(MOTOR2, REVERSE_VELOCITY);
		motorSet(MOTOR10, REVERSE_VELOCITY);
		motorSet(MOTOR9, REVERSE_VELOCITY);

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

	motorSet(MOTOR1, INERTIA_CANCELLATION_FACTOR); // no inertia
	motorSet(MOTOR2, INERTIA_CANCELLATION_FACTOR);
	motorSet(MOTOR10, INERTIA_CANCELLATION_FACTOR);
	motorSet(MOTOR9, INERTIA_CANCELLATION_FACTOR);
	delay(65);

	motorStop(MOTOR1);
	motorStop(MOTOR2);
	motorStop(MOTOR10);
	motorStop(MOTOR9);
	delay(200);
}
Пример #9
0
void turnLeft(int x)
{
	int counts = 0;
	imeReset(0); // rest rightLine ime
	imeGet(0, &counts);

	while (abs(counts) < x)
	 {
		motorSet (motor1, 64) ;
		motorSet (motor2, 64) ;
		motorSet (motor10, -64) ;
		motorSet (motor9, -64) ;

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

	motorSet (motor1, -10)	; // no inertia
	motorSet (motor2, -10)	;
	motorSet (motor10, 10)	;
	motorSet (motor9, 10)	;
	delay (45);

	motorStop (motor1);
	motorStop (motor2);
	motorStop (motor10);
	motorStop (motor9);
	delay (200);
}
Пример #10
0
void scrapeRight(int x) {
	int counts = 0;
	imeReset(1); // rest left I.M.E
	imeGet(1, &counts);

	while (abs(counts) < x) {
		motorSet(MOTOR1, 0);
		motorSet(MOTOR2, 0);
		motorSet(MOTOR10, 90);
		motorSet(MOTOR9, 90);

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

	motorSet(MOTOR1, 0);
	motorSet(MOTOR2, 0);
	motorSet(MOTOR10, -10);
	motorSet(MOTOR9, -10);
	delay(55);

	motorSet(MOTOR1, 0);
	motorSet(MOTOR2, 0);
	motorSet(MOTOR10, 0);
	motorSet(MOTOR9, 0);
	delay(200);
}
Пример #11
0
void driveForward(int x)
{
	int counts = 0;
	imeReset(0); // rest rightLine ime
	imeGet(0, &counts);

	while (abs(counts) < x)
	{
		motorSet(MOTOR_DRIVE_RIGHT_LINE_BACK, MOTOR_MAX);
		motorSet(MOTOR_DRIVE_RIGHT_LINE_FRONT, MOTOR_MAX);
		motorSet(MOTOR_DRIVE_LEFT_LINE_BACK, MOTOR_MAX);
		motorSet(MOTOR_DRIVE_LEFT_LINE_FRONT, MOTOR_MAX);

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

	motorSet (MOTOR_DRIVE_RIGHT_LINE_BACK, -7)	; // no inertia
	motorSet (MOTOR_DRIVE_RIGHT_LINE_FRONT, -7)	;
	motorSet (MOTOR_DRIVE_LEFT_LINE_BACK, -7)	;
	motorSet (MOTOR_DRIVE_LEFT_LINE_FRONT, -7)	;
	delay (65);

	motorSet (MOTOR_DRIVE_RIGHT_LINE_BACK, 0)	;
	motorSet (MOTOR_DRIVE_RIGHT_LINE_FRONT, 0)	;
	motorSet (MOTOR_DRIVE_LEFT_LINE_BACK, 0)	;
	motorSet (MOTOR_DRIVE_LEFT_LINE_FRONT, 0)	;
	delay(200);

}
Пример #12
0
void driveBack(int x)
{
	int counts = 0;
	imeReset(0); // rest rightLine ime
	imeGet(0, &counts);

	while (abs(counts) < x)
	 {
	   motorSet (motor1, -100) ;
	   motorSet (motor2, -100) ;
	   motorSet (motor10, -100) ;
	   motorSet (motor9, -100) ;

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

	motorSet (motor1, 7)	; // no inertia
	motorSet (motor2, 7)	;
	motorSet (motor10, 7)	;
	motorSet (motor9, 7)	;
	delay (65);


	motorStop (motor1);
	motorStop (motor2);
	motorStop (motor10);
	motorStop (motor9);
	delay (200);
}
Пример #13
0
void driveBackSlow(int x) {
	int counts = 0;
	imeReset(0); // rest rightLine I.M.E
	imeGet(0, &counts);

	while (abs(counts) < x) {
		motorSet(MOTOR1, -40);
		motorSet(MOTOR2, -40);
		motorSet(MOTOR10, -40);
		motorSet(MOTOR9, -40);

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

	motorSet(MOTOR1, 10); // no inertia
	motorSet(MOTOR2, 10);
	motorSet(MOTOR10, 10);
	motorSet(MOTOR9, 10);
	delay(45);

	motorSet(MOTOR1, 0);
	motorSet(MOTOR2, 0);
	motorSet(MOTOR10, 0);
	motorSet(MOTOR9, 0);

	delay(200);
}
Пример #14
0
void driveForward(int x)
{
	int counts = 0;
	imeReset(0); // rest rightLine ime
	imeGet(0, &counts);

	while (abs(counts) < x)
	    {
	      motorSet (motor1, 127)	;
	      motorSet (motor2, 127)	;
	      motorSet (motor10, 127)	;
	      motorSet (motor9, 127)	;

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

	motorSet (motor1, -7)	; // no inertia
	motorSet (motor2, -7)	;
	motorSet (motor10, -7)	;
	motorSet (motor9, -7)	;
	delay (65);

	motorSet (motor1, 0)	;
	motorSet (motor2, 0)	;
	motorSet (motor10, 0)	;
	motorSet (motor9, 0)	;
	delay(200);

}
Пример #15
0
void LiftTask(void *ignore) {
  int counts;

  while (1) {
    imeGet(IME_LIFT, &counts); // get lift encoder count

    if (joystickGetDigital(1, 8, JOY_UP) || joystickGetDigital(2, 8, JOY_UP)) { // check stash waypoint
      liftPIDRequestedValue = LIFT_STASH_HEIGHT;
    } else if (joystickGetDigital(1, 8, JOY_LEFT) || joystickGetDigital(2, 8, JOY_LEFT)) { // check bump waypoint
      liftPIDRequestedValue = LIFT_BUMP_HEIGHT;
    } else if (joystickGetDigital(1, 8, JOY_DOWN) || joystickGetDigital(2, 8, JOY_DOWN)) { // check floor waypoint
      liftPIDRequestedValue = LIFT_FLOOR_HEIGHT;
    } else if (joystickGetDigital(1, 8, JOY_RIGHT) || joystickGetDigital(2, 8, JOY_RIGHT)) { // check hang waypoint
      liftPIDRequestedValue = LIFT_HANG_HEIGHT;
    } else if (joystickGetDigital(1, 6, JOY_UP) || joystickGetDigital(2, 6, JOY_UP)) { // if trigger up, turn PID off and override motors up, then turn PID on for the new position
      mutexTake(liftMutex, ULONG_MAX);
      liftSet(127);
      liftPIDRequestedValue = counts;
      mutexGive(liftMutex);
    } else if (joystickGetDigital(1, 6, JOY_DOWN) || joystickGetDigital(2, 6, JOY_DOWN)) { // if trigger up, turn PID off and override motors up, then turn PID on for the new position
      mutexTake(liftMutex, ULONG_MAX);
      liftSet(-127);
      liftPIDRequestedValue = counts;
      mutexGive(liftMutex);
    } else {
      liftSet(0);
    }

    taskDelay(20);
  }
}
Пример #16
0
int integratedEncoderGet(IntegratedEncoder encoder) {
	int value;
	imeGet(encoder.imeAddress, &value);
	if (encoder.inverted) {
		value = -value;
	}
	return value;
}
Пример #17
0
void driveBackSlow(int x)
{
	int counts = 0;
	imeReset(0); // rest rightLine ime
	imeGet(0, &counts);

	while (abs(counts) < x)
	 {
		  motorSet (motor1, -40)	;
		  motorSet (motor2, -40)	;
		  motorSet (motor10, -40);
		  motorSet (motor9, -40)	;

		  imeGet(0, &counts); // keep getting the value
	 }
	delay (200);
}
Пример #18
0
int getIME(IME ime)
{
	int value;

	imeGet(ime.port, &value);

	return (ime.inverted) ? -value : value;
}
Пример #19
0
void scrapeLeftBack (int x)
{
	int counts = 0;
		imeReset(0); // rest rightLine ime
		imeGet(0, &counts);

			while (abs(counts) < x)
			    {
			      motorSet (motor1, 0)	;
			      motorSet (motor2,0)	;
			      motorSet (motor10, 90);
			      motorSet (motor9, 90)	;

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

}
Пример #20
0
void turnLeftArc(int x) {
	int counts = 0;
	imeReset(0); // rest rightLine I.M.E
	imeGet(0, &counts);

	while (abs(counts) < x) {
		motorSet(MOTOR1, 127);
		motorSet(MOTOR2, 127);
		motorSet(MOTOR10, 0);
		motorSet(MOTOR9, 0);

		imeGet(0, &counts); // keep getting the value
	}
	motorStop(MOTOR1);
	motorStop(MOTOR2);
	motorStop(MOTOR10);
	motorStop(MOTOR9);
	delay(200);
}
Пример #21
0
void turnRightArc(int x) {
	int counts = 1;
	imeReset(1); // rest rightLine I.M.E
	imeGet(1, &counts);

	while (abs(counts) < x) {
		motorSet(MOTOR1, 0);
		motorSet(MOTOR2, 0);
		motorSet(MOTOR10, 127);
		motorSet(MOTOR9, 127);

		imeGet(0, &counts); // keep getting the value
	}
	motorStop(MOTOR1);
	motorStop(MOTOR2);
	motorStop(MOTOR10);
	motorStop(MOTOR9);

}
Пример #22
0
void scrapeLeftBack (int x)
{
	int counts = 0;
			imeReset(0); // rest left ime
			imeGet(0, &counts);

				while (abs(counts) < x)
				    {
				      motorSet (motor1, -90)	;
				      motorSet (motor2,-90)	;
				      motorSet (motor10, 0);
				      motorSet (motor9, 0)	;

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

				motorSet (motor1, 10)	; // no intertia
				motorSet (motor2, 10)	;
				motorSet (motor10, 0);
				motorSet (motor9, 0)	;
				delay (55);


				motorSet (motor1, 0)	;
				motorSet (motor2, 0)	;
				motorSet (motor10, 0);
				motorSet (motor9, 0)	;
				delay (200);



	/*motorSet (motor1, 90)	;
	motorSet (motor2, 90)	;
	motorSet (motor10, 0);
	motorSet (motor9, 0)	;
	delay(x);

	motorSet (motor1, 0)	;
	motorSet (motor2, 0)	;
	motorSet (motor10, 0);
	motorSet (motor9, 0)	;
*/
}
Пример #23
0
void turnLeftArc(int x)
{
	int counts = 0;
	imeReset(0); // rest rightLine ime
	imeGet(0, &counts);

	while (abs(counts) < x)
	 {
		motorSet (motor1, 127) ;
		motorSet (motor2, 127) ;
		motorSet (motor10, 0) ;
		motorSet (motor9, 0) ;

		imeGet(0, &counts); // keep getting the value
	 }
	motorStop (motor1);
	motorStop (motor2);
	motorStop (motor10);
	motorStop (motor9);
	delay (200);
}
Пример #24
0
void driveForward(int x)
{
	int counts = 0;
	imeReset(0); // rest rightLine ime
	imeGet(0, &counts);

	while (abs(counts) < x)
	    {
	      motorSet (motor1, 100)	;
	      motorSet (motor2, 100)	;
	      motorSet (motor10, 100)	;
	      motorSet (motor9, 100)	;

	      imeGet(0, &counts); // keep getting the value
	    }
	motorStop (motor1);
	motorStop (motor2);
	motorStop (motor10);
	motorStop (motor9);
	delay (200);
}
Пример #25
0
void turnRightArc(int x)
{
	int counts = 1;
	imeReset(1); // rest rightLine ime
	imeGet(1, &counts);

	while (abs(counts) < x)
	 {
		motorSet (motor1, 0) ;
		motorSet (motor2, 0) ;
		motorSet (motor10, 127) ;
		motorSet (motor9, 127) ;

		imeGet(0, &counts); // keep getting the value
	 }
	motorStop (motor1);
	motorStop (motor2);
	motorStop (motor10);
	motorStop (motor9);

}
Пример #26
0
void startIOTask(void *ignore) {
	while(1) {
		setDriveTrainMotors(driveTrainStyle);

		motorSet(ARMLeft.port, ARMLeft.out * ARMLeft.reflected);
		motorSet(ARMRight.port, ARMRight.out * ARMRight.reflected);
		motorSet(ARMTopLeft.port, ARMTopLeft.out * ARMTopLeft.reflected);
		motorSet(ARMTopRight.port, ARMTopRight.out * ARMTopRight.reflected);
		motorSet(ARMBottomLeft.port, ARMBottomLeft.out * ARMBottomLeft.reflected);
		motorSet(ARMBottomRight.port, ARMBottomRight.out * ARMBottomRight.reflected);

		if(useIMEs) {
			imeGet(IMELEFT, &encVals[0]);
			imeGet(IMERIGHT, &encVals[1]);
		} else {
			encVals[0] = encoderGet(encLeft);
			encVals[1] = encoderGet(encRight);
		}
		gyroVal = gyroGet(gyro);

		delay(10);
	}
}
Пример #27
0
void LiftPIDTask(void *ignore) {
  int counts;

  while (1) {
    imeGet(IME_LIFT, &counts); // block until lift resources are available

    if (mutexTake(liftMutex, ULONG_MAX)) { // block until mutex is available.
      pidSensorCurrentValue = counts; // getting current position
      pidError = pidSensorCurrentValue - liftPIDRequestedValue; // calculating error signal

      if (abs(pidError) < PID_INTEGRAL_LIMIT) { // calculating integral factor, given it is within bounds
        pidIntegral = pidIntegral + pidError;
      } else {
        pidIntegral = 0;
      }

      pidDerivative = pidError - pidLastError; // calculate derivative factor
      pidLastError  = pidError;

      pidDrive = (pid_Kp * pidError) + (pid_Ki * pidIntegral) + (pid_Kd * pidDerivative); // sum all factors

      if (pidDrive > PID_DRIVE_MAX) { // limit max output
        pidDrive = PID_DRIVE_MAX;
      }

      if (pidDrive < PID_DRIVE_MIN) {
        pidDrive = PID_DRIVE_MIN;
      }

      liftSet (pidDrive * PID_MOTOR_SCALE); // set lift motors
      mutexGive(liftMutex); // give control back to LiftTask.

    } else { // reset all
      pidError = 0;
      pidLastError = 0;
      pidIntegral = 0;
      pidDerivative = 0;
    }

    taskDelay(25);
  }
}
Пример #28
0
short getSensorValue(Sensor s) {
	short retVal = -1;
	switch (s.type) {
	case SHAFT_ENCODER:
		if (DEBUG) {
			print("got encoder value");
		}
		retVal = (short)encoderGet(s.enc);
		break;
	case GYROSCOPE:
		retVal = (short)gyroGet(s.gyr);
		break;
	case ULTRASONIC:
		retVal = (short)ultrasonicGet(s.ult);
		break;
	case ACCELEROMETER:
		retVal = (short)analogReadCalibratedHR(s.port);
		break;
	case BUMPER_SWITCH:
	case LIMIT_SWITCH:
		retVal = (short)digitalRead(s.port);
		break;
	case LINE_TRACKER:
	case POTENTIOMETER:
		retVal = (short)analogRead(s.port);
		break;
	case TIME:
		retVal = (short)getTimer(s.port, true);
		break;
	case IME: ; // so apparently due to history this ";" has to be here to declare
	// a variable inside a case statement?
	int imeVal = -1;
	imeGet(s.port, &imeVal);
	retVal = (short)imeVal;
	break;
	}
	return retVal;
}
Пример #29
0
void classic15Blue()
{
	int armMin = 290;
	int wallHeight = 1000;
	int goalHeight = 1700;
	int dead1 = 1200;
	int dead2 = 2000;
	int dead3 = 3000;
	int pot = analogRead(8);

	//encoder values
	int encoder1 = 900; //drive to goal
	int encoder2 = 325; //keep going towards goal
	int encoder3 = 0; //drive slow, adjust to 90 degrees
	int encoder4 = 325; //back up
	int encoder5 = 1350; //back up across the barrier again
	int encoder6 = 80; // turn towards center large ball
	int encoder7 = 600; // hit 1st ball off the barrier
	int encoder8 = 350; // drive back
	int encoder9 = 200; // turn towards other large ball
	int encoder10 = 400; // hit 2nd ball off the barrier
	int encoder11 = 300; // drive back to square
	int encoder12 = 290; // turn to barf
	int encoder13 = 800; // drive to barf + pickup
	int encoder14 = 400;
	int encoder15 = 700;

	// begin routine

	intakeDead();
	delay(1000);
	stopIntake();

	// driveforward (some curve)////////////////////////////////////////////////////
	int counts = 0;
	imeReset(0); // rest rightLine I.M.E
	imeGet(0, &counts);

	while (abs(counts) < encoder1)
	{
		motorSet(MOTOR_DRIVE_RIGHT_BACK, 110); // slight curve cuz friction
		motorSet(MOTOR_DRIVE_RIGHT_FRONT, 110);
		motorSet(MOTOR_ARM_LEFT_BACK, 127);
		motorSet(MOTOR_ARM_LEFT_FRONT, 127);

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

	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);

///////////////////////////////////////////////////////////////////////////////

	//driveForward(encoder1); // drive to goal
	armUp(goalHeight); //raise arm after cross barrier
	driveForwardSlow(encoder2);  //keep going towards goal
	driveForwardSlowDead(); //drive slow, adjust to 90 degrees
	delay(1000);
	outtake(1700); // outtake
	driveBack(encoder4); // back up
	delay(300);
	armDown(armMin); // lower arm
	driveBack(encoder5); //back up across the barrier again
	delay(300);
	turnLeft(encoder6); // turn towards center large ball
	armUp(wallHeight); // arm up to wall height
	driveForward(encoder7); // hit it off the barrier
	delay(500);
	driveBack(encoder8); // drive back
	delay(300);
	turnRight(encoder9); // turn towards other large ball
	driveForward(encoder10); // hit 2nd ball off the barrier
	delay(500);
	driveBack(encoder11); // drive back to square
	delay(600);
	turnLeft(encoder12); // turn to barf
	delay(300);
	armDown(armMin);
	intakeDead(); // pick up barf
	driveForward(encoder13); // drive towards barf + intake it
	delay(500);

	//end of routine

	stopAll();
	delay(60000); ///////////////////////////////////////////////////////////////////////////////////

}
Пример #30
0
void operatorControl() {
	lcdInit(uart1);
	lcdClear(uart1);
	lcdSetBacklight(uart1, true);

	liftTaskCreate();

	bool hookButtonPressed = false;

	int imeLeft;
	int imeRight;
	while (true) {
		imeGet(0, &imeLeft);
		imeGet(1, &imeRight);
		lcdPrint(uart1, 1, "IME Left %d", imeLeft);
		lcdPrint(uart1, 2, "IME Right %d", imeRight);

		// toggle hook
		bool hookButtonPressedNow = joystickGetDigital(1, 7, JOY_UP);
		if (hookButtonPressedNow && hookButtonPressedNow != hookButtonPressed) {
			if (isHookDeployed()) {
				retractHook();
			} else {
				deployHook();
			}
		}
		hookButtonPressed = hookButtonPressedNow;

		if (joystickGetDigital(1, 8, JOY_UP)) {
			hang();
		}

		if (joystickGetDigital(1, 8, JOY_DOWN)) {
			unhang();
		}


		// Drive
		int leftY  = ramp(joystickGetAnalog(1, 3));
		int rightY = ramp(joystickGetAnalog(1, 2));
		motorSet(driveFrontRight,  -rightY);
		motorSet(driveMiddleRight, -rightY);
		motorSet(driveBackRight,   -rightY);
		motorSet(driveFrontLeft,   leftY);
		motorSet(driveMiddleLeft,  leftY);
		motorSet(driveBackLeft,    leftY);

		// Lift
		int liftUp   = joystickGetDigital(1, 6, JOY_UP);
		int liftDown = joystickGetDigital(1, 6, JOY_DOWN);

		if (liftUp && liftDown) {
			//motorSet(liftLeft, 40);
			//motorSet(liftRight, -40);
		} else if (liftUp) {
			liftManual(LIFT_SPEED);
			//motorSet(liftLeft, 127);
			//motorSet(liftRight, -127);
		} else if (liftDown) {
			liftManual(-LIFT_SPEED);
			//motorSet(liftLeft, -127);
			//motorSet(liftRight, 127);
		} else {
			//motorSet(liftLeft, 0);
			//motorSet(liftRight, 0);
		}

		// Manipulator
		int manipulatorIn = joystickGetDigital(1, 5, JOY_DOWN);
		int manipulatorOut = joystickGetDigital(1, 5, JOY_UP);
		if (manipulatorIn) {
			motorSet(intakeLeft, -127);
			motorSet(intakeRight, 127);
		} else if (manipulatorOut) {
			motorSet(intakeLeft, 60);
			motorSet(intakeRight, -60);
		} else {
			motorSet(intakeLeft, 0);
			motorSet(intakeRight, 0);
		}


		delay(50);
	}
}