int8_t calculateShooterSpeed() {
float dist = ultrasonicGet(ultra) / 2.54;
float speed = 1.11 * dist - 1.6;
if (speed > MAX_SPEED) {
speed = MAX_SPEED;
} else if (speed < 0) {
speed = 0;
}
return (int8_t) speed;
}
void vTaskUltraSonic()
{
Ultrasonic us = ultrasonicInit(ULTRASONIC_ECHO_CHANNEL, ULTRASONIC_PING_CHANNEL);
unsigned long nextWakeTime;
while (1)
{
nextWakeTime = millis();
//get reading from sensor
usValue = ultrasonicGet(us);
fprintf(stdout,"Ultrasonc value = %d\n", usValue);
taskDelayUntil(&nextWakeTime, 50);
}
}
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;
}
//if it is a limit switch or push button a 1 is returned if it is pressed
int sensorGet(Sensor sensor) {
int value = NULL;
switch (sensor.type) {
case Sensor_IntegratedEncoder:
value = integratedEncoderGet(sensor.sensorData.ime);
break;
case Sensor_QuadEncoder:
value = quadEncoderGet(sensor.sensorData.quadEncoder);
break;
case Sensor_Sonar:
value = ultrasonicGet(sensor.sensorData.sonar);
break;
case Sensor_Line:
value = analogSensorGet(sensor.sensorData.analog);
break;
case Sensor_Light:
value = analogSensorGet(sensor.sensorData.analog);
break;
case Sensor_Bumper:
value = pushButtonPressed(sensor.sensorData.pushButton);
break;
case Sensor_LimitSwitch:
value = pushButtonPressed(sensor.sensorData.pushButton);
break;
case Sensor_Potentiometer:
value = analogSensorGet(sensor.sensorData.analog);
break;
case Sensor_Gyroscope:
value = analogSensorGet(sensor.sensorData.analog);
break;
case Sensor_Accelerometer:
value = analogSensorGet(sensor.sensorData.analog);
break;
}
return value;
}
/*
* 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
}
int getSonarRight()
{
/** gets the value of the right ultrasonic sensor */
return ultrasonicGet(ultraRight) * TO_INCHES - toCenterX;
}
int getSonarBack()
{
/** gets the value of the back ultrasonic sensor */
return ultrasonicGet(ultraBack) * TO_INCHES + toCenterY;
}
int getSonarLeft()
{
/** gets the value of the left ultrasonic sensor */
return ultrasonicGet(ultraLeft) * TO_INCHES + toCenterX;
}
int getSonar(Drive drive)
{
return ultrasonicGet(drive.sonar);
}
