/** * Set servo positions (0.0 to 1.0) translated from normalized values * (-1.0 to 1.0). * * @param normalizedHorizontal Pan Position from -1.0 to 1.0. * @param normalizedVertical Tilt Position from -1.0 to 1.0. */ void setServoPositions(float normalizedHorizontal, float normalizedVertical) { float servoH = NormalizeToRange(normalizedHorizontal); /* narrow vertical range keep vertical servo from going too far */ //float servoV = NormalizeToRange(normalizedVertical, 0.2, 0.8); float servoV = NormalizeToRange(normalizedVertical); float currentH = horizontalServo->Get(); float currentV = verticalServo->Get(); /* make sure the movement isn't too small */ if (fabs(servoH - currentH) > servoDeadband) { horizontalServo->Set(servoH); /* save new normalized horizontal position */ horizontalPosition = RangeToNormalized(servoH, 1); } if (fabs(servoV - currentV) > servoDeadband) { verticalServo->Set(servoV); // save new normalized vertical position verticalPosition = RangeToNormalized(servoV, 1); } ShowActivity("Servo set to: x: %f y: %f", servoH, servoV); }
/** * Gets calibrated parameters from previously calibrated gyro, allocates a new * gyro with the given parameters for center and offset, and re-runs tests on * the new gyro. */ void TiltPanCameraTest::GyroCalibratedParameters() { uint32_t cCenter = m_gyro->GetCenter(); float cOffset = m_gyro->GetOffset(); delete m_gyro; m_gyro = new AnalogGyro(TestBench::kCameraGyroChannel, cCenter, cOffset); m_gyro->SetSensitivity(kSensitivity); // Default gyro angle test // Accumulator needs a small amount of time to reset before being tested m_gyro->Reset(); Wait(.001); EXPECT_NEAR(0.0f, m_gyro->GetAngle(), 1.0f); // Gyro angle test // Make sure that the gyro doesn't get jerked when the servo goes to zero. m_pan->SetAngle(0.0); Wait(0.5); m_gyro->Reset(); for (int i = 0; i < 600; i++) { m_pan->Set(i / 1000.0); Wait(0.001); } double gyroAngle = m_gyro->GetAngle(); EXPECT_NEAR(gyroAngle, kTestAngle, 10.0) << "Gyro measured " << gyroAngle << " degrees, servo should have turned " << kTestAngle << " degrees"; }
/** * Adjust servo positions (0.0 to 1.0) translated from normalized * values (-1.0 to 1.0). * * @param normalizedHorizontal Pan adjustment from -1.0 to 1.0. * @param normalizedVertical Tilt adjustment from -1.0 to 1.0. */ void adjustServoPositions(float normDeltaHorizontal, float normDeltaVertical) { /* adjust for the fact that servo overshoots based on image input */ normDeltaHorizontal /= 8.0; normDeltaVertical /= 4.0; /* compute horizontal goal */ float currentH = horizontalServo->Get(); float normCurrentH = RangeToNormalized(currentH, 1); float normDestH = normCurrentH + normDeltaHorizontal; /* narrow range keep servo from going too far */ if (normDestH > 1.0) normDestH = 1.0; if (normDestH < -1.0) normDestH = -1.0; /* convert inputs to servo range */ float servoH = NormalizeToRange(normDestH); /* compute vertical goal */ float currentV = verticalServo->Get(); float normCurrentV = RangeToNormalized(currentV, 1); float normDestV = normCurrentV + normDeltaVertical; if (normDestV > 1.0) normDestV = 1.0; if (normDestV < -1.0) normDestV = -1.0; float servoV = NormalizeToRange(normDestV, 0.2, 0.8); /* make sure the movement isn't too small */ if (fabs(currentH - servoH) > servoDeadband) { horizontalServo->Set(servoH); /* save new normalized horizontal position */ horizontalPosition = RangeToNormalized(servoH, 1); } if (fabs(currentV - servoV) > servoDeadband) { verticalServo->Set(servoV); // save new normalized vertical position verticalPosition = RangeToNormalized(servoV, 1); } ShowActivity("Servo set to: x: %f y: %f", servoH, servoV); }
/** * Set servo positions (0.0 to 1.0) translated from normalized values (-1.0 to 1.0). * * @param normalizedHorizontal Pan Position from -1.0 to 1.0. * @param normalizedVertical Tilt Position from -1.0 to 1.0. */ void setServoPositions(float normalizedHorizontal, float normalizedVertical) { float servoH = NormalizeToRange(normalizedHorizontal); float servoV = NormalizeToRange(normalizedVertical); float currentH = horizontalServo->Get(); float currentV = verticalServo->Get(); /* make sure the movement isn't too small */ if ( fabs(servoH - currentH) > servoDeadband ) { horizontalServo->Set( servoH ); /* save new normalized horizontal position */ horizontalPosition = RangeToNormalized(servoH, 1); } if ( fabs(servoV - currentV) > servoDeadband ) { verticalServo->Set( servoV ); verticalPosition = RangeToNormalized(servoV, 1); } }
virtual void SetUp() override { m_tilt = new Servo(TestBench::kCameraTiltChannel); m_pan = new Servo(TestBench::kCameraPanChannel); m_spiAccel = new ADXL345_SPI(SPI::kOnboardCS0); m_tilt->Set(kTiltSetpoint45); m_pan->SetAngle(0.0f); Wait(kServoResetTime); m_gyro->Reset(); }
/** * Test if the servo turns 90 degrees and the gyroscope measures this angle * Note servo on TestBench is not the same type of servo that servo class * was designed for so setAngle is significantly off. This has been calibrated * for the servo on the rig. */ void TiltPanCameraTest::GyroAngle() { // Make sure that the gyro doesn't get jerked when the servo goes to zero. m_pan->SetAngle(0.0); Wait(0.5); m_gyro->Reset(); for (int i = 0; i < 600; i++) { m_pan->Set(i / 1000.0); Wait(0.001); } double gyroAngle = m_gyro->GetAngle(); EXPECT_NEAR(gyroAngle, kTestAngle, 10.0) << "Gyro measured " << gyroAngle << " degrees, servo should have turned " << kTestAngle << " degrees"; }
void TeleopPeriodic() { SmartDashboard::PutNumber("joystickX",stick.GetX()); SmartDashboard::PutNumber("joystickY",stick.GetY()); //SmartDashboard::PutBoolean("f*****g buttons", stick.GetRawButton(1)); //SmartDashboard::PutNumber("potentiometer voltage", pot.GetVoltage()); SmartDashboard::PutBoolean("infra",infra.Get()); SmartDashboard::PutNumber("accelX",accel.GetX()); SmartDashboard::PutNumber("accelY",accel.GetY()); SmartDashboard::PutNumber("accelZ",accel.GetZ()); servo.Set( trueMap(stick.GetX(), 1, -1, 1, 0) // trueMap allows use of entire joystick ); SmartDashboard::PutNumber("servo", servo.Get()); jag1.Set(stick.GetY()); jag2.Set(stick.GetY()); //tal1.Set(stick.GetY()); SmartDashboard::PutNumber("jag1", jag1.Get()); SmartDashboard::PutNumber("jag2", jag2.Get()); /*SmartDashboard::PutNumber("encpos", enc.Get()); SmartDashboard::PutNumber("encspd", enc.GetRate());*/ if (stick.GetRawButton(1) && !actuatePressed) { pistonVal=!pistonVal; piston.Set(pistonVal ? DoubleSolenoid::kForward : DoubleSolenoid::kReverse); actuatePressed = true; } else if (!stick.GetRawButton(1)) actuatePressed = false; SmartDashboard::PutBoolean("piston forward", piston.Get() == DoubleSolenoid::kForward); }
void TeleopPeriodic() { cameraTilt->Set(0.3); switch (robotState) { case kCentering: stateCentering(); break; case kAiming: stateAiming(); break; case kLaunching: stateLaunching(); break; default: case kOperatorControl: stateOperatorControl(); break; } }
void Autonomous(void) { char funcName[] = "Autonomous"; DPRINTF(LOG_DEBUG, "start VisionDemo autonomous"); //GetWatchdog().Feed(); // image data for tracking ColorMode mode = IMAQ_HSL; // RGB or HSL // TrackingThreshold td = GetTrackingData(RED, FLUORESCENT); TrackingThreshold td = GetTrackingData(GREEN, FLUORESCENT); int panIncrement = 0; // pan needs a 1-up number for each call DPRINTF(LOG_DEBUG, "SERVO - looking for COLOR %s ", td.name); /* initialize position and destination variables * position settings range from -1 to 1 * setServoPositions is a wrapper that handles the conversion to * range for servo */ horizontalDestination = 0.0; // final destination range -1.0 to +1.0 verticalDestination = 0.0; // current position range -1.0 to +1.0 horizontalPosition = RangeToNormalized(horizontalServo->Get(), 1); verticalPosition = RangeToNormalized(verticalServo->Get(), 1); // incremental tasking toward dest (-1.0 to 1.0) float incrementH, incrementV; // set servos to start at center position setServoPositions(horizontalDestination, verticalDestination); /* for controlling loop execution time */ float loopTime = 0.05; double currentTime = GetTime(); double lastTime = currentTime; double savedImageTimestamp = 0.0; bool foundColor = false; bool staleImage = false; while (IsAutonomous()) { /* calculate gimbal position based on color found */ if (FindColor (mode, &td.hue, &td.saturation, &td.luminance, &par, &cReport)) { foundColor = true; panIncrement = 0; // reset pan if (par.imageTimestamp == savedImageTimestamp) { // This image has been processed already, // so don't do anything for this loop staleImage = true; } else { staleImage = false; savedImageTimestamp = par.imageTimestamp; // compute final H & V destination horizontalDestination = par.center_mass_x_normalized; verticalDestination = par.center_mass_y_normalized; } // ShowActivity("Found color "); } else { // need to pan foundColor = false; // ShowActivity("No color found"); } PrintReport(&cReport); if (foundColor && !staleImage) { /* Move the servo a bit each loop toward the destination. * Alternative ways to task servos are to move immediately * vs. incrementally toward the final * destination. Incremental method reduces the need for * calibration of the servo movement while moving toward * the target. */ incrementH = horizontalDestination - horizontalPosition; incrementV = verticalPosition - verticalDestination; adjustServoPositions(incrementH, -incrementV); ShowActivity ("** %s found: Servo: x: %f y: %f increment: %f y: %f ", td.name, horizontalDestination, verticalDestination, incrementH, incrementV); } else if (!staleImage) { /* pan to find color after a short wait to settle servos * panning must start directly after panInit or timing * will be off */ // adjust sine wave for panning based on last movement // direction if (horizontalDestination > 0.0) { sinStart = PI / 2.0; } else { sinStart = -PI / 2.0; } if (panIncrement == 3) { panInit(); } else if (panIncrement > 3) { panForTarget(horizontalServo, sinStart); /* Vertical action: center the vertical after several * loops searching */ if (panIncrement == 20) { verticalServo->Set(0.5); } } panIncrement++; } // end if found color dashboardData.UpdateAndSend(); // sleep to keep loop at constant rate // elapsed time can vary significantly due to debug printout currentTime = GetTime(); lastTime = currentTime; if (loopTime > ElapsedTime(lastTime)) { Wait(loopTime - ElapsedTime(lastTime)); } } // end while myRobot->Drive(0.0, 0.0); // stop robot DPRINTF(LOG_DEBUG, "end autonomous"); ShowActivity ("Autonomous end "); } // end autonomous
void RawControl::camAng(float angle) { tilt->Set(angle); //((tilt->GetMaxAngle())-(tilt->GetMinAngle()))/2; }
/** * Adjusts the servo gimbal based on the color tracked. * Driving the robot or operating an arm based on color input from gimbal-mounted * camera is currently left as an exercise for the teams. */ void Autonomous(void) { char funcName[] = "Autonomous"; DPRINTF(LOG_DEBUG, "Autonomous"); DPRINTF(LOG_DEBUG, "SERVO - looking for COLOR %s ABOVE %s", td2.name, td1.name); // initialize position and destination variables // position settings range from -1 to 1 // setServoPositions is a wrapper that handles the conversion to range for servo horizontalDestination = 0.0; // final destination range -1.0 to +1.0 verticalDestination = 0.0; // initialize pan variables // incremental tasking toward dest (-1.0 to 1.0) float incrementH, incrementV; // pan needs a 1-up number for each call int panIncrement = 0; // current position range -1.0 to +1.0 horizontalPosition = RangeToNormalized(horizontalServo->Get(), 1); verticalPosition = RangeToNormalized(verticalServo->Get(), 1); // set servos to start at center position setServoPositions(horizontalDestination, verticalDestination); // for controlling loop execution time float loopTime = 0.1; //float loopTime = 0.05; double currentTime = GetTime(); double lastTime = currentTime; // search variables bool foundColor = 0; double savedImageTimestamp = 0.0; bool staleImage = false; while (IsAutonomous()) { //GetWatchdog().Feed(); // turn watchdog off while debugging // calculate gimbal position based on colors found if (FindTwoColors(td1, td2, ABOVE, &par)) { //PrintReport(&par); foundColor = true; // reset pan panIncrement = 0; if (par.imageTimestamp == savedImageTimestamp) { // This image has been processed already, // so don't do anything for this loop staleImage = true; DPRINTF(LOG_DEBUG, "STALE IMAGE"); } else { // The target was recognized // save the timestamp staleImage = false; savedImageTimestamp = par.imageTimestamp; DPRINTF(LOG_DEBUG, "image timetamp: %lf", savedImageTimestamp); // Here is where your game-specific code goes // when you recognize the target // get center of target // TODO: use par.average_mass_x_normalized and // par.average_mass_y_normalized after updated WPILib is distributed horizontalDestination = par.center_mass_x_normalized; verticalDestination = par.center_mass_y_normalized; } } else { // need to pan foundColor = false; } if (foundColor && !staleImage) { /* Move the servo a bit each loop toward the destination. * Alternative ways to task servos are to move immediately vs. * incrementally toward the final destination. Incremental method * reduces the need for calibration of the servo movement while * moving toward the target. */ incrementH = horizontalDestination - horizontalPosition; // you may need to reverse this based on your vertical servo installation //incrementV = verticalPosition - verticalDestination; incrementV = verticalDestination - verticalPosition; adjustServoPositions(incrementH, incrementV); ShowActivity("** %s & %s found: Servo: x: %f y: %f ** ", td1.name, td2.name, horizontalDestination, verticalDestination); } else { //if (!staleImage) { // new image, but didn't find two colors // adjust sine wave for panning based on last movement direction if (horizontalDestination > 0.0) { sinStart = PI / 2.0; } else { sinStart = -PI / 2.0; } /* pan to find color after a short wait to settle servos * panning must start directly after panInit or timing will be off */ if (panIncrement == 3) { panInit(8.0); // number of seconds for a pan } else if (panIncrement > 3) { panForTarget(horizontalServo, sinStart); /* Vertical action: In case the vertical servo is pointed off center, * center the vertical after several loops searching */ if (panIncrement == 20) { verticalServo->Set(0.5); } } panIncrement++; ShowActivity ("** %s and %s not found ", td1.name, td2.name); } // end if found color // sleep to keep loop at constant rate // this helps keep pan consistant // elapsed time can vary significantly due to debug printout currentTime = GetTime(); lastTime = currentTime; if (loopTime > ElapsedTime(lastTime)) { Wait(loopTime - ElapsedTime(lastTime)); // seconds } } // end while DPRINTF(LOG_DEBUG, "end Autonomous"); ShowActivity ("Autonomous end "); } // end autonomous