C++ AnalogGyro::Reset方法代码示例

/**
  * 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";
}

    void AutonomousInit()
    {
        gyro.Reset();

        if(Auto==1)
        {
            d1=84;
        }
        else if(Auto==2)
        {
            d1=36;
        }
        else if(Auto==3)
        {
            d1=12;
            ang=-ang;
        }
        else if(Auto==4)
        {
            d1=60;
            ang=-ang;
        }

        autoLoopCounter = 0;
        EncVal1=(d1/cir)*Rev;

    }

// During every loop intervel of the teleop period
void Robot::TeleopPeriodic() {
	// Tank drive, both left and right joystick control their respective motor along the
	// joystick's 'y' axis
	//myRobot.TankDrive(-rStick.GetRawAxis(RIGHT_STICK_Y), -lStick.GetRawAxis(LEFT_STICK_Y));

	// Choose the teleop drive option
	for(int c=0;c<7;c++) {
		buttonDone[c] = false;
	}

	DriverControl(driveOption);

	if(fabs(gyro.GetRate()) > GYRO_DRIFT_VALUE) {
	editedGyroRate = gyro.GetAngle();
	}
	else {
		editedGyroRate = 0;
		gyro.Reset();
	}





	SmartDashboard::PutString("Operation Type:", "TeleOp");

	// Print gyro data
	if (showGyro == true) {
		SmartDashboard::PutNumber("Gyro Angle", gyro.GetAngle()*GYRO_SCALE_FACTOR);
		SmartDashboard::PutNumber("Gyro Rate (Raw)", gyro.GetRate()*GYRO_SCALE_FACTOR);
		SmartDashboard::PutNumber("Gyro Rate (Edited)", editedGyroRate*GYRO_SCALE_FACTOR);
	}


	// Print out the encoder data
	// Raw encoder data
	if (showEncoderRaw == true) {
		SmartDashboard::PutNumber("Encoder L get raw", encoder1.GetRaw());
		SmartDashboard::PutNumber("Encoder R get raw", encoder2.GetRaw());

	}

	// The delta encoder change
	if (showEncoderRate == true) {
		SmartDashboard::PutNumber("Encoder L get rate", encoder1.GetRate());
		SmartDashboard::PutNumber("Encoder R (reversed) get rate", encoder2.GetRate());
	}

	// Print the encoder index
	if (showEncoderIndex == true) {
		SmartDashboard::PutNumber("Encoder L index", encoder1.GetFPGAIndex());
		SmartDashboard::PutNumber("Encoder R index", encoder2.GetFPGAIndex());
	}
}

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

// When the robot is on autonomous period, it will drive forwards at half speed for about two
// seconds, then stops
void Robot::AutonomousPeriodic() {
	/*
	if(autoLoopCounter < 100) { //Check if we've completed 100 loops (approximately 2 seconds)
		myRobot.Drive(-0.5, 0.0); 	// drive forwards half speed
		autoLoopCounter++;
	} else {
		myRobot.Drive(0.0, 0.0); 	// stop robot
	}
	*/



	// Edits the gyro angle to account for drift
	if (fabs(gyro.GetRate()) > GYRO_DRIFT_VALUE)
		editedGyroAngle = gyro.GetAngle();
	else {
		gyro.Reset();
		editedGyroAngle = 0;
	}



	// Adjust course based on gyro data
	if (editedGyroAngle > 0)
		autoTurn = 0.3;
	else if (editedGyroAngle < 0)
		autoTurn = -0.3;
	else
		autoTurn = 0;

	// Go foward 3 feet at 1/3 speed, stop (adjusting for drift)
	if (encoder1.GetRaw() < ONE_FOOT_LEFT_ENCODER*3 || encoder2.GetRaw() < ONE_FOOT_RIGHT_ENCODER*3)
		myRobot.Drive(0.3, autoTurn);
	else
		myRobot.Drive(0.0, 0.0);


	// Print the raw encoder data
	SmartDashboard::PutNumber("Encoder L get raw", encoder1.GetRaw());
	SmartDashboard::PutNumber("Encoder R get raw", encoder2.GetRaw());
}

// To determine which drive control type the robot to use
// In case user operator wants different controls
void Robot::DriverControl(int driveControl) {
	switch (driveControl) {
	// Arcade drive (think racing games) w/ 1 joysticks
	case ARCADE_1:
		// If the turn is outside the joystick's standard drift, robot will assume the robot is turning
		// If not, robot will assume it is pushed, or has naturally drifted
		if (fabs(rStick.GetRawAxis(X_AXIS)) > JOYSTICK_STANDARD_DRIFT) {
			myRobot.ArcadeDrive(rStick.GetRawAxis(Y_AXIS), -rStick.GetRawAxis(X_AXIS));
			gyro.Reset();
		} else {
			// The turn will be the opposite of what the gyro says the angle of unintentional drift is,
			// which will have the robot go straight
			myRobot.ArcadeDrive(rStick.GetRawAxis(Y_AXIS), editedGyroRate*GYRO_SCALE_FACTOR);
		}

		break;

	// Arcade drive w/ 2 joysticks
	case ARCADE_2:
		if (fabs(rStick.GetRawAxis(X_AXIS)) > JOYSTICK_STANDARD_DRIFT) {
			myRobot.ArcadeDrive(-lStick.GetRawAxis(Y_AXIS), -rStick.GetRawAxis(X_AXIS));
			gyro.Reset();
		} else {
			myRobot.ArcadeDrive(-lStick.GetRawAxis(Y_AXIS), editedGyroRate*GYRO_SCALE_FACTOR);
		}

		break;

	// Arcade drive w/ left stick on gamepad (the knockoff xbox controller)
	case ARCADE_GAMEPAD_1:
		if (fabs(gamePad.GetRawAxis(GAMEPAD_LEFT_STICK_X)) > JOYSTICK_STANDARD_DRIFT) {
			myRobot.ArcadeDrive(gamePad.GetRawAxis(GAMEPAD_LEFT_STICK_Y), -gamePad.GetRawAxis(GAMEPAD_LEFT_STICK_X));
			gyro.Reset();
		} else {
			myRobot.ArcadeDrive(gamePad.GetRawAxis(GAMEPAD_LEFT_STICK_Y), editedGyroRate*GYRO_SCALE_FACTOR);
		}

		break;

	// Arcade drive w/ BOTH gamepad
	case ARCADE_GAMEPAD_2:
		if (fabs(gamePad.GetRawAxis(GAMEPAD_RIGHT_STICK_X)) > JOYSTICK_STANDARD_DRIFT) {
			myRobot.ArcadeDrive(-gamePad.GetRawAxis(GAMEPAD_LEFT_STICK_Y), -gamePad.GetRawAxis(GAMEPAD_RIGHT_STICK_X));
			gyro.Reset();
		} else {
			myRobot.ArcadeDrive(-gamePad.GetRawAxis(GAMEPAD_LEFT_STICK_Y), editedGyroRate*GYRO_SCALE_FACTOR);
		}

		break;

	// Tank drive (requires 2 sticks; each stick controls its respective 'tread' or side;
	//			   Ex: moving the right stick moves only the wheels on the right) w/ gamepad
	case TANK_GAMEPAD:
		joystickDifference = gamePad.GetRawAxis(GAMEPAD_LEFT_STICK_Y) - gamePad.GetRawAxis(GAMEPAD_RIGHT_STICK_Y);
		joystickAverage = (gamePad.GetRawAxis(GAMEPAD_LEFT_STICK_Y) + gamePad.GetRawAxis(GAMEPAD_RIGHT_STICK_Y))/2;
		if (fabs(joystickDifference) > TANK_TURN_THRESHOLD) {
			myRobot.TankDrive(gamePad.GetRawAxis(GAMEPAD_LEFT_STICK_Y), gamePad.GetRawAxis(GAMEPAD_RIGHT_STICK_Y));
			gyro.Reset();
		} else {
			myRobot.ArcadeDrive(joystickAverage, editedGyroRate*GYRO_SCALE_FACTOR);
		}

		break;

	// Tank drive w/ joysticks
	case TANK_2:
		joystickDifference = lStick.GetRawAxis(LEFT_STICK_Y) - rStick.GetRawAxis(RIGHT_STICK_Y);
		joystickAverage = (lStick.GetRawAxis(LEFT_STICK_Y) + rStick.GetRawAxis(RIGHT_STICK_Y))/2;
		if (fabs(joystickDifference) > TANK_TURN_THRESHOLD) {
			myRobot.TankDrive(lStick.GetRawAxis(LEFT_STICK_Y), rStick.GetRawAxis(RIGHT_STICK_Y));
			gyro.Reset();
		} else {
			myRobot.ArcadeDrive(joystickAverage, -rStick.GetRawAxis(RIGHT_STICK_Y));
		}

		break;
	}
}

// Starts at the beginning of the teleop (user controlled) period
void Robot::TeleopInit() {

	gyro.Reset();
}

// During every loop intervel of the teleop period
void Robot::TeleopPeriodic() {
	// Choose the teleop drive option
	DriverControl(driveOption);


	// A simple thing to test a motor
	if (gamePad.GetRawButton(GAMEPAD_BUTTON_A) == true)
		ballManipulator.GoUp();
	else
		ballManipulator.StopAll();



	// Edits the gyro rate to account for drift
	/*
	editedGyroRate = gyro.GetRate();
	if (gyro.GetRate() > GYRO_DRIFT_VALUE_MIN && gyro.GetRate() < GYRO_DRIFT_VALUE_MAX) {
		editedGyroRate = 0;
	} else {
		editedGyroRate += GYRO_DRIFT_VALUE_AVERAGE;
	}
	*/

	// Edits the gyro angle to account for drift
	if (fabs(gyro.GetRate()) > GYRO_DRIFT_VALUE)
		editedGyroAngle = gyro.GetAngle();
	else {
		gyro.Reset();
		editedGyroAngle = 0;
	}


	// Reset the buttons
	for(int c=0; c<7; c++)
		buttonDone[c] = false;



	// To reset encoder data for the wheels
	if (gamePad.GetRawButton(GAMEPAD_BUTTON_Y) == true) {
			encoder1.Reset();
			encoder2.Reset();
	}

	// Print gyro data
	if (showGyro == true) {
		SmartDashboard::PutNumber("Gyro Angle (Raw)", gyro.GetAngle()*GYRO_SCALE_FACTOR);
		SmartDashboard::PutNumber("Gyro Angle (Edited)", editedGyroAngle*GYRO_SCALE_FACTOR);
		SmartDashboard::PutNumber("Gyro Rate (Raw)", gyro.GetRate()*GYRO_SCALE_FACTOR);
		SmartDashboard::PutNumber("Gyro Rate (Edited)", editedGyroRate*GYRO_SCALE_FACTOR);
	}


	// Print out the encoder data
	// Raw encoder data
//	if (showEncoderRaw == true) {
		SmartDashboard::PutNumber("Encoder L get raw", encoder1.GetRaw());
		SmartDashboard::PutNumber("Encoder R get raw", encoder2.GetRaw());

//	}

	// The delta encoder change
	if (showEncoderRate == true) {
		SmartDashboard::PutNumber("Encoder L get rate", encoder1.GetRate());
		SmartDashboard::PutNumber("Encoder R (reversed) get rate", encoder2.GetRate());
	}

	// Print the encoder index
	if (showEncoderIndex == true) {
		SmartDashboard::PutNumber("Encoder L index", encoder1.GetFPGAIndex());
		SmartDashboard::PutNumber("Encoder R index", encoder2.GetFPGAIndex());
	}
}