/*
* 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 left 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() {
//lfilterClear();
int8_t shooterSpeed = shooterSpeedPresets[2];
int8_t n = 0;
while (true) {
if (abs(motorGet(SHOOTER_MOTOR_CHANNEL)) == shooterSpeed && abs(motorGet(SHOOTER_MOTOR_CHANNEL2)) == shooterSpeed) {
if (n < 75) {
++n;
} else {
lifter(60);
takeInInternal(60);
}
}
shooter(shooterSpeed);
delay(20);
}
}
WFA::AccessibleStateSetMap
WFA::computeAllReachingWeights(SemElemSet::SemElemSubsumptionComputer computer,
bool include_zeroes) const
{
// Lift weights to the sets
WFA lifted;
sem_elem_t lifted_zero = new SemElemSet(computer, include_zeroes, this->getSomeWeight()->zero());
SemElemSetLifter lifter(&lifted, computer, include_zeroes);
for (std::set<Key>::const_iterator q = Q.begin(); q != Q.end(); ++q) {
lifted.addState(*q, lifted_zero);
}
this->for_each(lifter);
lifted.setInitialState(this->getInitialState());
// finals don't matter
assert(num_trans(*this) == num_trans(lifted));
// Issue poststar from the initial state in the lifted automaton.
std::set<Key> sources;
sources.insert(lifted.getInitialState());
EpsilonCloseCache const & poststar_answer = lifted.genericFwpdsPoststar(sources, is_any_transition);
EpsilonCloseCache::const_iterator loc = poststar_answer.find(lifted.getInitialState());
assert(loc != poststar_answer.end());
assert(1u == poststar_answer.size());
AccessibleStateMap const & set_weights = loc->second;
// set_weights is almost what we want, except we don't want to make the
// user downcast everything to get the actual result.
AccessibleStateSetMap result;
for (AccessibleStateMap::const_iterator state_weight_pair = set_weights.begin();
state_weight_pair != set_weights.end(); ++state_weight_pair)
{
SemElemSet * set_weight = dynamic_cast<SemElemSet *>(state_weight_pair->second.get_ptr());
assert(result.find(state_weight_pair->first) == result.end());
result[state_weight_pair->first] = set_weight->getElements();
}
return result;
}
/*
* 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() {
#ifdef AUTO
autonomous();
#elif defined(TEST)
#define DEFAULT_SHOOTER_SPEED 0
#define SHOOTER_SPEED_INCREMENT 5
int8_t xSpeed, ySpeed, rotation;
int8_t lifterSpeed/*, intakeSpeed*/;
int16_t shooterSpeed = DEFAULT_SHOOTER_SPEED; //shooter is on when robot starts
int8_t frontIntakeSpeed = INTAKE_SPEED;
bool isShooterOn = true;
bool isAutoShootOn = false;
//lfilterClear();
toggleBtnInit(JOYSTICK_SLOT, INTAKE_BUTTON_GROUP, JOY_UP); // intake forward
toggleBtnInit(JOYSTICK_SLOT, INTAKE_BUTTON_GROUP, JOY_LEFT); // intake off
toggleBtnInit(JOYSTICK_SLOT, INTAKE_BUTTON_GROUP, JOY_RIGHT); // intake off
toggleBtnInit(JOYSTICK_SLOT, INTAKE_BUTTON_GROUP, JOY_DOWN); // intake backward
toggleBtnInit(JOYSTICK_SLOT, CONTROL_BUTTON_GROUP, JOY_DOWN); // shooter on off
toggleBtnInit(JOYSTICK_SLOT, CONTROL_BUTTON_GROUP, JOY_RIGHT); // auto shoot on off
toggleBtnInit(JOYSTICK_SLOT, SHOOTER_ADJUST_BUTTON_GROUP, JOY_UP); // shooter speed up
toggleBtnInit(JOYSTICK_SLOT, SHOOTER_ADJUST_BUTTON_GROUP, JOY_DOWN); // shooter speed down
while (true) {
printf("ultra distance (in): %f\r\n", ultrasonicGet(ultra) / 2.54);
toggleBtnUpdateAll();
// drive
xSpeed = (int8_t) joystickGetAnalog(JOYSTICK_SLOT, STRAFE_AXIS);
ySpeed = (int8_t) joystickGetAnalog(JOYSTICK_SLOT, DRIVE_AXIS);
rotation = (int8_t) joystickGetAnalog(JOYSTICK_SLOT, ROTATION_AXIS) / 2;
if (abs(ySpeed) < DIAGONAL_DRIVE_DEADBAND) {
ySpeed = 0;
}
if (abs(xSpeed) < DIAGONAL_DRIVE_DEADBAND) {
xSpeed = 0;
}
drive(xSpeed, ySpeed, rotation, false);
// lifter up down
if (joystickGetDigital(JOYSTICK_SLOT, LIFTER_BUTTON_GROUP, JOY_UP)) {
lifterSpeed = LIFTER_SPEED;
} else if (joystickGetDigital(JOYSTICK_SLOT, LIFTER_BUTTON_GROUP, JOY_DOWN)) {
lifterSpeed = -LIFTER_SPEED;
} else {
lifterSpeed = 0;
}
lifter(lifterSpeed);
takeInInternal(lifterSpeed);
if (isShooterOn) {
if (isAutoShootOn) {
shooterSpeed = calculateShooterSpeed();
} else {
// shooter increase speed
if (toggleBtnGet(JOYSTICK_SLOT, SHOOTER_ADJUST_BUTTON_GROUP, JOY_UP) == BUTTON_PRESSED) {
shooterSpeed += SHOOTER_SPEED_INCREMENT;
if (shooterSpeed > SHOOTER_MAX_SPEED) {
shooterSpeed = SHOOTER_MAX_SPEED;
}
}
// shooter decrease speed
if (toggleBtnGet(JOYSTICK_SLOT, SHOOTER_ADJUST_BUTTON_GROUP, JOY_DOWN) == BUTTON_PRESSED) {
shooterSpeed -= SHOOTER_SPEED_INCREMENT;
if (shooterSpeed < SHOOTER_MIN_SPEED) {
shooterSpeed = SHOOTER_MIN_SPEED;
}
}
}
// auto shooter on off
if (toggleBtnGet(JOYSTICK_SLOT, CONTROL_BUTTON_GROUP, JOY_RIGHT) == BUTTON_PRESSED) {
isAutoShootOn = !isAutoShootOn;
}
}
// shooter on off
if (toggleBtnGet(JOYSTICK_SLOT, CONTROL_BUTTON_GROUP, JOY_DOWN) == BUTTON_PRESSED) {
isShooterOn = !isShooterOn;
shooterSpeed = isShooterOn ? DEFAULT_SHOOTER_SPEED : 0;
}
shooter(shooterSpeed);
// intake mode
if (toggleBtnGet(JOYSTICK_SLOT, INTAKE_BUTTON_GROUP, JOY_LEFT) == BUTTON_PRESSED
|| toggleBtnGet(JOYSTICK_SLOT, INTAKE_BUTTON_GROUP, JOY_RIGHT) == BUTTON_PRESSED) {
frontIntakeSpeed = 0;
} else if (toggleBtnGet(JOYSTICK_SLOT, INTAKE_BUTTON_GROUP, JOY_UP) == BUTTON_PRESSED) {
frontIntakeSpeed = -INTAKE_SPEED;
} else if (toggleBtnGet(JOYSTICK_SLOT, INTAKE_BUTTON_GROUP, JOY_DOWN) == BUTTON_PRESSED) {
frontIntakeSpeed = INTAKE_SPEED;
}
takeInFront(frontIntakeSpeed);
delay(20);
}
#else
int8_t xSpeed, ySpeed, rotation;
int8_t lifterSpeed/*, intakeSpeed*/;
int8_t defaultPreset = 0;
int8_t currentPreset = defaultPreset;
int16_t shooterSpeed = shooterSpeedPresets[defaultPreset]; //shooter is on when robot starts
int8_t frontIntakeSpeed = INTAKE_SPEED;
bool isShooterOn = true;
//lfilterClear();
toggleBtnInit(JOYSTICK_SLOT, INTAKE_BUTTON_GROUP, JOY_UP); // intake forward
toggleBtnInit(JOYSTICK_SLOT, INTAKE_BUTTON_GROUP, JOY_LEFT); // intake off
toggleBtnInit(JOYSTICK_SLOT, INTAKE_BUTTON_GROUP, JOY_RIGHT); // intake off
toggleBtnInit(JOYSTICK_SLOT, INTAKE_BUTTON_GROUP, JOY_DOWN); // intake backward
toggleBtnInit(JOYSTICK_SLOT, CONTROL_BUTTON_GROUP, JOY_DOWN); // shooter on off
toggleBtnInit(JOYSTICK_SLOT, SHOOTER_ADJUST_BUTTON_GROUP, JOY_UP); // shooter speed up
toggleBtnInit(JOYSTICK_SLOT, SHOOTER_ADJUST_BUTTON_GROUP, JOY_DOWN); // shooter speed down
while (true) {
printf("ultra distance (in): %f\r\n", ultrasonicGet(ultra) / 2.54);
toggleBtnUpdateAll();
// drive
xSpeed = (int8_t) joystickGetAnalog(JOYSTICK_SLOT, STRAFE_AXIS);
ySpeed = (int8_t) joystickGetAnalog(JOYSTICK_SLOT, DRIVE_AXIS);
rotation = (int8_t) joystickGetAnalog(JOYSTICK_SLOT, ROTATION_AXIS) / 2;
if (abs(ySpeed) < DIAGONAL_DRIVE_DEADBAND) {
ySpeed = 0;
}
if (abs(xSpeed) < DIAGONAL_DRIVE_DEADBAND) {
xSpeed = 0;
}
drive(xSpeed, ySpeed, rotation, false);
// lifter up down
if (joystickGetDigital(JOYSTICK_SLOT, LIFTER_BUTTON_GROUP, JOY_UP)) {
lifterSpeed = LIFTER_SPEED;
} else if (joystickGetDigital(JOYSTICK_SLOT, LIFTER_BUTTON_GROUP, JOY_DOWN)) {
lifterSpeed = -LIFTER_SPEED;
} else {
lifterSpeed = 0;
}
lifter(lifterSpeed);
takeInInternal(lifterSpeed);
if (isShooterOn) {
// shooter increase speed
if (toggleBtnGet(JOYSTICK_SLOT, SHOOTER_ADJUST_BUTTON_GROUP, JOY_UP) == BUTTON_PRESSED) {
++currentPreset;
if (currentPreset >= NUM_SHOOTER_SPEED_PRESETS) {
currentPreset = NUM_SHOOTER_SPEED_PRESETS - 1;
}
}
// shooter decrease speed
if (toggleBtnGet(JOYSTICK_SLOT, SHOOTER_ADJUST_BUTTON_GROUP, JOY_DOWN) == BUTTON_PRESSED) {
--currentPreset;
if (currentPreset < 0) {
currentPreset = 0;
}
}
shooterSpeed = shooterSpeedPresets[currentPreset];
}
// shooter on off
if (toggleBtnGet(JOYSTICK_SLOT, CONTROL_BUTTON_GROUP, JOY_DOWN) == BUTTON_PRESSED) {
isShooterOn = !isShooterOn;
shooterSpeed = isShooterOn ? shooterSpeedPresets[defaultPreset] : 0;
}
shooter(shooterSpeed);
// intake mode
if (toggleBtnGet(JOYSTICK_SLOT, INTAKE_BUTTON_GROUP, JOY_LEFT) == BUTTON_PRESSED
|| toggleBtnGet(JOYSTICK_SLOT, INTAKE_BUTTON_GROUP, JOY_RIGHT) == BUTTON_PRESSED) {
frontIntakeSpeed = 0;
} else if (toggleBtnGet(JOYSTICK_SLOT, INTAKE_BUTTON_GROUP, JOY_UP) == BUTTON_PRESSED) {
frontIntakeSpeed = -INTAKE_SPEED;
} else if (toggleBtnGet(JOYSTICK_SLOT, INTAKE_BUTTON_GROUP, JOY_DOWN) == BUTTON_PRESSED) {
frontIntakeSpeed = INTAKE_SPEED;
}
takeInFront(frontIntakeSpeed);
delay(20);
}
#endif
}
