001/* 002 * Software License Agreement 003 * 004 * Copyright (C) Cross The Road Electronics. All rights 005 * reserved. 006 * 007 * Cross The Road Electronics (CTRE) licenses to you the right to 008 * use, publish, and distribute copies of CRF (Cross The Road) firmware files (*.crf) and Software 009 * API Libraries ONLY when in use with Cross The Road Electronics hardware products. 010 * 011 * THE SOFTWARE AND DOCUMENTATION ARE PROVIDED "AS IS" WITHOUT 012 * WARRANTY OF ANY KIND, EITHER EXPRESS OR IMPLIED, INCLUDING WITHOUT 013 * LIMITATION, ANY WARRANTY OF MERCHANTABILITY, FITNESS FOR A 014 * PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT SHALL 015 * CROSS THE ROAD ELECTRONICS BE LIABLE FOR ANY INCIDENTAL, SPECIAL, 016 * INDIRECT OR CONSEQUENTIAL DAMAGES, LOST PROFITS OR LOST DATA, COST OF 017 * PROCUREMENT OF SUBSTITUTE GOODS, TECHNOLOGY OR SERVICES, ANY CLAIMS 018 * BY THIRD PARTIES (INCLUDING BUT NOT LIMITED TO ANY DEFENSE 019 * THEREOF), ANY CLAIMS FOR INDEMNITY OR CONTRIBUTION, OR OTHER 020 * SIMILAR COSTS, WHETHER ASSERTED ON THE BASIS OF CONTRACT, TORT 021 * (INCLUDING NEGLIGENCE), BREACH OF WARRANTY, OR OTHERWISE 022 */ 023package com.ctre.phoenix.sensors; 024 025import com.ctre.phoenix.ErrorCode; 026import com.ctre.phoenix.ErrorCollection; 027import com.ctre.phoenix.ParamEnum; 028 029/** 030 * Pigeon 2 Class. Class supports communicating over CANbus. 031 * 032 * <pre> 033 * {@code 034 * // Example usage of a Pigeon 2 035 * Pigeon2 pigeon = new Pigeon2(0); // creates a new Pigeon2 with ID 0 036 * 037 * Pigeon2Configuration config = new Pigeon2Configuration(); 038 * // set mount pose as rolled 90 degrees counter-clockwise 039 * config.MountPoseYaw = 0; 040 * config.MountPosePitch = 0; 041 * config.MountPoseRoll = 90; 042 * pigeon.configAllSettings(config); 043 * 044 * System.out.println(pigeon.getYaw()); // prints the yaw of the Pigeon 045 * System.out.println(pigeon.getPitch()); // prints the pitch of the Pigeon 046 * System.out.println(pigeon.getRoll()); // prints the roll of the Pigeon 047 * 048 * double gravityVec[] = new double[3]; 049 * pigeon.getGravityVector(gravityVec); // gets the gravity vector of the Pigeon 2 050 * 051 * ErrorCode error = pigeon.getLastError(); // gets the last error generated by the Pigeon 052 * Pigeon2_Faults faults = new Pigeon2_Faults(); 053 * ErrorCode faultsError = pigeon.getFaults(faults); // fills faults with the current Pigeon 2 faults; returns the last error generated 054 * } 055 * </pre> 056 */ 057public class Pigeon2 extends BasePigeon { 058 /** 059 * Create a Pigeon object that communicates with Pigeon on CAN Bus. 060 * 061 * @param deviceNumber 062 * CAN Device Id of Pigeon [0,62] 063 * @param canbus Name of the CANbus; can be a SocketCAN interface (on Linux), 064 * or a CANivore device name or serial number 065 */ 066 public Pigeon2(int deviceNumber, String canbus) { 067 super(deviceNumber, "v2", canbus); 068 } 069 /** 070 * Create a Pigeon object that communicates with Pigeon on CAN Bus. 071 * 072 * @param deviceNumber 073 * CAN Device Id of Pigeon [0,62] 074 */ 075 public Pigeon2(int deviceNumber) { 076 this(deviceNumber, ""); 077 } 078 079 /** 080 * Configure the mounting pose of the Pigeon2.<p> 081 * This is the Yaw-Pitch-Roll the Pigeon2 underwent to get to its current 082 * orientation, referenced from the robot's point of view.<p> 083 * This is only necessary if the Pigeon2 is mounted at an exotic angle 084 * near the gimbal lock point or not forward. <p> 085 * If the pigeon is relatively flat and pointed forward, this is not needed.<p> 086 * <p> 087 * Examples:<p> 088 * If the Pigeon2 is pointed directly right, that corresponds to a -90 yaw, 089 * 0 pitch, and 0 roll, as it yaw'd 90 degrees clockwise.<p> 090 * If the Pigeon2 points upwards, that's a 0 yaw, -90 pitch, 0 roll, as it 091 * pitched 90 degrees clockwise. <p> 092 * @param yaw Yaw angle needed to reach the current orientation in degrees. 093 * @param pitch Pitch angle needed to reach the current orientation in degrees. 094 * @param roll Roll angle needed to reach the current orientation in degrees. 095 * @param timeoutMs Config timeout in milliseconds. 096 * @return Worst error code of all config sets. 097 */ 098 public ErrorCode configMountPose(double yaw, double pitch, double roll, int timeoutMs) { 099 ErrorCode err = ErrorCode.OK; 100 if(err == ErrorCode.OK) { 101 err = configMountPoseYaw(yaw, timeoutMs); 102 } 103 if(err == ErrorCode.OK) { 104 err = configMountPosePitch(pitch, timeoutMs); 105 } 106 if(err == ErrorCode.OK) { 107 err = configMountPoseRoll(roll, timeoutMs); 108 } 109 return err; 110 } 111 /** 112 * Configure the mounting pose of the Pigeon2.<p> 113 * This is the Yaw-Pitch-Roll the Pigeon2 underwent to get to its current 114 * orientation, referenced from the robot's point of view.<p> 115 * This is only necessary if the Pigeon2 is mounted at an exotic angle 116 * near the gimbal lock point or not forward. <p> 117 * If the pigeon is relatively flat and pointed forward, this is not needed.<p> 118 * <p> 119 * Examples:<p> 120 * If the Pigeon2 is pointed directly right, that corresponds to a -90 yaw, 121 * 0 pitch, and 0 roll, as it yaw'd 90 degrees clockwise.<p> 122 * If the Pigeon2 points upwards, that's a 0 yaw, -90 pitch, 0 roll, as it 123 * pitched 90 degrees clockwise. <p> 124 * @param yaw Yaw angle needed to reach the current orientation in degrees. 125 * @param pitch Pitch angle needed to reach the current orientation in degrees. 126 * @param roll Roll angle needed to reach the current orientation in degrees. 127 * @return Worst error code of all config sets. 128 */ 129 public ErrorCode configMountPose(double yaw, double pitch, double roll) { 130 return configMountPose(yaw, pitch, roll, 0); 131 } 132 133 /** 134 * Configure the mounting pose Yaw of the Pigeon2. 135 * See {@link #configMountPose(double, double, double, int)} 136 * 137 * @param yaw Yaw angle needed to reach the current orientation in degrees. 138 * @param timeoutMs Config timeout in milliseconds. 139 * @return ErrorCode of configSet 140 */ 141 public ErrorCode configMountPoseYaw(double yaw, int timeoutMs) { 142 return configSetParameter(ParamEnum.eConfigMountPoseYaw, yaw, 0, 0, timeoutMs); 143 } 144 /** 145 * Configure the mounting pose Yaw of the Pigeon2. 146 * See {@link #configMountPose(double, double, double)} 147 * 148 * @param yaw Yaw angle needed to reach the current orientation in degrees. 149 * @return ErrorCode of configSet 150 */ 151 public ErrorCode configMountPoseYaw(double yaw) { 152 return configMountPoseYaw(yaw, 0); 153 } 154 155 /** 156 * Configure the mounting pose Pitch of the Pigeon2. 157 * See {@link #configMountPose(double, double, double, int)} 158 * 159 * @param pitch Pitch angle needed to reach the current orientation in degrees. 160 * @param timeoutMs Config timeout in milliseconds. 161 * @return ErrorCode of configSet 162 */ 163 public ErrorCode configMountPosePitch(double pitch, int timeoutMs) { 164 return configSetParameter(ParamEnum.eConfigMountPosePitch, pitch, 0, 0, timeoutMs); 165 } 166 /** 167 * Configure the mounting pose Pitch of the Pigeon2. 168 * See {@link #configMountPose(double, double, double)} 169 * 170 * @param pitch Pitch angle needed to reach the current orientation in degrees. 171 * @return ErrorCode of configSet 172 */ 173 public ErrorCode configMountPosePitch(double pitch) { 174 return configMountPosePitch(pitch, 0); 175 } 176 177 /** 178 * Configure the mounting pose Roll of the Pigeon2. 179 * See {@link #configMountPose(double, double, double, int)} 180 * 181 * @param roll Roll angle needed to reach the current orientation in degrees. 182 * @param timeoutMs Config timeout in milliseconds. 183 * @return ErrorCode of configSet 184 */ 185 public ErrorCode configMountPoseRoll(double roll, int timeoutMs) { 186 return configSetParameter(ParamEnum.eConfigMountPoseRoll, roll, 0, 0, timeoutMs); 187 } 188 /** 189 * Configure the mounting pose Roll of the Pigeon2. 190 * See {@link #configMountPose(double, double, double)} 191 * 192 * @param roll Roll angle needed to reach the current orientation in degrees. 193 * @return ErrorCode of configSet 194 */ 195 public ErrorCode configMountPoseRoll(double roll) { 196 return configMountPoseRoll(roll, 0); 197 } 198 /** 199 * Enables the magnetometer fusion for Pigeon2. This is **not** recommended for FRC use 200 * 201 * @param enable Boolean to enable/disable magnetometer fusion 202 * @param timeoutMs Config timeout in milliseconds. 203 * @return ErrorCode Status of the config response 204 */ 205 public ErrorCode configEnableCompass(boolean enable, int timeoutMs) { 206 return configSetParameter(ParamEnum.eChangeCompassUse, enable ? 1 : 0, 0, 0, timeoutMs); 207 } 208 /** 209 * Enables the magnetometer fusion for Pigeon2. This is **not** recommended for FRC use 210 * 211 * @param enable Boolean to enable/disable magnetometer fusion 212 * @return ErrorCode Status of the config response 213 */ 214 public ErrorCode configEnableCompass(boolean enable) { 215 return configEnableCompass(enable, 0); 216 } 217 /** 218 * Disables temperature compensation from Pigeon2. 219 * 220 * @param disable Boolean to disable/enable temperature compensation 221 * @param timeoutMs Config timeout in milliseconds. 222 * @return ErrorCode Status of the config response 223 */ 224 public ErrorCode configDisableTemperatureCompensation(boolean disable, int timeoutMs) { 225 return configSetParameter(ParamEnum.eDontRunThermComp, disable ? 1 : 0, 0, 0, timeoutMs); 226 } 227 /** 228 * Disables temperature compensation from Pigeon2. 229 * 230 * @param disable Boolean to disable/enable temperature compensation 231 * @return ErrorCode Status of the config response 232 */ 233 public ErrorCode configDisableTemperatureCompensation(boolean disable) { 234 return configDisableTemperatureCompensation(disable, 0); 235 } 236 /** 237 * Disables the no-motion calibration from Pigeon2 238 * 239 * @param disable Boolean to disable/enable no-motion calibration 240 * @param timeoutMs Config timeout in milliseconds. 241 * @return ErrorCode Status of the config response 242 */ 243 public ErrorCode configDisableNoMotionCalibration(boolean disable, int timeoutMs) { 244 return configSetParameter(ParamEnum.eSetNoMotionCalDisable, disable ? 1 : 0, 0, 0, timeoutMs); 245 } 246 /** 247 * Disables the no-motion calibration from Pigeon2 248 * 249 * @param disable Boolean to disable/enable no-motion calibration 250 * @return ErrorCode Status of the config response 251 */ 252 public ErrorCode configDisableNoMotionCalibration(boolean disable) { 253 return configDisableNoMotionCalibration(disable, 0); 254 } 255 /** 256 * Performs an offset calibration on gyro bias 257 * 258 * @param timeoutMs Config timeout in milliseconds. 259 * @return ErrorCode Status of the config response 260 */ 261 public ErrorCode zeroGyroBiasNow(int timeoutMs) { 262 return configSetParameter(ParamEnum.eGyroBias, 1, 0, 0, timeoutMs); 263 } 264 /** 265 * Performs an offset calibration on gyro bias 266 * 267 * @return ErrorCode Status of the config response 268 */ 269 public ErrorCode zeroGyroBiasNow() { 270 return zeroGyroBiasNow(0); 271 } 272 273 /** 274 * Configures all persistent settings. 275 * 276 * @param allConfigs Object with all of the persistant settings 277 * @param timeoutMs 278 * Timeout value in ms. If nonzero, function will wait for 279 * config success and report an error if it times out. 280 * If zero, no blocking or checking is performed. 281 * 282 * @return Error Code generated by function. 0 indicates no error. 283 */ 284 public ErrorCode configAllSettings(Pigeon2Configuration settings, int timeoutMs) { 285 ErrorCollection errorCollection = new ErrorCollection(); 286 errorCollection.NewError(configFactoryDefault(timeoutMs)); 287 288 if(Pigeon2ConfigUtil.mountPoseYawDifferent(settings)) errorCollection.NewError(configMountPoseYaw(settings.MountPoseYaw, timeoutMs)); 289 if(Pigeon2ConfigUtil.mountPosePitchDifferent(settings)) errorCollection.NewError(configMountPosePitch(settings.MountPosePitch, timeoutMs)); 290 if(Pigeon2ConfigUtil.mountPoseRollDifferent(settings)) errorCollection.NewError(configMountPoseRoll(settings.MountPoseRoll, timeoutMs)); 291 if(Pigeon2ConfigUtil.enableCompassDifferent(settings)) errorCollection.NewError(configEnableCompass(settings.EnableCompass, timeoutMs)); 292 if(Pigeon2ConfigUtil.disableNoMotionCalibrationDifferent(settings)) errorCollection.NewError(configDisableNoMotionCalibration(settings.DisableNoMotionCalibration, timeoutMs)); 293 if(Pigeon2ConfigUtil.disableTemperatureCompensationDifferent(settings)) errorCollection.NewError(configDisableTemperatureCompensation(settings.DisableTemperatureCompensation, timeoutMs)); 294 if(Pigeon2ConfigUtil.customParam0Different(settings)) errorCollection.NewError(configSetCustomParam(settings.customParam0, 0, timeoutMs)); 295 if(Pigeon2ConfigUtil.customParam1Different(settings)) errorCollection.NewError(configSetCustomParam(settings.customParam1, 1, timeoutMs)); 296 297 return errorCollection._worstError; 298 } 299 /** 300 * Configures all persistent settings. 301 * 302 * @param allConfigs Object with all of the persistant settings 303 * 304 * @return Error Code generated by function. 0 indicates no error. 305 */ 306 public ErrorCode configAllSettings(Pigeon2Configuration settings) { 307 int timeoutMs = 50; 308 return configAllSettings(settings, timeoutMs); 309 } 310 311 /** 312 * Get the Gravity Vector. 313 * 314 * This provides a vector that points toward ground. This is useful for applications like 315 * an arm, where the z-value of the gravity vector corresponds to the feed-forward needed 316 * to hold the arm steady. 317 * The gravity vector is calculated after the mount pose, so if the pigeon is where it was 318 * mounted, the gravity vector is {0, 0, 1}. 319 * @param gravVector Pass in a double array of size 3 to get the gravity vector 320 * @return Errorcode of getter 321 */ 322 public ErrorCode getGravityVector(double[] gravVector) { 323 int retval = PigeonImuJNI.JNI_GetGravityVector(getHandle(), gravVector); 324 return ErrorCode.valueOf(retval); 325 } 326 /** 327 * Gets the fault status 328 * 329 * @param toFill 330 * Container for fault statuses. 331 * @return Error Code generated by function. 0 indicates no error. 332 */ 333 public ErrorCode getFaults(Pigeon2_Faults toFill) { 334 int bits = PigeonImuJNI.JNI_GetFaults(getHandle()); 335 toFill.update(bits); 336 return getLastError(); 337 } 338 /** 339 * Gets the sticky fault status 340 * 341 * @param toFill 342 * Container for sticky fault statuses. 343 * @return Error Code generated by function. 0 indicates no error. 344 */ 345 public ErrorCode getStickyFaults(Pigeon2_Faults toFill) { 346 int bits = PigeonImuJNI.JNI_GetStickyFaults(getHandle()); 347 toFill.update(bits); 348 return getLastError(); 349 } 350 /** 351 * Gets all persistant settings. 352 * 353 * @param allConfigs Object with all of the persistant settings 354 * @param timeoutMs 355 * Timeout value in ms. If nonzero, function will wait for 356 * config success and report an error if it times out. 357 * If zero, no blocking or checking is performed. 358 */ 359 public void getAllConfigs(Pigeon2Configuration allConfigs, int timeoutMs) { 360 allConfigs.MountPoseYaw = configGetParameter(ParamEnum.eConfigMountPoseYaw, 0, timeoutMs); 361 allConfigs.MountPosePitch = configGetParameter(ParamEnum.eConfigMountPosePitch, 0, timeoutMs); 362 allConfigs.MountPoseRoll = configGetParameter(ParamEnum.eConfigMountPoseRoll, 0, timeoutMs); 363 allConfigs.DisableNoMotionCalibration = configGetParameter(ParamEnum.eSetNoMotionCalDisable, 0, timeoutMs) != 0; 364 allConfigs.DisableTemperatureCompensation = configGetParameter(ParamEnum.eDontRunThermComp, 0, timeoutMs) != 0; 365 allConfigs.EnableCompass = configGetParameter(ParamEnum.eChangeCompassUse, 0, timeoutMs) != 0; 366 367 allConfigs.customParam0 = (int)configGetParameter(ParamEnum.eCustomParam, 0, timeoutMs); 368 allConfigs.customParam1 = (int)configGetParameter(ParamEnum.eCustomParam, 1, timeoutMs); 369 } 370 /** 371 * Gets all persistant settings. 372 * 373 * @param allConfigs Object with all of the persistant settings 374 */ 375 public void getAllConfigs(Pigeon2Configuration allConfigs) { 376 int timeoutMs = 50; 377 getAllConfigs(allConfigs, timeoutMs); 378 } 379}