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