diff --git a/wpilibc/athena/include/CAN/can_proto.h b/wpilibc/athena/include/CAN/can_proto.h deleted file mode 100644 index c2737d7854..0000000000 --- a/wpilibc/athena/include/CAN/can_proto.h +++ /dev/null @@ -1,415 +0,0 @@ -//***************************************************************************** -// -// can_proto.h - Definitions for the CAN protocol used to communicate with the -// BDC motor controller. -// -// Copyright (c) 2008 Texas Instruments Incorporated. All rights reserved. -// TI Information - Selective Disclosure -// -//***************************************************************************** - -#ifndef __CAN_PROTO_H__ -#define __CAN_PROTO_H__ - -//***************************************************************************** -// -// The masks of the fields that are used in the message identifier. -// -//***************************************************************************** -#define CAN_MSGID_FULL_M 0x1fffffff -#define CAN_MSGID_DEVNO_M 0x0000003f -#define CAN_MSGID_API_M 0x0000ffc0 -#define CAN_MSGID_MFR_M 0x00ff0000 -#define CAN_MSGID_DTYPE_M 0x1f000000 -#define CAN_MSGID_DEVNO_S 0 -#define CAN_MSGID_API_S 6 -#define CAN_MSGID_MFR_S 16 -#define CAN_MSGID_DTYPE_S 24 - -//***************************************************************************** -// -// The Reserved device number values in the Message Id. -// -//***************************************************************************** -#define CAN_MSGID_DEVNO_BCAST 0x00000000 - -//***************************************************************************** -// -// The Reserved system control API numbers in the Message Id. -// -//***************************************************************************** -#define CAN_MSGID_API_SYSHALT 0x00000000 -#define CAN_MSGID_API_SYSRST 0x00000040 -#define CAN_MSGID_API_DEVASSIGN 0x00000080 -#define CAN_MSGID_API_DEVQUERY 0x000000c0 -#define CAN_MSGID_API_HEARTBEAT 0x00000140 -#define CAN_MSGID_API_SYNC 0x00000180 -#define CAN_MSGID_API_UPDATE 0x000001c0 -#define CAN_MSGID_API_FIRMVER 0x00000200 -#define CAN_MSGID_API_ENUMERATE 0x00000240 -#define CAN_MSGID_API_SYSRESUME 0x00000280 - -//***************************************************************************** -// -// The 32 bit values associated with the CAN_MSGID_API_STATUS request. -// -//***************************************************************************** -#define CAN_STATUS_CODE_M 0x0000ffff -#define CAN_STATUS_MFG_M 0x00ff0000 -#define CAN_STATUS_DTYPE_M 0x1f000000 -#define CAN_STATUS_CODE_S 0 -#define CAN_STATUS_MFG_S 16 -#define CAN_STATUS_DTYPE_S 24 - -//***************************************************************************** -// -// The Reserved manufacturer identifiers in the Message Id. -// -//***************************************************************************** -#define CAN_MSGID_MFR_NI 0x00010000 -#define CAN_MSGID_MFR_LM 0x00020000 -#define CAN_MSGID_MFR_DEKA 0x00030000 - -//***************************************************************************** -// -// The Reserved device type identifiers in the Message Id. -// -//***************************************************************************** -#define CAN_MSGID_DTYPE_BCAST 0x00000000 -#define CAN_MSGID_DTYPE_ROBOT 0x01000000 -#define CAN_MSGID_DTYPE_MOTOR 0x02000000 -#define CAN_MSGID_DTYPE_RELAY 0x03000000 -#define CAN_MSGID_DTYPE_GYRO 0x04000000 -#define CAN_MSGID_DTYPE_ACCEL 0x05000000 -#define CAN_MSGID_DTYPE_USONIC 0x06000000 -#define CAN_MSGID_DTYPE_GEART 0x07000000 -#define CAN_MSGID_DTYPE_UPDATE 0x1f000000 - -//***************************************************************************** -// -// LM Motor Control API Classes API Class and ID masks. -// -//***************************************************************************** -#define CAN_MSGID_API_CLASS_M 0x0000fc00 -#define CAN_MSGID_API_ID_M 0x000003c0 - -//***************************************************************************** -// -// LM Motor Control API Classes in the Message Id for non-broadcast. -// These are the upper 6 bits of the API field, the lower 4 bits determine -// the APIId. -// -//***************************************************************************** -#define CAN_API_MC_VOLTAGE 0x00000000 -#define CAN_API_MC_SPD 0x00000400 -#define CAN_API_MC_VCOMP 0x00000800 -#define CAN_API_MC_POS 0x00000c00 -#define CAN_API_MC_ICTRL 0x00001000 -#define CAN_API_MC_STATUS 0x00001400 -#define CAN_API_MC_PSTAT 0x00001800 -#define CAN_API_MC_CFG 0x00001c00 -#define CAN_API_MC_ACK 0x00002000 - -//***************************************************************************** -// -// The Stellaris Motor Class Control Voltage API definitions. -// -//***************************************************************************** -#define LM_API_VOLT \ - (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_VOLTAGE) -#define LM_API_VOLT_EN (LM_API_VOLT | (0 << CAN_MSGID_API_S)) -#define LM_API_VOLT_DIS (LM_API_VOLT | (1 << CAN_MSGID_API_S)) -#define LM_API_VOLT_SET (LM_API_VOLT | (2 << CAN_MSGID_API_S)) -#define LM_API_VOLT_SET_RAMP (LM_API_VOLT | (3 << CAN_MSGID_API_S)) -//##### FIRST BEGIN ##### -#define LM_API_VOLT_T_EN (LM_API_VOLT | (4 << CAN_MSGID_API_S)) -#define LM_API_VOLT_T_SET (LM_API_VOLT | (5 << CAN_MSGID_API_S)) -#define LM_API_VOLT_T_SET_NO_ACK (LM_API_VOLT | (7 << CAN_MSGID_API_S)) -//##### FIRST END ##### -#define LM_API_VOLT_SET_NO_ACK (LM_API_VOLT | (8 << CAN_MSGID_API_S)) - -//***************************************************************************** -// -// The Stellaris Motor Class Control API definitions for LM_API_VOLT_SET_RAMP. -// -//***************************************************************************** -#define LM_API_VOLT_RAMP_DIS 0 - -//***************************************************************************** -// -// The Stellaris Motor Class Control API definitions for CAN_MSGID_API_SYNC. -// -//***************************************************************************** -#define LM_API_SYNC_PEND_NOW 0 - -//***************************************************************************** -// -// The Stellaris Motor Class Speed Control API definitions. -// -//***************************************************************************** -#define LM_API_SPD (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_SPD) -#define LM_API_SPD_EN (LM_API_SPD | (0 << CAN_MSGID_API_S)) -#define LM_API_SPD_DIS (LM_API_SPD | (1 << CAN_MSGID_API_S)) -#define LM_API_SPD_SET (LM_API_SPD | (2 << CAN_MSGID_API_S)) -#define LM_API_SPD_PC (LM_API_SPD | (3 << CAN_MSGID_API_S)) -#define LM_API_SPD_IC (LM_API_SPD | (4 << CAN_MSGID_API_S)) -#define LM_API_SPD_DC (LM_API_SPD | (5 << CAN_MSGID_API_S)) -#define LM_API_SPD_REF (LM_API_SPD | (6 << CAN_MSGID_API_S)) -//##### FIRST BEGIN ##### -#define LM_API_SPD_T_EN (LM_API_SPD | (7 << CAN_MSGID_API_S)) -#define LM_API_SPD_T_SET (LM_API_SPD | (8 << CAN_MSGID_API_S)) -#define LM_API_SPD_T_SET_NO_ACK (LM_API_SPD | (10 << CAN_MSGID_API_S)) -//##### FIRST END ##### -#define LM_API_SPD_SET_NO_ACK (LM_API_SPD | (11 << CAN_MSGID_API_S)) - -//***************************************************************************** -// -// The Stellaris Motor Control Voltage Compensation Control API definitions. -// -//***************************************************************************** -#define LM_API_VCOMP \ - (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_VCOMP) -#define LM_API_VCOMP_EN (LM_API_VCOMP | (0 << CAN_MSGID_API_S)) -#define LM_API_VCOMP_DIS (LM_API_VCOMP | (1 << CAN_MSGID_API_S)) -#define LM_API_VCOMP_SET (LM_API_VCOMP | (2 << CAN_MSGID_API_S)) -#define LM_API_VCOMP_IN_RAMP (LM_API_VCOMP | (3 << CAN_MSGID_API_S)) -#define LM_API_VCOMP_COMP_RAMP (LM_API_VCOMP | (4 << CAN_MSGID_API_S)) -//##### FIRST BEGIN ##### -#define LM_API_VCOMP_T_EN (LM_API_VCOMP | (5 << CAN_MSGID_API_S)) -#define LM_API_VCOMP_T_SET (LM_API_VCOMP | (6 << CAN_MSGID_API_S)) -#define LM_API_VCOMP_T_SET_NO_ACK (LM_API_VCOMP | (8 << CAN_MSGID_API_S)) -//##### FIRST END ##### -#define LM_API_VCOMP_SET_NO_ACK (LM_API_VCOMP | (9 << CAN_MSGID_API_S)) - -//***************************************************************************** -// -// The Stellaris Motor Class Position Control API definitions. -// -//***************************************************************************** -#define LM_API_POS (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_POS) -#define LM_API_POS_EN (LM_API_POS | (0 << CAN_MSGID_API_S)) -#define LM_API_POS_DIS (LM_API_POS | (1 << CAN_MSGID_API_S)) -#define LM_API_POS_SET (LM_API_POS | (2 << CAN_MSGID_API_S)) -#define LM_API_POS_PC (LM_API_POS | (3 << CAN_MSGID_API_S)) -#define LM_API_POS_IC (LM_API_POS | (4 << CAN_MSGID_API_S)) -#define LM_API_POS_DC (LM_API_POS | (5 << CAN_MSGID_API_S)) -#define LM_API_POS_REF (LM_API_POS | (6 << CAN_MSGID_API_S)) -//##### FIRST BEGIN ##### -#define LM_API_POS_T_EN (LM_API_POS | (7 << CAN_MSGID_API_S)) -#define LM_API_POS_T_SET (LM_API_POS | (8 << CAN_MSGID_API_S)) -#define LM_API_POS_T_SET_NO_ACK (LM_API_POS | (10 << CAN_MSGID_API_S)) -//##### FIRST END ##### -#define LM_API_POS_SET_NO_ACK (LM_API_POS | (11 << CAN_MSGID_API_S)) - -//***************************************************************************** -// -// The Stellaris Motor Class Current Control API definitions. -// -//***************************************************************************** -#define LM_API_ICTRL \ - (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_ICTRL) -#define LM_API_ICTRL_EN (LM_API_ICTRL | (0 << CAN_MSGID_API_S)) -#define LM_API_ICTRL_DIS (LM_API_ICTRL | (1 << CAN_MSGID_API_S)) -#define LM_API_ICTRL_SET (LM_API_ICTRL | (2 << CAN_MSGID_API_S)) -#define LM_API_ICTRL_PC (LM_API_ICTRL | (3 << CAN_MSGID_API_S)) -#define LM_API_ICTRL_IC (LM_API_ICTRL | (4 << CAN_MSGID_API_S)) -#define LM_API_ICTRL_DC (LM_API_ICTRL | (5 << CAN_MSGID_API_S)) -//##### FIRST BEGIN ##### -#define LM_API_ICTRL_T_EN (LM_API_ICTRL | (6 << CAN_MSGID_API_S)) -#define LM_API_ICTRL_T_SET (LM_API_ICTRL | (7 << CAN_MSGID_API_S)) -#define LM_API_ICTRL_T_SET_NO_ACK (LM_API_ICTRL | (9 << CAN_MSGID_API_S)) -//##### FIRST END ##### -#define LM_API_ICTRL_SET_NO_ACK (LM_API_ICTRL | (10 << CAN_MSGID_API_S)) - -//***************************************************************************** -// -// The Stellaris Motor Class Firmware Update API definitions. -// -//***************************************************************************** -#define LM_API_UPD (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_UPDATE) -#define LM_API_UPD_PING (LM_API_UPD | (0 << CAN_MSGID_API_S)) -#define LM_API_UPD_DOWNLOAD (LM_API_UPD | (1 << CAN_MSGID_API_S)) -#define LM_API_UPD_SEND_DATA (LM_API_UPD | (2 << CAN_MSGID_API_S)) -#define LM_API_UPD_RESET (LM_API_UPD | (3 << CAN_MSGID_API_S)) -#define LM_API_UPD_ACK (LM_API_UPD | (4 << CAN_MSGID_API_S)) -#define LM_API_HWVER (LM_API_UPD | (5 << CAN_MSGID_API_S)) -#define LM_API_UPD_REQUEST (LM_API_UPD | (6 << CAN_MSGID_API_S)) -//##### FIRST BEGIN ##### -#define LM_API_UNTRUST_EN (LM_API_UPD | (11 << CAN_MSGID_API_S)) -#define LM_API_TRUST_EN (LM_API_UPD | (12 << CAN_MSGID_API_S)) -#define LM_API_TRUST_HEARTBEAT (LM_API_UPD | (13 << CAN_MSGID_API_S)) -//##### FIRST END ##### - -//***************************************************************************** -// -// The Stellaris Motor Class Status API definitions. -// -//***************************************************************************** -#define LM_API_STATUS \ - (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_STATUS) -#define LM_API_STATUS_VOLTOUT (LM_API_STATUS | (0 << CAN_MSGID_API_S)) -#define LM_API_STATUS_VOLTBUS (LM_API_STATUS | (1 << CAN_MSGID_API_S)) -#define LM_API_STATUS_CURRENT (LM_API_STATUS | (2 << CAN_MSGID_API_S)) -#define LM_API_STATUS_TEMP (LM_API_STATUS | (3 << CAN_MSGID_API_S)) -#define LM_API_STATUS_POS (LM_API_STATUS | (4 << CAN_MSGID_API_S)) -#define LM_API_STATUS_SPD (LM_API_STATUS | (5 << CAN_MSGID_API_S)) -#define LM_API_STATUS_LIMIT (LM_API_STATUS | (6 << CAN_MSGID_API_S)) -#define LM_API_STATUS_FAULT (LM_API_STATUS | (7 << CAN_MSGID_API_S)) -#define LM_API_STATUS_POWER (LM_API_STATUS | (8 << CAN_MSGID_API_S)) -#define LM_API_STATUS_CMODE (LM_API_STATUS | (9 << CAN_MSGID_API_S)) -#define LM_API_STATUS_VOUT (LM_API_STATUS | (10 << CAN_MSGID_API_S)) -#define LM_API_STATUS_STKY_FLT (LM_API_STATUS | (11 << CAN_MSGID_API_S)) -#define LM_API_STATUS_FLT_COUNT (LM_API_STATUS | (12 << CAN_MSGID_API_S)) - -//***************************************************************************** -// -// These definitions are used with the byte that is returned from -// the status request for LM_API_STATUS_LIMIT. -// -//***************************************************************************** -#define LM_STATUS_LIMIT_FWD 0x01 -#define LM_STATUS_LIMIT_REV 0x02 -#define LM_STATUS_LIMIT_SFWD 0x04 -#define LM_STATUS_LIMIT_SREV 0x08 -#define LM_STATUS_LIMIT_STKY_FWD 0x10 -#define LM_STATUS_LIMIT_STKY_REV 0x20 -#define LM_STATUS_LIMIT_STKY_SFWD 0x40 -#define LM_STATUS_LIMIT_STKY_SREV 0x80 - -//***************************************************************************** -// -// LM Motor Control status codes returned due to the CAN_STATUS_CODE_M field. -// -//***************************************************************************** -#define LM_STATUS_FAULT_ILIMIT 0x01 -#define LM_STATUS_FAULT_TLIMIT 0x02 -#define LM_STATUS_FAULT_VLIMIT 0x04 - -//***************************************************************************** -// -// The Stellaris Motor Class Configuration API definitions. -// -//***************************************************************************** -#define LM_API_CFG (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_CFG) -#define LM_API_CFG_NUM_BRUSHES (LM_API_CFG | (0 << CAN_MSGID_API_S)) -#define LM_API_CFG_ENC_LINES (LM_API_CFG | (1 << CAN_MSGID_API_S)) -#define LM_API_CFG_POT_TURNS (LM_API_CFG | (2 << CAN_MSGID_API_S)) -#define LM_API_CFG_BRAKE_COAST (LM_API_CFG | (3 << CAN_MSGID_API_S)) -#define LM_API_CFG_LIMIT_MODE (LM_API_CFG | (4 << CAN_MSGID_API_S)) -#define LM_API_CFG_LIMIT_FWD (LM_API_CFG | (5 << CAN_MSGID_API_S)) -#define LM_API_CFG_LIMIT_REV (LM_API_CFG | (6 << CAN_MSGID_API_S)) -#define LM_API_CFG_MAX_VOUT (LM_API_CFG | (7 << CAN_MSGID_API_S)) -#define LM_API_CFG_FAULT_TIME (LM_API_CFG | (8 << CAN_MSGID_API_S)) - -//***************************************************************************** -// -// The Stellaris ACK API definition. -// -//***************************************************************************** -#define LM_API_ACK (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_ACK) - -//***************************************************************************** -// -// The 8 bit values that can be returned by a call to LM_API_STATUS_HWVER. -// -//***************************************************************************** -#define LM_HWVER_UNKNOWN 0x00 -#define LM_HWVER_JAG_1_0 0x01 -#define LM_HWVER_JAG_2_0 0x02 - -//***************************************************************************** -// -// The 8 bit values that can be returned by a call to LM_API_STATUS_CMODE. -// -//***************************************************************************** -#define LM_STATUS_CMODE_VOLT 0x00 -#define LM_STATUS_CMODE_CURRENT 0x01 -#define LM_STATUS_CMODE_SPEED 0x02 -#define LM_STATUS_CMODE_POS 0x03 -#define LM_STATUS_CMODE_VCOMP 0x04 - -//***************************************************************************** -// -// The values that can specified as the position or speed reference. Not all -// values are valid for each reference; if an invalid reference is set, then -// none will be selected. -// -//***************************************************************************** -#define LM_REF_ENCODER 0x00 -#define LM_REF_POT 0x01 -#define LM_REF_INV_ENCODER 0x02 -#define LM_REF_QUAD_ENCODER 0x03 -#define LM_REF_NONE 0xff - -//***************************************************************************** -// -// The flags that are used to indicate the currently active fault sources. -// -//***************************************************************************** -#define LM_FAULT_CURRENT 0x01 -#define LM_FAULT_TEMP 0x02 -#define LM_FAULT_VBUS 0x04 -#define LM_FAULT_GATE_DRIVE 0x08 -#define LM_FAULT_COMM 0x10 - -//***************************************************************************** -// -// The Stellaris Motor Class Periodic Status API definitions. -// -//***************************************************************************** -#define LM_API_PSTAT \ - (CAN_MSGID_MFR_LM | CAN_MSGID_DTYPE_MOTOR | CAN_API_MC_PSTAT) -#define LM_API_PSTAT_PER_EN_S0 (LM_API_PSTAT | (0 << CAN_MSGID_API_S)) -#define LM_API_PSTAT_PER_EN_S1 (LM_API_PSTAT | (1 << CAN_MSGID_API_S)) -#define LM_API_PSTAT_PER_EN_S2 (LM_API_PSTAT | (2 << CAN_MSGID_API_S)) -#define LM_API_PSTAT_PER_EN_S3 (LM_API_PSTAT | (3 << CAN_MSGID_API_S)) -#define LM_API_PSTAT_CFG_S0 (LM_API_PSTAT | (4 << CAN_MSGID_API_S)) -#define LM_API_PSTAT_CFG_S1 (LM_API_PSTAT | (5 << CAN_MSGID_API_S)) -#define LM_API_PSTAT_CFG_S2 (LM_API_PSTAT | (6 << CAN_MSGID_API_S)) -#define LM_API_PSTAT_CFG_S3 (LM_API_PSTAT | (7 << CAN_MSGID_API_S)) -#define LM_API_PSTAT_DATA_S0 (LM_API_PSTAT | (8 << CAN_MSGID_API_S)) -#define LM_API_PSTAT_DATA_S1 (LM_API_PSTAT | (9 << CAN_MSGID_API_S)) -#define LM_API_PSTAT_DATA_S2 (LM_API_PSTAT | (10 << CAN_MSGID_API_S)) -#define LM_API_PSTAT_DATA_S3 (LM_API_PSTAT | (11 << CAN_MSGID_API_S)) - -//***************************************************************************** -// -// The values that can be used to configure the data the Periodic Status -// Message bytes. Bytes of a multi-byte data values are encoded as -// little-endian, therefore B0 is the least significant byte. -// -//***************************************************************************** -#define LM_PSTAT_END 0 -#define LM_PSTAT_VOLTOUT_B0 1 -#define LM_PSTAT_VOLTOUT_B1 2 -#define LM_PSTAT_VOLTBUS_B0 3 -#define LM_PSTAT_VOLTBUS_B1 4 -#define LM_PSTAT_CURRENT_B0 5 -#define LM_PSTAT_CURRENT_B1 6 -#define LM_PSTAT_TEMP_B0 7 -#define LM_PSTAT_TEMP_B1 8 -#define LM_PSTAT_POS_B0 9 -#define LM_PSTAT_POS_B1 10 -#define LM_PSTAT_POS_B2 11 -#define LM_PSTAT_POS_B3 12 -#define LM_PSTAT_SPD_B0 13 -#define LM_PSTAT_SPD_B1 14 -#define LM_PSTAT_SPD_B2 15 -#define LM_PSTAT_SPD_B3 16 -#define LM_PSTAT_LIMIT_NCLR 17 -#define LM_PSTAT_LIMIT_CLR 18 -#define LM_PSTAT_FAULT 19 -#define LM_PSTAT_STKY_FLT_NCLR 20 -#define LM_PSTAT_STKY_FLT_CLR 21 -#define LM_PSTAT_VOUT_B0 22 -#define LM_PSTAT_VOUT_B1 23 -#define LM_PSTAT_FLT_COUNT_CURRENT 24 -#define LM_PSTAT_FLT_COUNT_TEMP 25 -#define LM_PSTAT_FLT_COUNT_VOLTBUS 26 -#define LM_PSTAT_FLT_COUNT_GATE 27 -#define LM_PSTAT_FLT_COUNT_COMM 28 -#define LM_PSTAT_CANSTS 29 -#define LM_PSTAT_CANERR_B0 30 -#define LM_PSTAT_CANERR_B1 31 - -#endif // __CAN_PROTO_H__ diff --git a/wpilibc/athena/include/CANJaguar.h b/wpilibc/athena/include/CANJaguar.h deleted file mode 100644 index 3b1aeb4fc4..0000000000 --- a/wpilibc/athena/include/CANJaguar.h +++ /dev/null @@ -1,253 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) FIRST 2009-2016. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -#pragma once - -#include -#include -#include -#include -#include - -#include "CAN/can_proto.h" -#include "CANSpeedController.h" -#include "ErrorBase.h" -#include "FRC_NetworkCommunication/CANSessionMux.h" -#include "HAL/cpp/priority_mutex.h" -#include "LiveWindow/LiveWindowSendable.h" -#include "MotorSafety.h" -#include "MotorSafetyHelper.h" -#include "PIDOutput.h" -#include "tables/ITableListener.h" - -/** - * Luminary Micro / Vex Robotics Jaguar Speed Control - */ -class CANJaguar : public MotorSafety, - public CANSpeedController, - public ErrorBase, - public LiveWindowSendable, - public ITableListener { - public: - // The internal PID control loop in the Jaguar runs at 1kHz. - static const int kControllerRate = 1000; - static constexpr double kApproxBusVoltage = 12.0; - - // Control mode tags - /** Sets an encoder as the speed reference only.
Passed as the "tag" when - * setting the control mode.*/ - static const struct EncoderStruct { - } Encoder; - /** Sets a quadrature encoder as the position and speed reference.
Passed - * as the "tag" when setting the control mode.*/ - static const struct QuadEncoderStruct { - } QuadEncoder; - /** Sets a potentiometer as the position reference only.
Passed as the - * "tag" when setting the control mode. */ - static const struct PotentiometerStruct { - } Potentiometer; - - explicit CANJaguar(int deviceNumber); - virtual ~CANJaguar(); - - int getDeviceNumber() const; - int GetHardwareVersion() const; - - // PIDOutput interface - void PIDWrite(float output) override; - - // Control mode methods - void EnableControl(double encoderInitialPosition = 0.0); - void DisableControl(); - - void SetPercentMode(); - void SetPercentMode(EncoderStruct, uint16_t codesPerRev); - void SetPercentMode(QuadEncoderStruct, uint16_t codesPerRev); - void SetPercentMode(PotentiometerStruct); - - void SetCurrentMode(double p, double i, double d); - void SetCurrentMode(EncoderStruct, uint16_t codesPerRev, double p, double i, - double d); - void SetCurrentMode(QuadEncoderStruct, uint16_t codesPerRev, double p, - double i, double d); - void SetCurrentMode(PotentiometerStruct, double p, double i, double d); - - void SetSpeedMode(EncoderStruct, uint16_t codesPerRev, double p, double i, - double d); - void SetSpeedMode(QuadEncoderStruct, uint16_t codesPerRev, double p, double i, - double d); - - void SetPositionMode(QuadEncoderStruct, uint16_t codesPerRev, double p, - double i, double d); - void SetPositionMode(PotentiometerStruct, double p, double i, double d); - - void SetVoltageMode(); - void SetVoltageMode(EncoderStruct, uint16_t codesPerRev); - void SetVoltageMode(QuadEncoderStruct, uint16_t codesPerRev); - void SetVoltageMode(PotentiometerStruct); - - void Set(float value, int syncGroup); - - // CANSpeedController interface - float Get() const override; - void Set(float value) override; - void Disable() override; - void SetP(double p) override; - void SetI(double i) override; - void SetD(double d) override; - void SetPID(double p, double i, double d) override; - double GetP() const override; - double GetI() const override; - double GetD() const override; - bool IsModePID(CANSpeedController::ControlMode mode) const override; - float GetBusVoltage() const override; - float GetOutputVoltage() const override; - float GetOutputCurrent() const override; - float GetTemperature() const override; - double GetPosition() const override; - double GetSpeed() const override; - bool GetForwardLimitOK() const override; - bool GetReverseLimitOK() const override; - uint16_t GetFaults() const override; - void SetVoltageRampRate(double rampRate) override; - int GetFirmwareVersion() const override; - void ConfigNeutralMode(NeutralMode mode) override; - void ConfigEncoderCodesPerRev(uint16_t codesPerRev) override; - void ConfigPotentiometerTurns(uint16_t turns) override; - void ConfigSoftPositionLimits(double forwardLimitPosition, - double reverseLimitPosition) override; - void DisableSoftPositionLimits() override; - void ConfigLimitMode(LimitMode mode) override; - void ConfigForwardLimit(double forwardLimitPosition) override; - void ConfigReverseLimit(double reverseLimitPosition) override; - void ConfigMaxOutputVoltage(double voltage) override; - void ConfigFaultTime(float faultTime) override; - virtual void SetControlMode(ControlMode mode); - virtual ControlMode GetControlMode() const; - - static void UpdateSyncGroup(uint8_t syncGroup); - - void SetExpiration(float timeout) override; - float GetExpiration() const override; - bool IsAlive() const override; - void StopMotor() override; - bool IsSafetyEnabled() const override; - void SetSafetyEnabled(bool enabled) override; - void GetDescription(std::ostringstream& desc) const override; - int GetDeviceID() const; - - // SpeedController overrides - void SetInverted(bool isInverted) override; - bool GetInverted() const override; - - protected: - // Control mode helpers - void SetSpeedReference(int reference); - int GetSpeedReference() const; - - void SetPositionReference(int reference); - int GetPositionReference() const; - - int packPercentage(uint8_t* buffer, double value); - int packFXP8_8(uint8_t* buffer, double value); - int packFXP16_16(uint8_t* buffer, double value); - int packInt16(uint8_t* buffer, int16_t value); - int packInt32(uint8_t* buffer, int32_t value); - double unpackPercentage(uint8_t* buffer) const; - double unpackFXP8_8(uint8_t* buffer) const; - double unpackFXP16_16(uint8_t* buffer) const; - int16_t unpackInt16(uint8_t* buffer) const; - int32_t unpackInt32(uint8_t* buffer) const; - - void sendMessage(int messageID, const uint8_t* data, uint8_t dataSize, - int period = CAN_SEND_PERIOD_NO_REPEAT); - void requestMessage(int messageID, int period = CAN_SEND_PERIOD_NO_REPEAT); - bool getMessage(int messageID, uint32_t mask, uint8_t* data, - uint8_t* dataSize) const; - - void setupPeriodicStatus(); - void updatePeriodicStatus() const; - - mutable priority_recursive_mutex m_mutex; - - int m_deviceNumber; - float m_value = 0.0f; - - // Parameters/configuration - ControlMode m_controlMode = kPercentVbus; - int m_speedReference = LM_REF_NONE; - int m_positionReference = LM_REF_NONE; - double m_p = 0.0; - double m_i = 0.0; - double m_d = 0.0; - NeutralMode m_neutralMode = kNeutralMode_Jumper; - uint16_t m_encoderCodesPerRev = 0; - uint16_t m_potentiometerTurns = 0; - LimitMode m_limitMode = kLimitMode_SwitchInputsOnly; - double m_forwardLimit = 0.0; - double m_reverseLimit = 0.0; - double m_maxOutputVoltage = 30.0; - double m_voltageRampRate = 0.0; - float m_faultTime = 0.0f; - - // Which parameters have been verified since they were last set? - bool m_controlModeVerified = - false; // Needs to be verified because it's set in the constructor - bool m_speedRefVerified = true; - bool m_posRefVerified = true; - bool m_pVerified = true; - bool m_iVerified = true; - bool m_dVerified = true; - bool m_neutralModeVerified = true; - bool m_encoderCodesPerRevVerified = true; - bool m_potentiometerTurnsVerified = true; - bool m_forwardLimitVerified = true; - bool m_reverseLimitVerified = true; - bool m_limitModeVerified = true; - bool m_maxOutputVoltageVerified = true; - bool m_voltageRampRateVerified = true; - bool m_faultTimeVerified = true; - - // Status data - mutable float m_busVoltage = 0.0f; - mutable float m_outputVoltage = 0.0f; - mutable float m_outputCurrent = 0.0f; - mutable float m_temperature = 0.0f; - mutable double m_position = 0.0; - mutable double m_speed = 0.0; - mutable int m_limits = 0x00; - mutable uint16_t m_faults = 0x0000; - int m_firmwareVersion = 0; - int m_hardwareVersion = 0; - - // Which periodic status messages have we received at least once? - mutable std::atomic m_receivedStatusMessage0{false}; - mutable std::atomic m_receivedStatusMessage1{false}; - mutable std::atomic m_receivedStatusMessage2{false}; - - bool m_controlEnabled = false; - bool m_stopped = false; - - void verify(); - - std::unique_ptr m_safetyHelper; - - void ValueChanged(ITable* source, llvm::StringRef key, - std::shared_ptr value, bool isNew) override; - void UpdateTable() override; - void StartLiveWindowMode() override; - void StopLiveWindowMode() override; - std::string GetSmartDashboardType() const override; - void InitTable(std::shared_ptr subTable) override; - std::shared_ptr GetTable() const override; - - std::shared_ptr m_table; - - private: - void InitCANJaguar(); - bool m_isInverted = false; -}; diff --git a/wpilibc/athena/include/CANSpeedController.h b/wpilibc/athena/include/CANSpeedController.h index 43b0da75ff..34d9918fac 100644 --- a/wpilibc/athena/include/CANSpeedController.h +++ b/wpilibc/athena/include/CANSpeedController.h @@ -11,8 +11,6 @@ /** * Interface for "smart" CAN-based speed controllers. - * @see CANJaguar - * @see CANTalon */ class CANSpeedController : public SpeedController { public: @@ -22,8 +20,8 @@ class CANSpeedController : public SpeedController { kSpeed = 2, kPosition = 3, kVoltage = 4, - kFollower = 5, // Not supported in Jaguar. - kMotionProfile = 6, // Not supported in Jaguar. + kFollower = 5, + kMotionProfile = 6, }; // Helper function for the ControlMode enum diff --git a/wpilibc/athena/include/WPILib.h b/wpilibc/athena/include/WPILib.h index c45d506c6e..27cf62cc2c 100644 --- a/wpilibc/athena/include/WPILib.h +++ b/wpilibc/athena/include/WPILib.h @@ -24,7 +24,6 @@ #include "Buttons/InternalButton.h" #include "Buttons/JoystickButton.h" #include "Buttons/NetworkButton.h" -#include "CANJaguar.h" #include "CameraServer.h" #include "Commands/Command.h" #include "Commands/CommandGroup.h" diff --git a/wpilibc/athena/src/CANJaguar.cpp b/wpilibc/athena/src/CANJaguar.cpp deleted file mode 100644 index 451b4eb653..0000000000 --- a/wpilibc/athena/src/CANJaguar.cpp +++ /dev/null @@ -1,2057 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) FIRST 2009-2016. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -#include "CANJaguar.h" - -#include -#include - -#include "FRC_NetworkCommunication/CANSessionMux.h" -#include "HAL/HAL.h" -#include "LiveWindow/LiveWindow.h" -#include "Resource.h" -#include "Timer.h" -#include "WPIErrors.h" - -#define tNIRIO_i32 int - -/* we are on ARM-LE now, not Freescale so no need to swap */ -#define swap16(x) (x) -#define swap32(x) (x) - -/* Compare floats for equality as fixed point numbers */ -#define FXP8_EQ(a, b) \ - (static_cast((a)*256.0) == static_cast((b)*256.0)) -#define FXP16_EQ(a, b) \ - (static_cast((a)*65536.0) == static_cast((b)*65536.0)) - -const int CANJaguar::kControllerRate; -constexpr double CANJaguar::kApproxBusVoltage; - -static const int kSendMessagePeriod = 20; -static const int kFullMessageIDMask = - (CAN_MSGID_API_M | CAN_MSGID_MFR_M | CAN_MSGID_DTYPE_M); - -static const int kReceiveStatusAttempts = 50; - -static std::unique_ptr allocated; - -static int sendMessageHelper(int messageID, const uint8_t* data, - uint8_t dataSize, int period) { - static const int kTrustedMessages[] = { - LM_API_VOLT_T_EN, LM_API_VOLT_T_SET, LM_API_SPD_T_EN, LM_API_SPD_T_SET, - LM_API_VCOMP_T_EN, LM_API_VCOMP_T_SET, LM_API_POS_T_EN, LM_API_POS_T_SET, - LM_API_ICTRL_T_EN, LM_API_ICTRL_T_SET}; - - int32_t status = 0; - - for (auto& kTrustedMessage : kTrustedMessages) { - if ((kFullMessageIDMask & messageID) == kTrustedMessage) { - uint8_t dataBuffer[8]; - dataBuffer[0] = 0; - dataBuffer[1] = 0; - - // Make sure the data will still fit after adjusting for the token. - assert(dataSize <= 6); - - for (int j = 0; j < dataSize; j++) { - dataBuffer[j + 2] = data[j]; - } - - FRC_NetworkCommunication_CANSessionMux_sendMessage( - messageID, dataBuffer, dataSize + 2, period, &status); - - return status; - } - } - - FRC_NetworkCommunication_CANSessionMux_sendMessage(messageID, data, dataSize, - period, &status); - - return status; -} - -/** - * Common initialization code called by all constructors. - */ -void CANJaguar::InitCANJaguar() { - m_safetyHelper = std::make_unique(this); - - bool receivedFirmwareVersion = false; - uint8_t dataBuffer[8]; - uint8_t dataSize; - - // Request firmware and hardware version only once - requestMessage(CAN_IS_FRAME_REMOTE | CAN_MSGID_API_FIRMVER); - requestMessage(LM_API_HWVER); - - // Wait until we've gotten all of the status data at least once. - for (int i = 0; i < kReceiveStatusAttempts; i++) { - Wait(0.001); - - setupPeriodicStatus(); - updatePeriodicStatus(); - - if (!receivedFirmwareVersion && - getMessage(CAN_MSGID_API_FIRMVER, CAN_MSGID_FULL_M, dataBuffer, - &dataSize)) { - m_firmwareVersion = unpackInt32(dataBuffer); - receivedFirmwareVersion = true; - } - - if (m_receivedStatusMessage0 && m_receivedStatusMessage1 && - m_receivedStatusMessage2 && receivedFirmwareVersion) { - break; - } - } - - if (!m_receivedStatusMessage0 || !m_receivedStatusMessage1 || - !m_receivedStatusMessage2 || !receivedFirmwareVersion) { - wpi_setWPIErrorWithContext(JaguarMessageNotFound, "Status data not found"); - } - - if (getMessage(LM_API_HWVER, CAN_MSGID_FULL_M, dataBuffer, &dataSize)) - m_hardwareVersion = dataBuffer[0]; - - if (m_deviceNumber < 1 || m_deviceNumber > 63) { - std::stringstream buf; - buf << "device number \"" << m_deviceNumber - << "\" must be between 1 and 63"; - wpi_setWPIErrorWithContext(ParameterOutOfRange, buf.str()); - return; - } - - if (StatusIsFatal()) return; - - // 3330 was the first shipping RDK firmware version for the Jaguar - if (m_firmwareVersion >= 3330 || m_firmwareVersion < 108) { - std::stringstream buf; - if (m_firmwareVersion < 3330) { - buf << "Jag #" << m_deviceNumber << " firmware (" << m_firmwareVersion - << ") is too old (must be at least version 108 " - "of the FIRST approved firmware)"; - } else { - buf << "Jag #" << m_deviceNumber << " firmware (" << m_firmwareVersion - << ") is not FIRST approved (must be at least " - "version 108 of the FIRST approved firmware)"; - } - wpi_setWPIErrorWithContext(JaguarVersionError, buf.str()); - return; - } - - switch (m_controlMode) { - case kPercentVbus: - case kVoltage: - // No additional configuration required... start enabled. - EnableControl(); - break; - default: - break; - } - HAL_Report(HALUsageReporting::kResourceType_CANJaguar, m_deviceNumber, - m_controlMode); - LiveWindow::GetInstance()->AddActuator("CANJaguar", m_deviceNumber, this); -} - -/** - * Constructor for the CANJaguar device. - * - *

By default the device is configured in Percent mode. - * The control mode can be changed by calling one of the control modes listed - * below. - * - * @param deviceNumber The address of the Jaguar on the CAN bus. - * @see CANJaguar#SetCurrentMode(double, double, double) - * @see CANJaguar#SetCurrentMode(PotentiometerTag, double, double, double) - * @see CANJaguar#SetCurrentMode(EncoderTag, int, double, double, double) - * @see CANJaguar#SetCurrentMode(QuadEncoderTag, int, double, double, double) - * @see CANJaguar#SetPercentMode() - * @see CANJaguar#SetPercentMode(PotentiometerTag) - * @see CANJaguar#SetPercentMode(EncoderTag, int) - * @see CANJaguar#SetPercentMode(QuadEncoderTag, int) - * @see CANJaguar#SetPositionMode(PotentiometerTag, double, double, double) - * @see CANJaguar#SetPositionMode(QuadEncoderTag, int, double, double, double) - * @see CANJaguar#SetSpeedMode(EncoderTag, int, double, double, double) - * @see CANJaguar#SetSpeedMode(QuadEncoderTag, int, double, double, double) - * @see CANJaguar#SetVoltageMode() - * @see CANJaguar#SetVoltageMode(PotentiometerTag) - * @see CANJaguar#SetVoltageMode(EncoderTag, int) - * @see CANJaguar#SetVoltageMode(QuadEncoderTag, int) - */ -CANJaguar::CANJaguar(int deviceNumber) : m_deviceNumber(deviceNumber) { - std::stringstream buf; - buf << "CANJaguar device number " << m_deviceNumber; - Resource::CreateResourceObject(allocated, 63); - - if (allocated->Allocate(m_deviceNumber - 1, buf.str()) == - std::numeric_limits::max()) { - CloneError(*allocated); - return; - } - - SetPercentMode(); - InitCANJaguar(); - ConfigMaxOutputVoltage(kApproxBusVoltage); -} - -CANJaguar::~CANJaguar() { - allocated->Free(m_deviceNumber - 1); - - int32_t status; - - // Disable periodic setpoints - if (m_controlMode == kPercentVbus) - FRC_NetworkCommunication_CANSessionMux_sendMessage( - m_deviceNumber | LM_API_VOLT_T_SET, nullptr, 0, - CAN_SEND_PERIOD_STOP_REPEATING, &status); - else if (m_controlMode == kSpeed) - FRC_NetworkCommunication_CANSessionMux_sendMessage( - m_deviceNumber | LM_API_SPD_T_SET, nullptr, 0, - CAN_SEND_PERIOD_STOP_REPEATING, &status); - else if (m_controlMode == kPosition) - FRC_NetworkCommunication_CANSessionMux_sendMessage( - m_deviceNumber | LM_API_POS_T_SET, nullptr, 0, - CAN_SEND_PERIOD_STOP_REPEATING, &status); - else if (m_controlMode == kCurrent) - FRC_NetworkCommunication_CANSessionMux_sendMessage( - m_deviceNumber | LM_API_ICTRL_T_SET, nullptr, 0, - CAN_SEND_PERIOD_STOP_REPEATING, &status); - else if (m_controlMode == kVoltage) - FRC_NetworkCommunication_CANSessionMux_sendMessage( - m_deviceNumber | LM_API_VCOMP_T_SET, nullptr, 0, - CAN_SEND_PERIOD_STOP_REPEATING, &status); - - if (m_table != nullptr) m_table->RemoveTableListener(this); -} - -/** - * @return The CAN ID passed in the constructor - */ -int CANJaguar::getDeviceNumber() const { return m_deviceNumber; } - -/** - * Sets the output set-point value. - * - * The scale and the units depend on the mode the Jaguar is in. - *

In percentVbus Mode, the outputValue is from -1.0 to 1.0 (same as PWM - * Jaguar). - *

In voltage Mode, the outputValue is in volts. - *

In current Mode, the outputValue is in amps. - *

In speed Mode, the outputValue is in rotations/minute. - *

In position Mode, the outputValue is in rotations. - * - * @param outputValue The set-point to sent to the motor controller. - * @param syncGroup The update group to add this Set() to, pending - * UpdateSyncGroup(). If 0, update immediately. - */ -void CANJaguar::Set(float outputValue, int syncGroup) { - int messageID; - uint8_t dataBuffer[8]; - uint8_t dataSize; - - if (m_safetyHelper) m_safetyHelper->Feed(); - - if (m_stopped) { - EnableControl(); - m_stopped = false; - } - - if (m_controlEnabled) { - switch (m_controlMode) { - case kPercentVbus: { - messageID = LM_API_VOLT_T_SET; - if (outputValue > 1.0) outputValue = 1.0; - if (outputValue < -1.0) outputValue = -1.0; - dataSize = packPercentage(dataBuffer, - (m_isInverted ? -outputValue : outputValue)); - } break; - case kSpeed: { - messageID = LM_API_SPD_T_SET; - dataSize = packFXP16_16(dataBuffer, - (m_isInverted ? -outputValue : outputValue)); - } break; - case kPosition: { - messageID = LM_API_POS_T_SET; - dataSize = packFXP16_16(dataBuffer, outputValue); - } break; - case kCurrent: { - messageID = LM_API_ICTRL_T_SET; - dataSize = packFXP8_8(dataBuffer, outputValue); - } break; - case kVoltage: { - messageID = LM_API_VCOMP_T_SET; - dataSize = - packFXP8_8(dataBuffer, (m_isInverted ? -outputValue : outputValue)); - } break; - default: - wpi_setWPIErrorWithContext(IncompatibleMode, - "The Jaguar only supports Current, Voltage, " - "Position, Speed, and Percent (Throttle) " - "modes."); - return; - } - if (syncGroup != 0) { - dataBuffer[dataSize] = syncGroup; - dataSize++; - } - - sendMessage(messageID, dataBuffer, dataSize, kSendMessagePeriod); - } - - m_value = outputValue; - - verify(); -} - -/** - * Get the recently set outputValue setpoint. - * - * The scale and the units depend on the mode the Jaguar is in. - *

In percentVbus Mode, the outputValue is from -1.0 to 1.0 (same as PWM - * Jaguar). - *

In voltage Mode, the outputValue is in volts. - *

In current Mode, the outputValue is in amps. - *

In speed Mode, the outputValue is in rotations/minute. - *

In position Mode, the outputValue is in rotations. - * - * @return The most recently set outputValue setpoint. - */ -float CANJaguar::Get() const { return m_value; } - -/** -* Common interface for disabling a motor. -* -* @deprecated Call {@link #DisableControl()} instead. -*/ -void CANJaguar::Disable() { DisableControl(); } - -/** - * Write out the PID value as seen in the PIDOutput base object. - * - * @deprecated Call Set instead. - * - * @param output Write out the PercentVbus value as was computed by the - * PIDController - */ -void CANJaguar::PIDWrite(float output) { - if (m_controlMode == kPercentVbus) { - Set(output); - } else { - wpi_setWPIErrorWithContext(IncompatibleMode, - "PID only supported in PercentVbus mode"); - } -} - -int CANJaguar::packPercentage(uint8_t* buffer, double value) { - int16_t intValue = static_cast(value * 32767.0); - *reinterpret_cast(buffer) = swap16(intValue); - return sizeof(int16_t); -} - -int CANJaguar::packFXP8_8(uint8_t* buffer, double value) { - int16_t intValue = static_cast(value * 256.0); - *reinterpret_cast(buffer) = swap16(intValue); - return sizeof(int16_t); -} - -int CANJaguar::packFXP16_16(uint8_t* buffer, double value) { - int intValue = static_cast(value * 65536.0); - *reinterpret_cast(buffer) = swap32(intValue); - return sizeof(int32_t); -} - -int CANJaguar::packInt16(uint8_t* buffer, int16_t value) { - *reinterpret_cast(buffer) = swap16(value); - return sizeof(int16_t); -} - -int CANJaguar::packInt32(uint8_t* buffer, int32_t value) { - *reinterpret_cast(buffer) = swap32(value); - return sizeof(int32_t); -} - -double CANJaguar::unpackPercentage(uint8_t* buffer) const { - int16_t value = *reinterpret_cast(buffer); - value = swap16(value); - return value / 32767.0; -} - -double CANJaguar::unpackFXP8_8(uint8_t* buffer) const { - int16_t value = *reinterpret_cast(buffer); - value = swap16(value); - return value / 256.0; -} - -double CANJaguar::unpackFXP16_16(uint8_t* buffer) const { - int value = *reinterpret_cast(buffer); - value = swap32(value); - return value / 65536.0; -} - -int16_t CANJaguar::unpackInt16(uint8_t* buffer) const { - int16_t value = *reinterpret_cast(buffer); - return swap16(value); -} - -int32_t CANJaguar::unpackInt32(uint8_t* buffer) const { - int32_t value = *reinterpret_cast(buffer); - return swap32(value); -} - -/** - * Send a message to the Jaguar. - * - * @param messageID The messageID to be used on the CAN bus (device number is - * added internally) - * @param data The up to 8 bytes of data to be sent with the message - * @param dataSize Specify how much of the data in "data" to send - * @param periodic If positive, tell Network Communications to send the - * message every "period" milliseconds. - */ -void CANJaguar::sendMessage(int messageID, const uint8_t* data, - uint8_t dataSize, int period) { - int localStatus = - sendMessageHelper(messageID | m_deviceNumber, data, dataSize, period); - - if (localStatus < 0) { - wpi_setErrorWithContext(localStatus, "sendMessage"); - } -} - -/** - * Request a message from the Jaguar, but don't wait for it to arrive. - * - * @param messageID The message to request - * @param periodic If positive, tell Network Communications to send the - * message every "period" milliseconds. - */ -void CANJaguar::requestMessage(int messageID, int period) { - sendMessageHelper(messageID | m_deviceNumber, nullptr, 0, period); -} - -/** - * Get a previously requested message. - * - * Jaguar always generates a message with the same message ID when replying. - * - * @param messageID The messageID to read from the CAN bus (device number is - * added internally) - * @param data The up to 8 bytes of data that was received with the message - * @param dataSize Indicates how much data was received - * - * @return true if the message was found. Otherwise, no new message is - * available. - */ -bool CANJaguar::getMessage(int messageID, uint32_t messageMask, uint8_t* data, - uint8_t* dataSize) const { - uint32_t targetedMessageID = messageID | m_deviceNumber; - int32_t status = 0; - uint32_t timeStamp; - - // Caller may have set bit31 for remote frame transmission so clear invalid - // bits[31-29] - targetedMessageID &= CAN_MSGID_FULL_M; - - // Get the data. - FRC_NetworkCommunication_CANSessionMux_receiveMessage( - &targetedMessageID, messageMask, data, dataSize, &timeStamp, &status); - - // Do we already have the most recent value? - if (status == ERR_CANSessionMux_MessageNotFound) - return false; - else - wpi_setErrorWithContext(status, "receiveMessage"); - - return true; -} - -/** - * Enables periodic status updates from the Jaguar. - */ -void CANJaguar::setupPeriodicStatus() { - uint8_t data[8]; - uint8_t dataSize; - - // Message 0 returns bus voltage, output voltage, output current, and - // temperature. - static const uint8_t kMessage0Data[] = { - LM_PSTAT_VOLTBUS_B0, LM_PSTAT_VOLTBUS_B1, LM_PSTAT_VOLTOUT_B0, - LM_PSTAT_VOLTOUT_B1, LM_PSTAT_CURRENT_B0, LM_PSTAT_CURRENT_B1, - LM_PSTAT_TEMP_B0, LM_PSTAT_TEMP_B1}; - - // Message 1 returns position and speed - static const uint8_t kMessage1Data[] = { - LM_PSTAT_POS_B0, LM_PSTAT_POS_B1, LM_PSTAT_POS_B2, LM_PSTAT_POS_B3, - LM_PSTAT_SPD_B0, LM_PSTAT_SPD_B1, LM_PSTAT_SPD_B2, LM_PSTAT_SPD_B3}; - - // Message 2 returns limits and faults - static const uint8_t kMessage2Data[] = {LM_PSTAT_LIMIT_CLR, LM_PSTAT_FAULT, - LM_PSTAT_END}; - - dataSize = packInt16(data, kSendMessagePeriod); - sendMessage(LM_API_PSTAT_PER_EN_S0, data, dataSize); - sendMessage(LM_API_PSTAT_PER_EN_S1, data, dataSize); - sendMessage(LM_API_PSTAT_PER_EN_S2, data, dataSize); - - dataSize = 8; - sendMessage(LM_API_PSTAT_CFG_S0, kMessage0Data, dataSize); - sendMessage(LM_API_PSTAT_CFG_S1, kMessage1Data, dataSize); - sendMessage(LM_API_PSTAT_CFG_S2, kMessage2Data, dataSize); -} - -/** - * Check for new periodic status updates and unpack them into local variables - */ -void CANJaguar::updatePeriodicStatus() const { - uint8_t data[8]; - uint8_t dataSize; - - // Check if a new bus voltage/output voltage/current/temperature message - // has arrived and unpack the values into the cached member variables - if (getMessage(LM_API_PSTAT_DATA_S0, CAN_MSGID_FULL_M, data, &dataSize)) { - m_mutex.lock(); - m_busVoltage = unpackFXP8_8(data); - m_outputVoltage = unpackPercentage(data + 2) * m_busVoltage; - m_outputCurrent = unpackFXP8_8(data + 4); - m_temperature = unpackFXP8_8(data + 6); - m_mutex.unlock(); - - m_receivedStatusMessage0 = true; - } - - // Check if a new position/speed message has arrived and do the same - if (getMessage(LM_API_PSTAT_DATA_S1, CAN_MSGID_FULL_M, data, &dataSize)) { - m_mutex.lock(); - m_position = unpackFXP16_16(data); - m_speed = unpackFXP16_16(data + 4); - m_mutex.unlock(); - - m_receivedStatusMessage1 = true; - } - - // Check if a new limits/faults message has arrived and do the same - if (getMessage(LM_API_PSTAT_DATA_S2, CAN_MSGID_FULL_M, data, &dataSize)) { - m_mutex.lock(); - m_limits = data[0]; - m_faults = data[1]; - m_mutex.unlock(); - - m_receivedStatusMessage2 = true; - } -} - -/** - * Check all unverified params and make sure they're equal to their local - * cached versions. If a value isn't available, it gets requested. If a value - * doesn't match up, it gets set again. - */ -void CANJaguar::verify() { - uint8_t dataBuffer[8]; - uint8_t dataSize; - - // If the Jaguar lost power, everything should be considered unverified. - if (getMessage(LM_API_STATUS_POWER, CAN_MSGID_FULL_M, dataBuffer, - &dataSize)) { - bool powerCycled = static_cast(dataBuffer[0]); - - if (powerCycled) { - // Clear the power cycled bit - dataBuffer[0] = 1; - sendMessage(LM_API_STATUS_POWER, dataBuffer, sizeof(uint8_t)); - - // Mark everything as unverified - m_controlModeVerified = false; - m_speedRefVerified = false; - m_posRefVerified = false; - m_neutralModeVerified = false; - m_encoderCodesPerRevVerified = false; - m_potentiometerTurnsVerified = false; - m_forwardLimitVerified = false; - m_reverseLimitVerified = false; - m_limitModeVerified = false; - m_maxOutputVoltageVerified = false; - m_faultTimeVerified = false; - - if (m_controlMode == kPercentVbus || m_controlMode == kVoltage) { - m_voltageRampRateVerified = false; - } else { - m_pVerified = false; - m_iVerified = false; - m_dVerified = false; - } - - // Verify periodic status messages again - m_receivedStatusMessage0 = false; - m_receivedStatusMessage1 = false; - m_receivedStatusMessage2 = false; - - // Remove any old values from netcomms. Otherwise, parameters are - // incorrectly marked as verified based on stale messages. - getMessage(LM_API_SPD_REF, CAN_MSGID_FULL_M, dataBuffer, &dataSize); - getMessage(LM_API_POS_REF, CAN_MSGID_FULL_M, dataBuffer, &dataSize); - getMessage(LM_API_SPD_PC, CAN_MSGID_FULL_M, dataBuffer, &dataSize); - getMessage(LM_API_POS_PC, CAN_MSGID_FULL_M, dataBuffer, &dataSize); - getMessage(LM_API_ICTRL_PC, CAN_MSGID_FULL_M, dataBuffer, &dataSize); - getMessage(LM_API_SPD_IC, CAN_MSGID_FULL_M, dataBuffer, &dataSize); - getMessage(LM_API_POS_IC, CAN_MSGID_FULL_M, dataBuffer, &dataSize); - getMessage(LM_API_ICTRL_IC, CAN_MSGID_FULL_M, dataBuffer, &dataSize); - getMessage(LM_API_SPD_DC, CAN_MSGID_FULL_M, dataBuffer, &dataSize); - getMessage(LM_API_POS_DC, CAN_MSGID_FULL_M, dataBuffer, &dataSize); - getMessage(LM_API_ICTRL_DC, CAN_MSGID_FULL_M, dataBuffer, &dataSize); - getMessage(LM_API_CFG_BRAKE_COAST, CAN_MSGID_FULL_M, dataBuffer, - &dataSize); - getMessage(LM_API_CFG_ENC_LINES, CAN_MSGID_FULL_M, dataBuffer, &dataSize); - getMessage(LM_API_CFG_POT_TURNS, CAN_MSGID_FULL_M, dataBuffer, &dataSize); - getMessage(LM_API_CFG_LIMIT_MODE, CAN_MSGID_FULL_M, dataBuffer, - &dataSize); - getMessage(LM_API_CFG_LIMIT_FWD, CAN_MSGID_FULL_M, dataBuffer, &dataSize); - getMessage(LM_API_CFG_LIMIT_REV, CAN_MSGID_FULL_M, dataBuffer, &dataSize); - getMessage(LM_API_CFG_MAX_VOUT, CAN_MSGID_FULL_M, dataBuffer, &dataSize); - getMessage(LM_API_VOLT_SET_RAMP, CAN_MSGID_FULL_M, dataBuffer, &dataSize); - getMessage(LM_API_VCOMP_COMP_RAMP, CAN_MSGID_FULL_M, dataBuffer, - &dataSize); - getMessage(LM_API_CFG_FAULT_TIME, CAN_MSGID_FULL_M, dataBuffer, - &dataSize); - } - } else { - requestMessage(LM_API_STATUS_POWER); - } - - // Verify that any recently set parameters are correct - if (!m_controlModeVerified && m_controlEnabled) { - if (getMessage(LM_API_STATUS_CMODE, CAN_MSGID_FULL_M, dataBuffer, - &dataSize)) { - ControlMode mode = (ControlMode)dataBuffer[0]; - - if (m_controlMode == mode) - m_controlModeVerified = true; - else - // Enable control again to resend the control mode - EnableControl(); - } else { - // Verification is needed but not available - request it again. - requestMessage(LM_API_STATUS_CMODE); - } - } - - if (!m_speedRefVerified) { - if (getMessage(LM_API_SPD_REF, CAN_MSGID_FULL_M, dataBuffer, &dataSize)) { - int speedRef = dataBuffer[0]; - - if (m_speedReference == speedRef) - m_speedRefVerified = true; - else - // It's wrong - set it again - SetSpeedReference(m_speedReference); - } else { - // Verification is needed but not available - request it again. - requestMessage(LM_API_SPD_REF); - } - } - - if (!m_posRefVerified) { - if (getMessage(LM_API_POS_REF, CAN_MSGID_FULL_M, dataBuffer, &dataSize)) { - int posRef = dataBuffer[0]; - - if (m_positionReference == posRef) - m_posRefVerified = true; - else - // It's wrong - set it again - SetPositionReference(m_positionReference); - } else { - // Verification is needed but not available - request it again. - requestMessage(LM_API_POS_REF); - } - } - - if (!m_pVerified) { - int message = 0; - - if (m_controlMode == kSpeed) { - message = LM_API_SPD_PC; - } else if (m_controlMode == kPosition) { - message = LM_API_POS_PC; - } else if (m_controlMode == kCurrent) { - message = LM_API_ICTRL_PC; - } else { - wpi_setWPIErrorWithContext( - IncompatibleMode, - "PID constants only apply in Speed, Position, and Current mode"); - return; - } - - if (getMessage(message, CAN_MSGID_FULL_M, dataBuffer, &dataSize)) { - double p = unpackFXP16_16(dataBuffer); - - if (FXP16_EQ(m_p, p)) - m_pVerified = true; - else - // It's wrong - set it again - SetP(m_p); - } else { - // Verification is needed but not available - request it again. - requestMessage(message); - } - } - - if (!m_iVerified) { - int message = 0; - - if (m_controlMode == kSpeed) { - message = LM_API_SPD_IC; - } else if (m_controlMode == kPosition) { - message = LM_API_POS_IC; - } else if (m_controlMode == kCurrent) { - message = LM_API_ICTRL_IC; - } else { - wpi_setWPIErrorWithContext( - IncompatibleMode, - "PID constants only apply in Speed, Position, and Current mode"); - return; - } - - if (getMessage(message, CAN_MSGID_FULL_M, dataBuffer, &dataSize)) { - double i = unpackFXP16_16(dataBuffer); - - if (FXP16_EQ(m_i, i)) - m_iVerified = true; - else - // It's wrong - set it again - SetI(m_i); - } else { - // Verification is needed but not available - request it again. - requestMessage(message); - } - } - - if (!m_dVerified) { - int message = 0; - - if (m_controlMode == kSpeed) { - message = LM_API_SPD_DC; - } else if (m_controlMode == kPosition) { - message = LM_API_POS_DC; - } else if (m_controlMode == kCurrent) { - message = LM_API_ICTRL_DC; - } else { - wpi_setWPIErrorWithContext( - IncompatibleMode, - "PID constants only apply in Speed, Position, and Current mode"); - return; - } - - if (getMessage(message, CAN_MSGID_FULL_M, dataBuffer, &dataSize)) { - double d = unpackFXP16_16(dataBuffer); - - if (FXP16_EQ(m_d, d)) - m_dVerified = true; - else - // It's wrong - set it again - SetD(m_d); - } else { - // Verification is needed but not available - request it again. - requestMessage(message); - } - } - - if (!m_neutralModeVerified) { - if (getMessage(LM_API_CFG_BRAKE_COAST, CAN_MSGID_FULL_M, dataBuffer, - &dataSize)) { - NeutralMode mode = (NeutralMode)dataBuffer[0]; - - if (mode == m_neutralMode) - m_neutralModeVerified = true; - else - // It's wrong - set it again - ConfigNeutralMode(m_neutralMode); - } else { - // Verification is needed but not available - request it again. - requestMessage(LM_API_CFG_BRAKE_COAST); - } - } - - if (!m_encoderCodesPerRevVerified) { - if (getMessage(LM_API_CFG_ENC_LINES, CAN_MSGID_FULL_M, dataBuffer, - &dataSize)) { - uint16_t codes = unpackInt16(dataBuffer); - - if (codes == m_encoderCodesPerRev) - m_encoderCodesPerRevVerified = true; - else - // It's wrong - set it again - ConfigEncoderCodesPerRev(m_encoderCodesPerRev); - } else { - // Verification is needed but not available - request it again. - requestMessage(LM_API_CFG_ENC_LINES); - } - } - - if (!m_potentiometerTurnsVerified) { - if (getMessage(LM_API_CFG_POT_TURNS, CAN_MSGID_FULL_M, dataBuffer, - &dataSize)) { - uint16_t turns = unpackInt16(dataBuffer); - - if (turns == m_potentiometerTurns) - m_potentiometerTurnsVerified = true; - else - // It's wrong - set it again - ConfigPotentiometerTurns(m_potentiometerTurns); - } else { - // Verification is needed but not available - request it again. - requestMessage(LM_API_CFG_POT_TURNS); - } - } - - if (!m_limitModeVerified) { - if (getMessage(LM_API_CFG_LIMIT_MODE, CAN_MSGID_FULL_M, dataBuffer, - &dataSize)) { - LimitMode mode = (LimitMode)dataBuffer[0]; - - if (mode == m_limitMode) { - m_limitModeVerified = true; - } else { - // It's wrong - set it again - ConfigLimitMode(m_limitMode); - } - } else { - // Verification is needed but not available - request it again. - requestMessage(LM_API_CFG_LIMIT_MODE); - } - } - - if (!m_forwardLimitVerified) { - if (getMessage(LM_API_CFG_LIMIT_FWD, CAN_MSGID_FULL_M, dataBuffer, - &dataSize)) { - double limit = unpackFXP16_16(dataBuffer); - - if (FXP16_EQ(limit, m_forwardLimit)) { - m_forwardLimitVerified = true; - } else { - // It's wrong - set it again - ConfigForwardLimit(m_forwardLimit); - } - } else { - // Verification is needed but not available - request it again. - requestMessage(LM_API_CFG_LIMIT_FWD); - } - } - - if (!m_reverseLimitVerified) { - if (getMessage(LM_API_CFG_LIMIT_REV, CAN_MSGID_FULL_M, dataBuffer, - &dataSize)) { - double limit = unpackFXP16_16(dataBuffer); - - if (FXP16_EQ(limit, m_reverseLimit)) { - m_reverseLimitVerified = true; - } else { - // It's wrong - set it again - ConfigReverseLimit(m_reverseLimit); - } - } else { - // Verification is needed but not available - request it again. - requestMessage(LM_API_CFG_LIMIT_REV); - } - } - - if (!m_maxOutputVoltageVerified) { - if (getMessage(LM_API_CFG_MAX_VOUT, CAN_MSGID_FULL_M, dataBuffer, - &dataSize)) { - double voltage = unpackFXP8_8(dataBuffer); - - // The returned max output voltage is sometimes slightly higher or - // lower than what was sent. This should not trigger resending - // the message. - if (std::fabs(voltage - m_maxOutputVoltage) < 0.1) { - m_maxOutputVoltageVerified = true; - } else { - // It's wrong - set it again - ConfigMaxOutputVoltage(m_maxOutputVoltage); - } - } else { - // Verification is needed but not available - request it again. - requestMessage(LM_API_CFG_MAX_VOUT); - } - } - - if (!m_voltageRampRateVerified) { - if (m_controlMode == kPercentVbus) { - if (getMessage(LM_API_VOLT_SET_RAMP, CAN_MSGID_FULL_M, dataBuffer, - &dataSize)) { - double rate = unpackPercentage(dataBuffer); - - if (FXP16_EQ(rate, m_voltageRampRate)) { - m_voltageRampRateVerified = true; - } else { - // It's wrong - set it again - SetVoltageRampRate(m_voltageRampRate); - } - } else { - // Verification is needed but not available - request it again. - requestMessage(LM_API_VOLT_SET_RAMP); - } - } else if (m_controlMode == kVoltage) { - if (getMessage(LM_API_VCOMP_COMP_RAMP, CAN_MSGID_FULL_M, dataBuffer, - &dataSize)) { - double rate = unpackFXP8_8(dataBuffer); - - if (FXP8_EQ(rate, m_voltageRampRate)) { - m_voltageRampRateVerified = true; - } else { - // It's wrong - set it again - SetVoltageRampRate(m_voltageRampRate); - } - } else { - // Verification is needed but not available - request it again. - requestMessage(LM_API_VCOMP_COMP_RAMP); - } - } - } - - if (!m_faultTimeVerified) { - if (getMessage(LM_API_CFG_FAULT_TIME, CAN_MSGID_FULL_M, dataBuffer, - &dataSize)) { - uint16_t faultTime = unpackInt16(dataBuffer); - - if (static_cast(m_faultTime * 1000.0) == faultTime) { - m_faultTimeVerified = true; - } else { - // It's wrong - set it again - ConfigFaultTime(m_faultTime); - } - } else { - // Verification is needed but not available - request it again. - requestMessage(LM_API_CFG_FAULT_TIME); - } - } - - if (!m_receivedStatusMessage0 || !m_receivedStatusMessage1 || - !m_receivedStatusMessage2) { - // If the periodic status messages haven't been verified as received, - // request periodic status messages again and attempt to unpack any - // available ones. - setupPeriodicStatus(); - GetTemperature(); - GetPosition(); - GetFaults(); - } -} - -/** - * Set the reference source device for speed controller mode. - * - * Choose encoder as the source of speed feedback when in speed control mode. - * - * @param reference Specify a speed reference. - */ -void CANJaguar::SetSpeedReference(int reference) { - uint8_t dataBuffer[8]; - - // Send the speed reference parameter - dataBuffer[0] = reference; - sendMessage(LM_API_SPD_REF, dataBuffer, sizeof(uint8_t)); - - m_speedReference = reference; - m_speedRefVerified = false; -} - -/** - * Get the reference source device for speed controller mode. - * - * @return A speed reference indicating the currently selected reference device - * for speed controller mode. - */ -int CANJaguar::GetSpeedReference() const { return m_speedReference; } - -/** - * Set the reference source device for position controller mode. - * - * Choose between using and encoder and using a potentiometer - * as the source of position feedback when in position control mode. - * - * @param reference Specify a PositionReference. - */ -void CANJaguar::SetPositionReference(int reference) { - uint8_t dataBuffer[8]; - - // Send the position reference parameter - dataBuffer[0] = reference; - sendMessage(LM_API_POS_REF, dataBuffer, sizeof(uint8_t)); - - m_positionReference = reference; - m_posRefVerified = false; -} - -/** - * Get the reference source device for position controller mode. - * - * @return A PositionReference indicating the currently selected reference - * device for position controller mode. - */ -int CANJaguar::GetPositionReference() const { return m_positionReference; } - -/** - * Set the P, I, and D constants for the closed loop modes. - * - * @param p The proportional gain of the Jaguar's PID controller. - * @param i The integral gain of the Jaguar's PID controller. - * @param d The differential gain of the Jaguar's PID controller. - */ -void CANJaguar::SetPID(double p, double i, double d) { - SetP(p); - SetI(i); - SetD(d); -} - -/** - * Set the P constant for the closed loop modes. - * - * @param p The proportional gain of the Jaguar's PID controller. - */ -void CANJaguar::SetP(double p) { - uint8_t dataBuffer[8]; - uint8_t dataSize; - - switch (m_controlMode) { - case kPercentVbus: - case kVoltage: - case kFollower: - case kMotionProfile: - wpi_setWPIErrorWithContext( - IncompatibleMode, - "PID constants only apply in Speed, Position, and Current mode"); - break; - case kSpeed: - dataSize = packFXP16_16(dataBuffer, p); - sendMessage(LM_API_SPD_PC, dataBuffer, dataSize); - break; - case kPosition: - dataSize = packFXP16_16(dataBuffer, p); - sendMessage(LM_API_POS_PC, dataBuffer, dataSize); - break; - case kCurrent: - dataSize = packFXP16_16(dataBuffer, p); - sendMessage(LM_API_ICTRL_PC, dataBuffer, dataSize); - break; - } - - m_p = p; - m_pVerified = false; -} - -/** - * Set the I constant for the closed loop modes. - * - * @param i The integral gain of the Jaguar's PID controller. - */ -void CANJaguar::SetI(double i) { - uint8_t dataBuffer[8]; - uint8_t dataSize; - - switch (m_controlMode) { - case kPercentVbus: - case kVoltage: - case kFollower: - case kMotionProfile: - wpi_setWPIErrorWithContext( - IncompatibleMode, - "PID constants only apply in Speed, Position, and Current mode"); - break; - case kSpeed: - dataSize = packFXP16_16(dataBuffer, i); - sendMessage(LM_API_SPD_IC, dataBuffer, dataSize); - break; - case kPosition: - dataSize = packFXP16_16(dataBuffer, i); - sendMessage(LM_API_POS_IC, dataBuffer, dataSize); - break; - case kCurrent: - dataSize = packFXP16_16(dataBuffer, i); - sendMessage(LM_API_ICTRL_IC, dataBuffer, dataSize); - break; - } - - m_i = i; - m_iVerified = false; -} - -/** - * Set the D constant for the closed loop modes. - * - * @param d The derivative gain of the Jaguar's PID controller. - */ -void CANJaguar::SetD(double d) { - uint8_t dataBuffer[8]; - uint8_t dataSize; - - switch (m_controlMode) { - case kPercentVbus: - case kVoltage: - case kFollower: - case kMotionProfile: - wpi_setWPIErrorWithContext( - IncompatibleMode, - "PID constants only apply in Speed, Position, and Current mode"); - break; - case kSpeed: - dataSize = packFXP16_16(dataBuffer, d); - sendMessage(LM_API_SPD_DC, dataBuffer, dataSize); - break; - case kPosition: - dataSize = packFXP16_16(dataBuffer, d); - sendMessage(LM_API_POS_DC, dataBuffer, dataSize); - break; - case kCurrent: - dataSize = packFXP16_16(dataBuffer, d); - sendMessage(LM_API_ICTRL_DC, dataBuffer, dataSize); - break; - } - - m_d = d; - m_dVerified = false; -} - -/** - * Get the Proportional gain of the controller. - * - * @return The proportional gain. - */ -double CANJaguar::GetP() const { - if (m_controlMode == kPercentVbus || m_controlMode == kVoltage) { - wpi_setWPIErrorWithContext( - IncompatibleMode, - "PID constants only apply in Speed, Position, and Current mode"); - return 0.0; - } - - return m_p; -} - -/** - * Get the Intregral gain of the controller. - * - * @return The integral gain. - */ -double CANJaguar::GetI() const { - if (m_controlMode == kPercentVbus || m_controlMode == kVoltage) { - wpi_setWPIErrorWithContext( - IncompatibleMode, - "PID constants only apply in Speed, Position, and Current mode"); - return 0.0; - } - - return m_i; -} - -/** - * Get the Differential gain of the controller. - * - * @return The differential gain. - */ -double CANJaguar::GetD() const { - if (m_controlMode == kPercentVbus || m_controlMode == kVoltage) { - wpi_setWPIErrorWithContext( - IncompatibleMode, - "PID constants only apply in Speed, Position, and Current mode"); - return 0.0; - } - - return m_d; -} - -/** - * Enable the closed loop controller. - * - * Start actually controlling the output based on the feedback. - * If starting a position controller with an encoder reference, - * use the encoderInitialPosition parameter to initialize the - * encoder state. - * - * @param encoderInitialPosition Encoder position to set if position with - * encoder reference. Ignored otherwise. - */ -void CANJaguar::EnableControl(double encoderInitialPosition) { - uint8_t dataBuffer[8]; - uint8_t dataSize = 0; - - switch (m_controlMode) { - case kPercentVbus: - sendMessage(LM_API_VOLT_T_EN, dataBuffer, dataSize); - break; - case kSpeed: - sendMessage(LM_API_SPD_T_EN, dataBuffer, dataSize); - break; - case kPosition: - dataSize = packFXP16_16(dataBuffer, encoderInitialPosition); - sendMessage(LM_API_POS_T_EN, dataBuffer, dataSize); - break; - case kCurrent: - sendMessage(LM_API_ICTRL_T_EN, dataBuffer, dataSize); - break; - case kVoltage: - sendMessage(LM_API_VCOMP_T_EN, dataBuffer, dataSize); - break; - default: - wpi_setWPIErrorWithContext(IncompatibleMode, - "The Jaguar only supports Current, Voltage, " - "Position, Speed, and Percent (Throttle) " - "modes."); - return; - } - - m_controlEnabled = true; - m_controlModeVerified = false; -} - -/** - * Disable the closed loop controller. - * - * Stop driving the output based on the feedback. - */ -void CANJaguar::DisableControl() { - uint8_t dataBuffer[8]; - uint8_t dataSize = 0; - - // Disable all control - sendMessage(LM_API_VOLT_DIS, dataBuffer, dataSize); - sendMessage(LM_API_SPD_DIS, dataBuffer, dataSize); - sendMessage(LM_API_POS_DIS, dataBuffer, dataSize); - sendMessage(LM_API_ICTRL_DIS, dataBuffer, dataSize); - sendMessage(LM_API_VCOMP_DIS, dataBuffer, dataSize); - - // Stop all periodic setpoints - sendMessage(LM_API_VOLT_T_SET, dataBuffer, dataSize, - CAN_SEND_PERIOD_STOP_REPEATING); - sendMessage(LM_API_SPD_T_SET, dataBuffer, dataSize, - CAN_SEND_PERIOD_STOP_REPEATING); - sendMessage(LM_API_POS_T_SET, dataBuffer, dataSize, - CAN_SEND_PERIOD_STOP_REPEATING); - sendMessage(LM_API_ICTRL_T_SET, dataBuffer, dataSize, - CAN_SEND_PERIOD_STOP_REPEATING); - sendMessage(LM_API_VCOMP_T_SET, dataBuffer, dataSize, - CAN_SEND_PERIOD_STOP_REPEATING); - - m_controlEnabled = false; -} - -/** - * Enable controlling the motor voltage as a percentage of the bus voltage - * without any position or speed feedback. - * - *

After calling this you must call {@link CANJaguar#EnableControl()} or - * {@link CANJaguar#EnableControl(double)} to enable the device. - */ -void CANJaguar::SetPercentMode() { - SetControlMode(kPercentVbus); - SetPositionReference(LM_REF_NONE); - SetSpeedReference(LM_REF_NONE); -} - -/** - * Enable controlling the motor voltage as a percentage of the bus voltage, - * and enable speed sensing from a non-quadrature encoder. - * - *

After calling this you must call {@link CANJaguar#EnableControl()} or - * {@link CANJaguar#EnableControl(double)} to enable the device. - * - * @param tag The constant CANJaguar::Encoder - * @param codesPerRev The counts per revolution on the encoder - */ -void CANJaguar::SetPercentMode(CANJaguar::EncoderStruct, uint16_t codesPerRev) { - SetControlMode(kPercentVbus); - SetPositionReference(LM_REF_NONE); - SetSpeedReference(LM_REF_ENCODER); - ConfigEncoderCodesPerRev(codesPerRev); -} - -/** - * Enable controlling the motor voltage as a percentage of the bus voltage, - * and enable speed sensing from a non-quadrature encoder. - * - *

After calling this you must call {@link CANJaguar#EnableControl()} or - * {@link CANJaguar#EnableControl(double)} to enable the device. - * - * @param tag The constant CANJaguar::QuadEncoder - * @param codesPerRev The counts per revolution on the encoder - */ -void CANJaguar::SetPercentMode(CANJaguar::QuadEncoderStruct, - uint16_t codesPerRev) { - SetControlMode(kPercentVbus); - SetPositionReference(LM_REF_ENCODER); - SetSpeedReference(LM_REF_QUAD_ENCODER); - ConfigEncoderCodesPerRev(codesPerRev); -} - -/** - * Enable controlling the motor voltage as a percentage of the bus voltage, - * and enable position sensing from a potentiometer and no speed feedback. - * - *

After calling this you must call {@link CANJaguar#EnableControl()} or - * {@link CANJaguar#EnableControl(double)} to enable the device. - * - * @param potentiometer The constant CANJaguar::Potentiometer - */ -void CANJaguar::SetPercentMode(CANJaguar::PotentiometerStruct) { - SetControlMode(kPercentVbus); - SetPositionReference(LM_REF_POT); - SetSpeedReference(LM_REF_NONE); - ConfigPotentiometerTurns(1); -} - -/** - * Enable controlling the motor current with a PID loop. - * - *

After calling this you must call {@link CANJaguar#EnableControl()} or - * {@link CANJaguar#EnableControl(double)} to enable the device. - * - * @param p The proportional gain of the Jaguar's PID controller. - * @param i The integral gain of the Jaguar's PID controller. - * @param d The differential gain of the Jaguar's PID controller. - */ -void CANJaguar::SetCurrentMode(double p, double i, double d) { - SetControlMode(kCurrent); - SetPositionReference(LM_REF_NONE); - SetSpeedReference(LM_REF_NONE); - SetPID(p, i, d); -} - -/** - * Enable controlling the motor current with a PID loop, and enable speed - * sensing from a non-quadrature encoder. - * - *

After calling this you must call {@link CANJaguar#EnableControl()} or - * {@link CANJaguar#EnableControl(double)} to enable the device. - * - * @param encoder The constant CANJaguar::Encoder - * @param p The proportional gain of the Jaguar's PID controller. - * @param i The integral gain of the Jaguar's PID controller. - * @param d The differential gain of the Jaguar's PID controller. - */ -void CANJaguar::SetCurrentMode(CANJaguar::EncoderStruct, uint16_t codesPerRev, - double p, double i, double d) { - SetControlMode(kCurrent); - SetPositionReference(LM_REF_NONE); - SetSpeedReference(LM_REF_NONE); - ConfigEncoderCodesPerRev(codesPerRev); - SetPID(p, i, d); -} - -/** - * Enable controlling the motor current with a PID loop, and enable speed and - * position sensing from a quadrature encoder. - * - *

After calling this you must call {@link CANJaguar#EnableControl()} or - * {@link CANJaguar#EnableControl(double)} to enable the device. - * - * @param encoder The constant CANJaguar::QuadEncoder - * @param p The proportional gain of the Jaguar's PID controller. - * @param i The integral gain of the Jaguar's PID controller. - * @param d The differential gain of the Jaguar's PID controller. - */ -void CANJaguar::SetCurrentMode(CANJaguar::QuadEncoderStruct, - uint16_t codesPerRev, double p, double i, - double d) { - SetControlMode(kCurrent); - SetPositionReference(LM_REF_ENCODER); - SetSpeedReference(LM_REF_QUAD_ENCODER); - ConfigEncoderCodesPerRev(codesPerRev); - SetPID(p, i, d); -} - -/** - * Enable controlling the motor current with a PID loop, and enable position - * sensing from a potentiometer. - * - *

After calling this you must call {@link CANJaguar#EnableControl()} or - * {@link CANJaguar#EnableControl(double)} to enable the device. - * - * @param potentiometer The constant CANJaguar::Potentiometer - * @param p The proportional gain of the Jaguar's PID controller. - * @param i The integral gain of the Jaguar's PID controller. - * @param d The differential gain of the Jaguar's PID controller. - */ -void CANJaguar::SetCurrentMode(CANJaguar::PotentiometerStruct, double p, - double i, double d) { - SetControlMode(kCurrent); - SetPositionReference(LM_REF_POT); - SetSpeedReference(LM_REF_NONE); - ConfigPotentiometerTurns(1); - SetPID(p, i, d); -} - -/** - * Enable controlling the speed with a feedback loop from a non-quadrature - * encoder. - * - *

After calling this you must call {@link CANJaguar#EnableControl()} or - * {@link CANJaguar#EnableControl(double)} to enable the device. - * - * @param encoder The constant CANJaguar::Encoder - * @param codesPerRev The counts per revolution on the encoder. - * @param p The proportional gain of the Jaguar's PID controller. - * @param i The integral gain of the Jaguar's PID controller. - * @param d The differential gain of the Jaguar's PID controller. - */ -void CANJaguar::SetSpeedMode(CANJaguar::EncoderStruct, uint16_t codesPerRev, - double p, double i, double d) { - SetControlMode(kSpeed); - SetPositionReference(LM_REF_NONE); - SetSpeedReference(LM_REF_ENCODER); - ConfigEncoderCodesPerRev(codesPerRev); - SetPID(p, i, d); -} - -/** - * Enable controlling the speed with a feedback loop from a quadrature - * encoder. - * - *

After calling this you must call {@link CANJaguar#EnableControl()} or - * {@link CANJaguar#EnableControl(double)} to enable the device. - * - * @param encoder The constant CANJaguar::QuadEncoder - * @param codesPerRev The counts per revolution on the encoder. - * @param p The proportional gain of the Jaguar's PID controller. - * @param i The integral gain of the Jaguar's PID controller. - * @param d The differential gain of the Jaguar's PID controller. - */ -void CANJaguar::SetSpeedMode(CANJaguar::QuadEncoderStruct, uint16_t codesPerRev, - double p, double i, double d) { - SetControlMode(kSpeed); - SetPositionReference(LM_REF_ENCODER); - SetSpeedReference(LM_REF_QUAD_ENCODER); - ConfigEncoderCodesPerRev(codesPerRev); - SetPID(p, i, d); -} - -/** - * Enable controlling the position with a feedback loop using an encoder. - * - *

After calling this you must call {@link CANJaguar#EnableControl()} or - * {@link CANJaguar#EnableControl(double)} to enable the device. - * - * @param encoder The constant CANJaguar::QuadEncoder - * @param codesPerRev The counts per revolution on the encoder. - * @param p The proportional gain of the Jaguar's PID controller. - * @param i The integral gain of the Jaguar's PID controller. - * @param d The differential gain of the Jaguar's PID controller. - */ -void CANJaguar::SetPositionMode(CANJaguar::QuadEncoderStruct, - uint16_t codesPerRev, double p, double i, - double d) { - SetControlMode(kPosition); - SetPositionReference(LM_REF_ENCODER); - ConfigEncoderCodesPerRev(codesPerRev); - SetPID(p, i, d); -} - -/** - * Enable controlling the position with a feedback loop using a - * potentiometer. - * - *

After calling this you must call {@link CANJaguar#EnableControl()} or - * {@link CANJaguar#EnableControl(double)} to enable the device. - * - * @param p The proportional gain of the Jaguar's PID controller. - * @param i The integral gain of the Jaguar's PID controller. - * @param d The differential gain of the Jaguar's PID controller. - */ -void CANJaguar::SetPositionMode(CANJaguar::PotentiometerStruct, double p, - double i, double d) { - SetControlMode(kPosition); - SetPositionReference(LM_REF_POT); - ConfigPotentiometerTurns(1); - SetPID(p, i, d); -} - -/** - * Enable controlling the motor voltage without any position or speed - * feedback. - * - *

After calling this you must call {@link CANJaguar#EnableControl()} or - * {@link CANJaguar#EnableControl(double)} to enable the device. - */ -void CANJaguar::SetVoltageMode() { - SetControlMode(kVoltage); - SetPositionReference(LM_REF_NONE); - SetSpeedReference(LM_REF_NONE); -} - -/** - * Enable controlling the motor voltage with speed feedback from a - * non-quadrature encoder and no position feedback. - * - *

After calling this you must call {@link CANJaguar#EnableControl()} or - * {@link CANJaguar#EnableControl(double)} to enable the device. - * - * @param encoder The constant CANJaguar::Encoder - * @param codesPerRev The counts per revolution on the encoder - */ -void CANJaguar::SetVoltageMode(CANJaguar::EncoderStruct, uint16_t codesPerRev) { - SetControlMode(kVoltage); - SetPositionReference(LM_REF_NONE); - SetSpeedReference(LM_REF_ENCODER); - ConfigEncoderCodesPerRev(codesPerRev); -} - -/** - * Enable controlling the motor voltage with position and speed feedback from a - * quadrature encoder. - * - *

After calling this you must call {@link CANJaguar#EnableControl()} or - * {@link CANJaguar#EnableControl(double)} to enable the device. - * - * @param encoder The constant CANJaguar::QuadEncoder - * @param codesPerRev The counts per revolution on the encoder - */ -void CANJaguar::SetVoltageMode(CANJaguar::QuadEncoderStruct, - uint16_t codesPerRev) { - SetControlMode(kVoltage); - SetPositionReference(LM_REF_ENCODER); - SetSpeedReference(LM_REF_QUAD_ENCODER); - ConfigEncoderCodesPerRev(codesPerRev); -} - -/** - * Enable controlling the motor voltage with position feedback from a - * potentiometer and no speed feedback. - * - *

After calling this you must call {@link CANJaguar#EnableControl()} or - * {@link CANJaguar#EnableControl(double)} to enable the device. - * - * @param potentiometer The constant CANJaguar::Potentiometer - */ -void CANJaguar::SetVoltageMode(CANJaguar::PotentiometerStruct) { - SetControlMode(kVoltage); - SetPositionReference(LM_REF_POT); - SetSpeedReference(LM_REF_NONE); - ConfigPotentiometerTurns(1); -} - -/** - * Sets the output set-point value. - * - * The scale and the units depend on the mode the Jaguar is in. - *

In percentVbus Mode, the outputValue is from -1.0 to 1.0 (same as PWM - * Jaguar). - *

In voltage Mode, the outputValue is in volts. - *

In current Mode, the outputValue is in amps. - *

In speed Mode, the outputValue is in rotations/minute. - *

In position Mode, the outputValue is in rotations. - * - * @param outputValue The set-point to sent to the motor controller. - */ -void CANJaguar::Set(float outputValue) { Set(outputValue, 0); } - -/** - * Used internally. In order to set the control mode see the methods listed - * below. - * - * Change the control mode of this Jaguar object. - * - * After changing modes, configure any PID constants or other settings needed - * and then EnableControl() to actually change the mode on the Jaguar. - * - * @param controlMode The new mode. - */ -void CANJaguar::SetControlMode(ControlMode controlMode) { - // Disable the previous mode - DisableControl(); - - if ((controlMode == kFollower) || (controlMode == kMotionProfile)) - wpi_setWPIErrorWithContext(IncompatibleMode, - "The Jaguar only supports Current, Voltage, " - "Position, Speed, and Percent (Throttle) " - "modes."); - - // Update the local mode - m_controlMode = controlMode; - m_controlModeVerified = false; - - HAL_Report(HALUsageReporting::kResourceType_CANJaguar, m_deviceNumber, - m_controlMode); -} - -/** - * Get the active control mode from the Jaguar. - * - * Ask the Jag what mode it is in. - * - * @return ControlMode that the Jag is in. - */ -CANJaguar::ControlMode CANJaguar::GetControlMode() const { - return m_controlMode; -} - -/** - * Get the voltage at the battery input terminals of the Jaguar. - * - * @return The bus voltage in volts. - */ -float CANJaguar::GetBusVoltage() const { - updatePeriodicStatus(); - std::lock_guard lock(m_mutex); - - return m_busVoltage; -} - -/** - * Get the voltage being output from the motor terminals of the Jaguar. - * - * @return The output voltage in volts. - */ -float CANJaguar::GetOutputVoltage() const { - updatePeriodicStatus(); - std::lock_guard lock(m_mutex); - - return m_outputVoltage; -} - -/** - * Get the current through the motor terminals of the Jaguar. - * - * @return The output current in amps. - */ -float CANJaguar::GetOutputCurrent() const { - updatePeriodicStatus(); - std::lock_guard lock(m_mutex); - - return m_outputCurrent; -} - -/** - * Get the internal temperature of the Jaguar. - * - * @return The temperature of the Jaguar in degrees Celsius. - */ -float CANJaguar::GetTemperature() const { - updatePeriodicStatus(); - std::lock_guard lock(m_mutex); - - return m_temperature; -} - -/** - * Get the position of the encoder or potentiometer. - * - * @return The position of the motor in rotations based on the configured - * feedback. - * @see CANJaguar#ConfigPotentiometerTurns(int) - * @see CANJaguar#ConfigEncoderCodesPerRev(int) - */ -double CANJaguar::GetPosition() const { - updatePeriodicStatus(); - std::lock_guard lock(m_mutex); - - return m_position; -} - -/** - * Get the speed of the encoder. - * - * @return The speed of the motor in RPM based on the configured feedback. - */ -double CANJaguar::GetSpeed() const { - updatePeriodicStatus(); - std::lock_guard lock(m_mutex); - - return m_speed; -} - -/** - * Get the status of the forward limit switch. - * - * @return The motor is allowed to turn in the forward direction when true. - */ -bool CANJaguar::GetForwardLimitOK() const { - updatePeriodicStatus(); - std::lock_guard lock(m_mutex); - - return m_limits & kForwardLimit; -} - -/** - * Get the status of the reverse limit switch. - * - * @return The motor is allowed to turn in the reverse direction when true. - */ -bool CANJaguar::GetReverseLimitOK() const { - updatePeriodicStatus(); - std::lock_guard lock(m_mutex); - - return m_limits & kReverseLimit; -} - -/** - * Get the status of any faults the Jaguar has detected. - * - * @return A bit-mask of faults defined by the "Faults" enum. - * @see #kCurrentFault - * @see #kBusVoltageFault - * @see #kTemperatureFault - * @see #kGateDriverFault - */ -uint16_t CANJaguar::GetFaults() const { - updatePeriodicStatus(); - std::lock_guard lock(m_mutex); - - return m_faults; -} - -/** - * Set the maximum voltage change rate. - * - * When in PercentVbus or Voltage output mode, the rate at which the voltage - * changes can be limited to reduce current spikes. Set this to 0.0 to disable - * rate limiting. - * - * @param rampRate The maximum rate of voltage change in Percent Voltage mode - * in V/s. - */ -void CANJaguar::SetVoltageRampRate(double rampRate) { - uint8_t dataBuffer[8]; - uint8_t dataSize; - int message; - - switch (m_controlMode) { - case kPercentVbus: - dataSize = packPercentage( - dataBuffer, rampRate / (m_maxOutputVoltage * kControllerRate)); - message = LM_API_VOLT_SET_RAMP; - break; - case kVoltage: - dataSize = packFXP8_8(dataBuffer, rampRate / kControllerRate); - message = LM_API_VCOMP_COMP_RAMP; - break; - default: - wpi_setWPIErrorWithContext( - IncompatibleMode, - "SetVoltageRampRate only applies in Voltage and Percent mode"); - return; - } - - sendMessage(message, dataBuffer, dataSize); - - m_voltageRampRate = rampRate; - m_voltageRampRateVerified = false; -} - -/** - * Get the version of the firmware running on the Jaguar. - * - * @return The firmware version. 0 if the device did not respond. - */ -int CANJaguar::GetFirmwareVersion() const { return m_firmwareVersion; } - -/** - * Get the version of the Jaguar hardware. - * - * @return The hardware version. 1: Jaguar, 2: Black Jaguar - */ -int CANJaguar::GetHardwareVersion() const { return m_hardwareVersion; } - -/** - * Configure what the controller does to the H-Bridge when neutral (not driving - * the output). - * - * This allows you to override the jumper configuration for brake or coast. - * - * @param mode Select to use the jumper setting or to override it to coast or - * brake. - */ -void CANJaguar::ConfigNeutralMode(NeutralMode mode) { - uint8_t dataBuffer[8]; - - // Set the neutral mode - sendMessage(LM_API_CFG_BRAKE_COAST, dataBuffer, sizeof(uint8_t)); - - m_neutralMode = mode; - m_neutralModeVerified = false; -} - -/** - * Configure how many codes per revolution are generated by your encoder. - * - * @param codesPerRev The number of counts per revolution in 1X mode. - */ -void CANJaguar::ConfigEncoderCodesPerRev(uint16_t codesPerRev) { - uint8_t dataBuffer[8]; - - // Set the codes per revolution mode - packInt16(dataBuffer, codesPerRev); - sendMessage(LM_API_CFG_ENC_LINES, dataBuffer, sizeof(uint16_t)); - - m_encoderCodesPerRev = codesPerRev; - m_encoderCodesPerRevVerified = false; -} - -/** - * Configure the number of turns on the potentiometer. - * - * There is no special support for continuous turn potentiometers. - * Only integer numbers of turns are supported. - * - * @param turns The number of turns of the potentiometer. - */ -void CANJaguar::ConfigPotentiometerTurns(uint16_t turns) { - uint8_t dataBuffer[8]; - uint8_t dataSize; - - // Set the pot turns - dataSize = packInt16(dataBuffer, turns); - sendMessage(LM_API_CFG_POT_TURNS, dataBuffer, dataSize); - - m_potentiometerTurns = turns; - m_potentiometerTurnsVerified = false; -} - -/** - * Configure Soft Position Limits when in Position Controller mode. - * - * When controlling position, you can add additional limits on top of the limit - * switch inputs that are based on the position feedback. If the position - * limit is reached or the switch is opened, that direction will be disabled. - * - * @param forwardLimitPosition The position that if exceeded will disable the - * forward direction. - * @param reverseLimitPosition The position that if exceeded will disable the - * reverse direction. - */ -void CANJaguar::ConfigSoftPositionLimits(double forwardLimitPosition, - double reverseLimitPosition) { - ConfigLimitMode(kLimitMode_SoftPositionLimits); - ConfigForwardLimit(forwardLimitPosition); - ConfigReverseLimit(reverseLimitPosition); -} - -/** - * Disable Soft Position Limits if previously enabled. - * - * Soft Position Limits are disabled by default. - */ -void CANJaguar::DisableSoftPositionLimits() { - ConfigLimitMode(kLimitMode_SwitchInputsOnly); -} - -/** - * Set the limit mode for position control mode. - * - * Use ConfigSoftPositionLimits or DisableSoftPositionLimits to set this - * automatically. - */ -void CANJaguar::ConfigLimitMode(LimitMode mode) { - uint8_t dataBuffer[8]; - - dataBuffer[0] = mode; - sendMessage(LM_API_CFG_LIMIT_MODE, dataBuffer, sizeof(uint8_t)); - - m_limitMode = mode; - m_limitModeVerified = false; -} - -/** - * Set the position that if exceeded will disable the forward direction. - * - * Use ConfigSoftPositionLimits to set this and the limit mode automatically. - */ -void CANJaguar::ConfigForwardLimit(double forwardLimitPosition) { - uint8_t dataBuffer[8]; - uint8_t dataSize; - - dataSize = packFXP16_16(dataBuffer, forwardLimitPosition); - dataBuffer[dataSize++] = 1; - sendMessage(LM_API_CFG_LIMIT_FWD, dataBuffer, dataSize); - - m_forwardLimit = forwardLimitPosition; - m_forwardLimitVerified = false; -} - -/** - * Set the position that if exceeded will disable the reverse direction. - * - * Use ConfigSoftPositionLimits to set this and the limit mode automatically. - */ -void CANJaguar::ConfigReverseLimit(double reverseLimitPosition) { - uint8_t dataBuffer[8]; - uint8_t dataSize; - - dataSize = packFXP16_16(dataBuffer, reverseLimitPosition); - dataBuffer[dataSize++] = 0; - sendMessage(LM_API_CFG_LIMIT_REV, dataBuffer, dataSize); - - m_reverseLimit = reverseLimitPosition; - m_reverseLimitVerified = false; -} - -/** - * Configure the maximum voltage that the Jaguar will ever output. - * - * This can be used to limit the maximum output voltage in all modes so that - * motors which cannot withstand full bus voltage can be used safely. - * - * @param voltage The maximum voltage output by the Jaguar. - */ -void CANJaguar::ConfigMaxOutputVoltage(double voltage) { - uint8_t dataBuffer[8]; - uint8_t dataSize; - - dataSize = packFXP8_8(dataBuffer, voltage); - sendMessage(LM_API_CFG_MAX_VOUT, dataBuffer, dataSize); - - m_maxOutputVoltage = voltage; - m_maxOutputVoltageVerified = false; -} - -/** - * Configure how long the Jaguar waits in the case of a fault before resuming - * operation. - * - * Faults include over temerature, over current, and bus under voltage. - * The default is 3.0 seconds, but can be reduced to as low as 0.5 seconds. - * - * @param faultTime The time to wait before resuming operation, in seconds. - */ -void CANJaguar::ConfigFaultTime(float faultTime) { - uint8_t dataBuffer[8]; - uint8_t dataSize; - - if (faultTime < 0.5) - faultTime = 0.5; - else if (faultTime > 3.0) - faultTime = 3.0; - - // Message takes ms - dataSize = packInt16(dataBuffer, static_cast(faultTime * 1000.0)); - sendMessage(LM_API_CFG_FAULT_TIME, dataBuffer, dataSize); - - m_faultTime = faultTime; - m_faultTimeVerified = false; -} - -/** - * Update all the motors that have pending sets in the syncGroup. - * - * @param syncGroup A bitmask of groups to generate synchronous output. - */ -void CANJaguar::UpdateSyncGroup(uint8_t syncGroup) { - sendMessageHelper(CAN_MSGID_API_SYNC, &syncGroup, sizeof(syncGroup), - CAN_SEND_PERIOD_NO_REPEAT); -} - -void CANJaguar::SetExpiration(float timeout) { - if (m_safetyHelper) m_safetyHelper->SetExpiration(timeout); -} - -float CANJaguar::GetExpiration() const { - if (!m_safetyHelper) return 0.0; - return m_safetyHelper->GetExpiration(); -} - -bool CANJaguar::IsAlive() const { - if (!m_safetyHelper) return false; - return m_safetyHelper->IsAlive(); -} - -bool CANJaguar::IsSafetyEnabled() const { - if (!m_safetyHelper) return false; - return m_safetyHelper->IsSafetyEnabled(); -} - -void CANJaguar::SetSafetyEnabled(bool enabled) { - if (m_safetyHelper) m_safetyHelper->SetSafetyEnabled(enabled); -} - -void CANJaguar::GetDescription(std::ostringstream& desc) const { - desc << "CANJaguar ID " << m_deviceNumber; -} - -int CANJaguar::GetDeviceID() const { return m_deviceNumber; } - -/** - * Common interface for stopping the motor until the next Set() command - * Part of the MotorSafety interface - * - * @deprecated Call DisableControl instead. - */ -void CANJaguar::StopMotor() { m_stopped = true; } - -/** - * Common interface for inverting direction of a speed controller. - * - * Only works in PercentVbus, speed, and Voltage modes. - * - * @param isInverted The state of inversion, true is inverted - */ -void CANJaguar::SetInverted(bool isInverted) { m_isInverted = isInverted; } - -/** - * Common interface for the inverting direction of a speed controller. - * - * @return isInverted The state of inversion, true is inverted. - * - */ -bool CANJaguar::GetInverted() const { return m_isInverted; } - -void CANJaguar::ValueChanged(ITable* source, llvm::StringRef key, - std::shared_ptr value, bool isNew) { - if (key == "Mode" && value->IsDouble()) - SetControlMode( - static_cast(value->GetDouble())); - if (IsModePID(m_controlMode) && value->IsDouble()) { - if (key == "p") SetP(value->GetDouble()); - if (key == "i") SetI(value->GetDouble()); - if (key == "d") SetD(value->GetDouble()); - } - if (key == "Enabled" && value->IsBoolean()) { - if (value->GetBoolean()) { - EnableControl(); - } else { - DisableControl(); - } - } - if (key == "Value" && value->IsDouble()) Set(value->GetDouble()); -} - -bool CANJaguar::IsModePID(CANSpeedController::ControlMode mode) const { - return mode == kCurrent || mode == kSpeed || mode == kPosition; -} - -void CANJaguar::UpdateTable() { - if (m_table != nullptr) { - m_table->PutString("~TYPE~", "CANSpeedController"); - m_table->PutString("Type", "CANJaguar"); - m_table->PutNumber("Mode", m_controlMode); - if (IsModePID(m_controlMode)) { - m_table->PutNumber("p", GetP()); - m_table->PutNumber("i", GetI()); - m_table->PutNumber("d", GetD()); - } - m_table->PutBoolean("Enabled", m_controlEnabled); - m_table->PutNumber("Value", Get()); - } -} - -void CANJaguar::StartLiveWindowMode() { - if (m_table != nullptr) { - m_table->AddTableListener(this, true); - } -} - -void CANJaguar::StopLiveWindowMode() { - if (m_table != nullptr) { - m_table->RemoveTableListener(this); - } -} - -std::string CANJaguar::GetSmartDashboardType() const { - return "CANSpeedController"; -} - -void CANJaguar::InitTable(std::shared_ptr subTable) { - m_table = subTable; - UpdateTable(); -} - -std::shared_ptr CANJaguar::GetTable() const { return m_table; } diff --git a/wpilibcIntegrationTests/include/TestBench.h b/wpilibcIntegrationTests/include/TestBench.h index a941b45d2a..0c514e3e05 100644 --- a/wpilibcIntegrationTests/include/TestBench.h +++ b/wpilibcIntegrationTests/include/TestBench.h @@ -18,7 +18,6 @@ class TestBench { /* Analog output channels */ static const uint32_t kAnalogOutputChannel = 0; - static const uint32_t kFakeJaguarPotentiometer = 1; /* DIO channels */ static const uint32_t kTalonEncoderChannelA = 0; @@ -45,15 +44,9 @@ class TestBench { static const uint32_t kFakeSolenoid2Channel = 13; static const uint32_t kFakeRelayForward = 18; static const uint32_t kFakeRelayReverse = 19; - static const uint32_t kFakeJaguarForwardLimit = 20; - static const uint32_t kFakeJaguarReverseLimit = 21; /* Relay channels */ static const uint32_t kRelayChannel = 0; - static const uint32_t kCANJaguarRelayChannel = 1; - - /* CAN IDs */ - static const uint32_t kCANJaguarID = 2; /* PDP channels */ static const uint32_t kJaguarPDPChannel = 6; diff --git a/wpilibcIntegrationTests/src/CANJaguarTest.cpp b/wpilibcIntegrationTests/src/CANJaguarTest.cpp deleted file mode 100644 index f8b95122cf..0000000000 --- a/wpilibcIntegrationTests/src/CANJaguarTest.cpp +++ /dev/null @@ -1,500 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) FIRST 2014-2016. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -#include "CANJaguar.h" // NOLINT(build/include_order) - -#include - -#include "AnalogOutput.h" -#include "DigitalOutput.h" -#include "Relay.h" -#include "TestBench.h" -#include "Timer.h" -#include "WPIErrors.h" -#include "gtest/gtest.h" - -static constexpr double kSpikeTime = 0.5; - -static constexpr double kExpectedBusVoltage = 14.0; -static constexpr double kExpectedTemperature = 25.0; - -static constexpr double kMotorTime = 0.5; - -static constexpr double kEncoderSettlingTime = 1.0; -static constexpr double kEncoderPositionTolerance = 0.1; -static constexpr double kEncoderSpeedTolerance = 30.0; - -static constexpr double kPotentiometerSettlingTime = 1.0; -static constexpr double kPotentiometerPositionTolerance = 0.1; - -static constexpr double kCurrentTolerance = 0.1; - -static constexpr double kVoltageTolerance = 0.1; - -static constexpr double kMotorVoltage = 5.0; - -static constexpr double kMotorPercent = 0.5; - -static constexpr double kMotorSpeed = 100; -class CANJaguarTest : public testing::Test { - protected: - CANJaguar* m_jaguar; - DigitalOutput *m_fakeForwardLimit, *m_fakeReverseLimit; - AnalogOutput* m_fakePotentiometer; - Relay* m_spike; - - void SetUp() override { - m_spike = new Relay(TestBench::kCANJaguarRelayChannel, Relay::kForwardOnly); - m_spike->Set(Relay::kOn); - Wait(kSpikeTime); - - m_jaguar = new CANJaguar(TestBench::kCANJaguarID); - - m_fakeForwardLimit = new DigitalOutput(TestBench::kFakeJaguarForwardLimit); - m_fakeForwardLimit->Set(0); - - m_fakeReverseLimit = new DigitalOutput(TestBench::kFakeJaguarReverseLimit); - m_fakeReverseLimit->Set(0); - - m_fakePotentiometer = new AnalogOutput(TestBench::kFakeJaguarPotentiometer); - m_fakePotentiometer->SetVoltage(0.0f); - - /* The motor might still have momentum from the previous test. */ - Wait(kEncoderSettlingTime); - } - - void TearDown() override { - delete m_jaguar; - delete m_fakeForwardLimit; - delete m_fakeReverseLimit; - delete m_fakePotentiometer; - delete m_spike; - } - - /** - * Calls CANJaguar::Set periodically 50 times to make sure everything is - * verified. This mimics a real robot program, where Set is presumably - * called in each iteration of the main loop. - */ - void SetJaguar(float totalTime, float value = 0.0f) { - for (int32_t i = 0; i < 50; i++) { - m_jaguar->Set(value); - Wait(totalTime / 50.0); - } - } - /** - * returns the sign of the given number - */ - int32_t SignNum(double value) { return -(value < 0) + (value > 0); } - void InversionTest(float motorValue, float delayTime = kMotorTime) { - m_jaguar->EnableControl(); - m_jaguar->SetInverted(false); - SetJaguar(delayTime, motorValue); - double initialSpeed = m_jaguar->GetSpeed(); - m_jaguar->Set(0.0); - m_jaguar->SetInverted(true); - SetJaguar(delayTime, motorValue); - double finalSpeed = m_jaguar->GetSpeed(); - // checks that the motor has changed direction - EXPECT_FALSE(SignNum(initialSpeed) == SignNum(finalSpeed)) - << "CAN Jaguar did not invert direction positive. Initial speed was: " - << initialSpeed << " Final displacement was: " << finalSpeed - << " Sign of initial displacement was: " << SignNum(initialSpeed) - << " Sign of final displacement was: " << SignNum(finalSpeed); - SetJaguar(delayTime, -motorValue); - initialSpeed = m_jaguar->GetSpeed(); - m_jaguar->Set(0.0); - m_jaguar->SetInverted(false); - SetJaguar(delayTime, -motorValue); - finalSpeed = m_jaguar->GetSpeed(); - EXPECT_FALSE(SignNum(initialSpeed) == SignNum(finalSpeed)) - << "CAN Jaguar did not invert direction negative. Initial displacement " - "was: " - << initialSpeed << " Final displacement was: " << finalSpeed - << " Sign of initial displacement was: " << SignNum(initialSpeed) - << " Sign of final displacement was: " << SignNum(finalSpeed); - } -}; - -/** - * Tests that allocating the same CANJaguar port as an already allocated port - * causes a ResourceAlreadyAllocated error. - */ -TEST_F(CANJaguarTest, AlreadyAllocatedError) { - std::cout << "The following errors are expected." << std::endl << std::endl; - - CANJaguar jaguar(TestBench::kCANJaguarID); - EXPECT_EQ(wpi_error_value_ResourceAlreadyAllocated, - jaguar.GetError().GetCode()) - << "An error should have been returned"; -} - -/** - * Test that allocating a CANJaguar with device number 64 causes an - * out-of-range error. - */ -TEST_F(CANJaguarTest, 64OutOfRangeError) { - std::cout << "The following errors are expected." << std::endl << std::endl; - - CANJaguar jaguar(64); - EXPECT_EQ(wpi_error_value_ChannelIndexOutOfRange, jaguar.GetError().GetCode()) - << "An error should have been returned"; -} - -/** - * Test that allocating a CANJaguar with device number 0 causes an out-of-range - * error. - */ -TEST_F(CANJaguarTest, 0OutOfRangeError) { - std::cout << "The following errors are expected." << std::endl << std::endl; - - CANJaguar jaguar(0); - EXPECT_EQ(wpi_error_value_ChannelIndexOutOfRange, jaguar.GetError().GetCode()) - << "An error should have been returned"; -} - -/** - * Checks the default status data for reasonable values to confirm that we're - * really getting status data from the Jaguar. - */ -TEST_F(CANJaguarTest, InitialStatus) { - m_jaguar->SetPercentMode(); - - EXPECT_NEAR(m_jaguar->GetBusVoltage(), kExpectedBusVoltage, 3.0) - << "Bus voltage is not a plausible value."; - - EXPECT_FLOAT_EQ(m_jaguar->GetOutputVoltage(), 0.0) - << "Output voltage is non-zero."; - - EXPECT_FLOAT_EQ(m_jaguar->GetOutputCurrent(), 0.0) - << "Output current is non-zero."; - - EXPECT_NEAR(m_jaguar->GetTemperature(), kExpectedTemperature, 5.0) - << "Temperature is not a plausible value."; - - EXPECT_EQ(m_jaguar->GetFaults(), 0) << "Jaguar has one or more fault set."; -} - -/** - * Ensure that the jaguar doesn't move when it's disabled - */ -TEST_F(CANJaguarTest, Disable) { - m_jaguar->SetPercentMode(CANJaguar::QuadEncoder, 360); - m_jaguar->EnableControl(); - m_jaguar->DisableControl(); - - Wait(kEncoderSettlingTime); - - double initialPosition = m_jaguar->GetPosition(); - - SetJaguar(kMotorTime, 1.0f); - m_jaguar->Set(0.0f); - - double finalPosition = m_jaguar->GetPosition(); - - EXPECT_NEAR(initialPosition, finalPosition, kEncoderPositionTolerance) - << "Jaguar moved while disabled"; -} - -/** - * Make sure the Jaguar keeps its state after a power cycle by setting a - * control mode, turning the spike on and off, then checking if the Jaguar - * behaves like it should in that control mode. - */ -TEST_F(CANJaguarTest, BrownOut) { - /* Set the jaguar to quad encoder position mode */ - m_jaguar->SetPositionMode(CANJaguar::QuadEncoder, 360, 20.0f, 0.01f, 0.0f); - m_jaguar->EnableControl(); - SetJaguar(kMotorTime, 0.0); - double setpoint = m_jaguar->GetPosition() + 1.0f; - - /* Turn the spike off and on again */ - m_spike->Set(Relay::kOff); - Wait(kSpikeTime); - m_spike->Set(Relay::kOn); - Wait(kSpikeTime); - - /* The jaguar should automatically get set to quad encoder position mode, - so it should be able to reach a setpoint in a couple seconds. */ - for (int32_t i = 0; i < 10; i++) { - SetJaguar(1.0f, setpoint); - - if (std::fabs(m_jaguar->GetPosition() - setpoint) <= - kEncoderPositionTolerance) { - return; - } - } - - EXPECT_NEAR(setpoint, m_jaguar->GetPosition(), kEncoderPositionTolerance) - << "CAN Jaguar should have resumed PID control after power cycle"; -} - -/** - * Test if we can set arbitrary setpoints and PID values each each applicable - * mode and get the same values back. - */ -TEST_F(CANJaguarTest, SetGet) { - m_jaguar->DisableControl(); - - m_jaguar->SetSpeedMode(CANJaguar::QuadEncoder, 360, 1, 2, 3); - m_jaguar->Set(4); - - EXPECT_FLOAT_EQ(1, m_jaguar->GetP()); - EXPECT_FLOAT_EQ(2, m_jaguar->GetI()); - EXPECT_FLOAT_EQ(3, m_jaguar->GetD()); - EXPECT_FLOAT_EQ(4, m_jaguar->Get()); -} - -/** - * Test if we can drive the motor in percentage mode and get a position back - */ -TEST_F(CANJaguarTest, PercentModeForwardWorks) { - m_jaguar->SetPercentMode(CANJaguar::QuadEncoder, 360); - m_jaguar->EnableControl(); - - /* The motor might still have momentum from the previous test. */ - SetJaguar(kEncoderSettlingTime, 0.0f); - - double initialPosition = m_jaguar->GetPosition(); - - /* Drive the speed controller briefly to move the encoder */ - SetJaguar(kMotorTime, 1.0f); - m_jaguar->Set(0.0f); - - /* The position should have increased */ - EXPECT_GT(m_jaguar->GetPosition(), initialPosition) - << "CAN Jaguar position should have increased after the motor moved"; -} - -/** - * Test if we can drive the motor backwards in percentage mode and get a - * position back - */ -TEST_F(CANJaguarTest, PercentModeReverseWorks) { - m_jaguar->SetPercentMode(CANJaguar::QuadEncoder, 360); - m_jaguar->EnableControl(); - - /* The motor might still have momentum from the previous test. */ - SetJaguar(kEncoderSettlingTime, 0.0f); - - double initialPosition = m_jaguar->GetPosition(); - - /* Drive the speed controller briefly to move the encoder */ - SetJaguar(kMotorTime, -1.0f); - m_jaguar->Set(0.0f); - - float p = m_jaguar->GetPosition(); - /* The position should have decreased */ - EXPECT_LT(p, initialPosition) - << "CAN Jaguar position should have decreased after the motor moved"; -} - -/** - * Test if we can set an absolute voltage and receive a matching output voltage - * status. - */ -TEST_F(CANJaguarTest, VoltageModeWorks) { - m_jaguar->SetVoltageMode(); - m_jaguar->EnableControl(); - - float setpoints[] = {M_PI, 8.0f, -10.0f}; - - for (auto setpoint : setpoints) { - SetJaguar(kMotorTime, setpoint); - EXPECT_NEAR(setpoint, m_jaguar->GetOutputVoltage(), kVoltageTolerance); - } -} - -/** - * Test if we can set a speed in speed control mode and receive a matching - * speed status. - */ -TEST_F(CANJaguarTest, SpeedModeWorks) { - m_jaguar->SetSpeedMode(CANJaguar::QuadEncoder, 360, 0.1f, 0.003f, 0.01f); - m_jaguar->EnableControl(); - - constexpr float speed = 50.0f; - - SetJaguar(kMotorTime, speed); - EXPECT_NEAR(speed, m_jaguar->GetSpeed(), kEncoderSpeedTolerance); -} - -/** - * Test if we can set a position and reach that position with PID control on - * the Jaguar. - */ -TEST_F(CANJaguarTest, PositionModeWorks) { - m_jaguar->SetPositionMode(CANJaguar::QuadEncoder, 360, 15.0f, 0.02f, 0.0f); - - double setpoint = m_jaguar->GetPosition() + 1.0f; - - m_jaguar->EnableControl(); - - /* It should get to the setpoint within 10 seconds */ - for (int32_t i = 0; i < 10; i++) { - SetJaguar(1.0f, setpoint); - - if (std::fabs(m_jaguar->GetPosition() - setpoint) <= - kEncoderPositionTolerance) { - return; - } - } - - EXPECT_NEAR(setpoint, m_jaguar->GetPosition(), kEncoderPositionTolerance) - << "CAN Jaguar should have reached setpoint with PID control"; -} - -/** - * Test if we can set a current setpoint with PID control on the Jaguar and get - * a corresponding output current - */ -TEST_F(CANJaguarTest, DISABLED_CurrentModeWorks) { - m_jaguar->SetCurrentMode(10.0, 4.0, 1.0); - m_jaguar->EnableControl(); - - float setpoints[] = {1.6f, 2.0f, -1.6f}; - - for (auto& setpoints_i : setpoints) { - float setpoint = setpoints_i; - float expectedCurrent = std::fabs(setpoints_i); - - /* It should get to each setpoint within 10 seconds */ - for (int32_t j = 0; j < 10; j++) { - SetJaguar(1.0, setpoint); - - if (std::fabs(m_jaguar->GetOutputCurrent() - expectedCurrent) <= - kCurrentTolerance) { - break; - } - } - - EXPECT_NEAR(expectedCurrent, m_jaguar->GetOutputCurrent(), - kCurrentTolerance); - } -} - -/** - * Test if we can get a position in potentiometer mode, using an analog output - * as a fake potentiometer. - */ -TEST_F(CANJaguarTest, FakePotentiometerPosition) { - m_jaguar->SetPercentMode(CANJaguar::Potentiometer); - m_jaguar->EnableControl(); - - // Set the analog output to 4 different voltages and check if the Jaguar - // returns corresponding positions. - for (int32_t i = 0; i <= 3; i++) { - m_fakePotentiometer->SetVoltage(static_cast(i)); - - SetJaguar(kPotentiometerSettlingTime); - - EXPECT_NEAR(m_fakePotentiometer->GetVoltage() / 3.0f, - m_jaguar->GetPosition(), kPotentiometerPositionTolerance) - << "CAN Jaguar should have returned the potentiometer position set by " - "the analog output"; - } -} - -/** - * Test if we can limit the Jaguar to only moving in reverse with a fake - * limit switch. - */ -TEST_F(CANJaguarTest, FakeLimitSwitchForwards) { - m_jaguar->SetPercentMode(CANJaguar::QuadEncoder, 360); - m_jaguar->ConfigLimitMode(CANJaguar::kLimitMode_SwitchInputsOnly); - m_fakeForwardLimit->Set(1); - m_fakeReverseLimit->Set(0); - m_jaguar->EnableControl(); - - SetJaguar(kEncoderSettlingTime); - - /* Make sure the limits are recognized by the Jaguar. */ - ASSERT_FALSE(m_jaguar->GetForwardLimitOK()); - ASSERT_TRUE(m_jaguar->GetReverseLimitOK()); - - double initialPosition = m_jaguar->GetPosition(); - - /* Drive the speed controller briefly to move the encoder. If the limit - switch is recognized, it shouldn't actually move. */ - SetJaguar(kMotorTime, 1.0f); - - /* The position should be the same, since the limit switch was on. */ - EXPECT_NEAR(initialPosition, m_jaguar->GetPosition(), - kEncoderPositionTolerance) - << "CAN Jaguar should not have moved with the limit switch pressed"; - - /* Drive the speed controller in the other direction. It should actually - move, since only the forward switch is activated.*/ - SetJaguar(kMotorTime, -1.0f); - - /* The position should have decreased */ - EXPECT_LT(m_jaguar->GetPosition(), initialPosition) - << "CAN Jaguar should have moved in reverse while the forward limit was " - "on"; -} - -/** - * Test if we can limit the Jaguar to only moving forwards with a fake limit - * switch. - */ -TEST_F(CANJaguarTest, FakeLimitSwitchReverse) { - m_jaguar->SetPercentMode(CANJaguar::QuadEncoder, 360); - m_jaguar->ConfigLimitMode(CANJaguar::kLimitMode_SwitchInputsOnly); - m_fakeForwardLimit->Set(0); - m_fakeReverseLimit->Set(1); - m_jaguar->EnableControl(); - - SetJaguar(kEncoderSettlingTime); - - /* Make sure the limits are recognized by the Jaguar. */ - ASSERT_TRUE(m_jaguar->GetForwardLimitOK()); - ASSERT_FALSE(m_jaguar->GetReverseLimitOK()); - - double initialPosition = m_jaguar->GetPosition(); - - /* Drive the speed controller backwards briefly to move the encoder. If - the limit switch is recognized, it shouldn't actually move. */ - SetJaguar(kMotorTime, -1.0f); - - /* The position should be the same, since the limit switch was on. */ - EXPECT_NEAR(initialPosition, m_jaguar->GetPosition(), - kEncoderPositionTolerance) - << "CAN Jaguar should not have moved with the limit switch pressed"; - - /* Drive the speed controller in the other direction. It should actually - move, since only the reverse switch is activated.*/ - SetJaguar(kMotorTime, 1.0f); - - /* The position should have increased */ - EXPECT_GT(m_jaguar->GetPosition(), initialPosition) - << "CAN Jaguar should have moved forwards while the reverse limit was on"; -} -/** -* Tests that inversion works in voltage mode -*/ -TEST_F(CANJaguarTest, InvertingVoltageMode) { - m_jaguar->SetVoltageMode(CANJaguar::QuadEncoder, 360); - m_jaguar->EnableControl(); - InversionTest(kMotorVoltage); -} - -/** -* Tests that inversion works in percentMode -*/ -TEST_F(CANJaguarTest, InvertingPercentMode) { - m_jaguar->SetPercentMode(CANJaguar::QuadEncoder, 360); - m_jaguar->EnableControl(); - InversionTest(kMotorPercent); -} -/** -* Tests that inversion works in SpeedMode -*/ -TEST_F(CANJaguarTest, InvertingSpeedMode) { - m_jaguar->SetSpeedMode(CANJaguar::QuadEncoder, 360, 0.1f, 0.005f, 0.00f); - m_jaguar->EnableControl(); - InversionTest(kMotorSpeed, kMotorTime); -} diff --git a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/CANJaguar.java b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/CANJaguar.java deleted file mode 100644 index 7d18711409..0000000000 --- a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/CANJaguar.java +++ /dev/null @@ -1,2300 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) FIRST 2008-2016. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package edu.wpi.first.wpilibj; - -import java.nio.ByteBuffer; -import java.nio.ByteOrder; - -import edu.wpi.first.wpilibj.can.CANJNI; -import edu.wpi.first.wpilibj.can.CANMessageNotFoundException; -import edu.wpi.first.wpilibj.tables.ITable; -import edu.wpi.first.wpilibj.tables.ITableListener; -import edu.wpi.first.wpilibj.util.AllocationException; -import edu.wpi.first.wpilibj.util.CheckedAllocationException; - -/** - * Texas Instruments Jaguar Speed Controller as a CAN device. - */ -public class CANJaguar implements MotorSafety, PIDOutput, CANSpeedController { - - public static final int kMaxMessageDataSize = 8; - - // The internal PID control loop in the Jaguar runs at 1kHz. - public static final int kControllerRate = 1000; - public static final double kApproxBusVoltage = 12.0; - - private MotorSafetyHelper m_safetyHelper; - private static final Resource allocated = new Resource(63); - - private static final int kFullMessageIDMask = CANJNI.CAN_MSGID_API_M | CANJNI.CAN_MSGID_MFR_M - | CANJNI.CAN_MSGID_DTYPE_M; - private static final int kSendMessagePeriod = 20; - - // Control Mode tags - private static class EncoderTag { - } - - /** - * Sets an encoder as the speed reference only.
Passed as the "tag" when setting the control - * mode. - */ - public static final EncoderTag kEncoder = new EncoderTag(); - - private static class QuadEncoderTag { - } - - /** - * Sets a quadrature encoder as the position and speed reference.
Passed as the "tag" when - * setting the control mode. - */ - public static final QuadEncoderTag kQuadEncoder = new QuadEncoderTag(); - - private static class PotentiometerTag { - } - - /** - * Sets a potentiometer as the position reference only.
Passed as the "tag" when setting the - * control mode. - */ - public static final PotentiometerTag kPotentiometer = new PotentiometerTag(); - - private boolean m_isInverted = false; - - /** - * Mode determines how the Jaguar is controlled, used internally. - */ - public enum JaguarControlMode implements CANSpeedController.ControlMode { - PercentVbus, Current, Speed, Position, Voltage; - - @Override - public boolean isPID() { - return this == Current || this == Speed || this == Position; - } - - @Override - public int getValue() { - return ordinal(); - } - - } - - public static final int kCurrentFault = 1; - public static final int kTemperatureFault = 2; - public static final int kBusVoltageFault = 4; - public static final int kGateDriverFault = 8; - - /** - * Limit switch masks. - */ - public static final int kForwardLimit = 1; - public static final int kReverseLimit = 2; - - /** - * Determines how the Jaguar behaves when sending a zero signal. - */ - public enum NeutralMode { - /** - * Use the NeutralMode that is set by the jumper wire on the CAN device. - */ - Jumper((byte) 0), - /** - * Stop the motor's rotation by applying a force. - */ - Brake((byte) 1), - /** - * Do not attempt to stop the motor. Instead allow it to coast to a stop without applying - * resistance. - */ - Coast((byte) 2); - - @SuppressWarnings("MemberName") - public final byte value; - - @SuppressWarnings("JavadocMethod") - public static NeutralMode valueOf(byte value) { - for (NeutralMode mode : values()) { - if (mode.value == value) { - return mode; - } - } - - return null; - } - - NeutralMode(byte value) { - this.value = value; - } - } - - - /** - * Determines which sensor to use for position reference. Limit switches will always be used to - * limit the rotation. This can not be disabled. - */ - public enum LimitMode { - /** - * Disables the soft position limits and only uses the limit switches to limit rotation. - * - * @see CANJaguar#getForwardLimitOK() - * @see CANJaguar#getReverseLimitOK() - */ - SwitchInputsOnly((byte) 0), - /** - * Enables the soft position limits on the Jaguar. These will be used in addition to the limit - * switches. This does not disable the behavior of the limit switch input. - * - * @see CANJaguar#configSoftPositionLimits(double, double) - */ - SoftPositionLimits((byte) 1); - - @SuppressWarnings("MemberName") - public final byte value; - - @SuppressWarnings("JavadocMethod") - public static LimitMode valueOf(byte value) { - for (LimitMode mode : values()) { - if (mode.value == value) { - return mode; - } - } - - return null; - } - - LimitMode(byte value) { - this.value = value; - } - } - - /** - * Constructor for the CANJaguar device.
By default the device is configured in Percent mode. - * The control mode can be changed by calling one of the control modes listed below. - * - * @param deviceNumber The address of the Jaguar on the CAN bus. - * @see CANJaguar#setCurrentMode(double, double, double) - * @see CANJaguar#setCurrentMode(PotentiometerTag, double, double, double) - * @see CANJaguar#setCurrentMode(EncoderTag, int, double, double, double) - * @see CANJaguar#setCurrentMode(QuadEncoderTag, int, double, double, double) - * @see CANJaguar#setPercentMode() - * @see CANJaguar#setPercentMode(PotentiometerTag) - * @see CANJaguar#setPercentMode(EncoderTag, int) - * @see CANJaguar#setPercentMode(QuadEncoderTag, int) - * @see CANJaguar#setPositionMode(PotentiometerTag, double, double, double) - * @see CANJaguar#setPositionMode(QuadEncoderTag, int, double, double, double) - * @see CANJaguar#setSpeedMode(EncoderTag, int, double, double, double) - * @see CANJaguar#setSpeedMode(QuadEncoderTag, int, double, double, double) - * @see CANJaguar#setVoltageMode() - * @see CANJaguar#setVoltageMode(PotentiometerTag) - * @see CANJaguar#setVoltageMode(EncoderTag, int) - * @see CANJaguar#setVoltageMode(QuadEncoderTag, int) - */ - public CANJaguar(int deviceNumber) { - try { - allocated.allocate(deviceNumber - 1); - } catch (CheckedAllocationException e1) { - throw new AllocationException("CANJaguar device " + e1.getMessage() - + "(increment index by one)"); - } - - m_deviceNumber = (byte) deviceNumber; - m_controlMode = JaguarControlMode.PercentVbus; - - m_safetyHelper = new MotorSafetyHelper(this); - - boolean receivedFirmwareVersion = false; - byte[] data = new byte[8]; - - // Request firmware and hardware version only once - requestMessage(CANJNI.CAN_IS_FRAME_REMOTE | CANJNI.CAN_MSGID_API_FIRMVER); - requestMessage(CANJNI.LM_API_HWVER); - - // Wait until we've gotten all of the status data at least once. - for (int i = 0; i < kReceiveStatusAttempts; i++) { - Timer.delay(0.001); - - setupPeriodicStatus(); - updatePeriodicStatus(); - - if (!receivedFirmwareVersion) { - try { - getMessage(CANJNI.CAN_MSGID_API_FIRMVER, CANJNI.CAN_MSGID_FULL_M, data); - m_firmwareVersion = unpackINT32(data); - receivedFirmwareVersion = true; - } catch (CANMessageNotFoundException ex) { - // no-op - } - } - - if (m_receivedStatusMessage0 && m_receivedStatusMessage1 && m_receivedStatusMessage2 - && receivedFirmwareVersion) { - break; - } - } - - if (!m_receivedStatusMessage0 || !m_receivedStatusMessage1 || !m_receivedStatusMessage2 - || !receivedFirmwareVersion) { - /* Free the resource */ - free(); - throw new CANMessageNotFoundException(); - } - - try { - getMessage(CANJNI.LM_API_HWVER, CANJNI.CAN_MSGID_FULL_M, data); - m_hardwareVersion = data[0]; - } catch (CANMessageNotFoundException ex) { - // Not all Jaguar firmware reports a hardware version. - m_hardwareVersion = 0; - } - - // 3330 was the first shipping RDK firmware version for the Jaguar - if (m_firmwareVersion >= 3330 || m_firmwareVersion < 108) { - if (m_firmwareVersion < 3330) { - DriverStation.reportError("Jag " + m_deviceNumber + " firmware " + m_firmwareVersion - + " is too old (must be at least version 108 of the FIRST approved firmware)", false); - } else { - DriverStation - .reportError( - "Jag" - + m_deviceNumber - + " firmware " - + m_firmwareVersion - + " is not FIRST approved (must be at least version 108 of the FIRST approved" - + " firmware)", - false); - } - return; - } - } - - /** - * Cancel periodic messages to the Jaguar, effectively disabling it. No other methods should be - * called after this is called. - */ - public void free() { - allocated.free(m_deviceNumber - 1); - m_safetyHelper = null; - - int messageID; - - // Disable periodic setpoints - switch (m_controlMode) { - case PercentVbus: - messageID = m_deviceNumber | CANJNI.LM_API_VOLT_T_SET; - break; - - case Speed: - messageID = m_deviceNumber | CANJNI.LM_API_SPD_T_SET; - break; - - case Position: - messageID = m_deviceNumber | CANJNI.LM_API_POS_T_SET; - break; - - case Current: - messageID = m_deviceNumber | CANJNI.LM_API_ICTRL_T_SET; - break; - - case Voltage: - messageID = m_deviceNumber | CANJNI.LM_API_VCOMP_T_SET; - break; - - default: - return; - } - - CANJNI.FRCNetCommCANSessionMuxSendMessage(messageID, null, - CANJNI.CAN_SEND_PERIOD_STOP_REPEATING); - - configMaxOutputVoltage(kApproxBusVoltage); - } - - /** - * The CAN ID. - * - * @return The CAN ID passed in the constructor - */ - int getDeviceNumber() { - return m_deviceNumber; - } - - /** - * Get the recently set outputValue set point. - * - *

The scale and the units depend on the mode the Jaguar is in.
In percentVbus mode, the - * outputValue is from -1.0 to 1.0 (same as PWM Jaguar).
In voltage mode, the outputValue is - * in volts.
In current mode, the outputValue is in amps.
In speed mode, the outputValue - * is in rotations/minute.
In position mode, the outputValue is in rotations.
- * - * @return The most recently set outputValue set point. - */ - @Override - public double get() { - return m_value; - } - - /** - * Equivalent to {@link #get()}. - */ - @Override - public double getSetpoint() { - return get(); - } - - /** - * Get the difference between the setpoint and goal in closed loop modes. - * - *

Outside of position and velocity modes the return value of getError() has relatively little - * meaning. - * - * @return The difference between the setpoint and the current position. - */ - @Override - public double getError() { - return get() - getPosition(); - } - - /** - * Sets the output set-point value. - * - *

The scale and the units depend on the mode the Jaguar is in.
In percentVbus Mode, the - * outputValue is from -1.0 to 1.0 (same as PWM Jaguar).
In voltage Mode, the outputValue is - * in volts.
In current Mode, the outputValue is in amps.
In speed mode, the outputValue - * is in rotations/minute.
In position Mode, the outputValue is in rotations. - * - * @param outputValue The set-point to sent to the motor controller. - * @param syncGroup The update group to add this set() to, pending UpdateSyncGroup(). If 0, - * update immediately. - */ - public void set(double outputValue, byte syncGroup) { - int messageID; - byte[] data = new byte[8]; - byte dataSize; - - if (m_safetyHelper != null) { - m_safetyHelper.feed(); - } - - if (m_stopped) { - enableControl(); - m_stopped = false; - } - - if (m_controlEnabled) { - switch (m_controlMode) { - case PercentVbus: - messageID = CANJNI.LM_API_VOLT_T_SET; - dataSize = packPercentage(data, m_isInverted ? -outputValue : outputValue); - break; - - case Speed: - messageID = CANJNI.LM_API_SPD_T_SET; - dataSize = packFXP16_16(data, m_isInverted ? -outputValue : outputValue); - break; - - case Position: - messageID = CANJNI.LM_API_POS_T_SET; - dataSize = packFXP16_16(data, outputValue); - break; - - case Current: - messageID = CANJNI.LM_API_ICTRL_T_SET; - dataSize = packFXP8_8(data, outputValue); - break; - - - case Voltage: - messageID = CANJNI.LM_API_VCOMP_T_SET; - dataSize = packFXP8_8(data, m_isInverted ? -outputValue : outputValue); - break; - - default: - return; - } - - if (syncGroup != 0) { - data[dataSize++] = syncGroup; - } - - sendMessage(messageID, data, dataSize, kSendMessagePeriod); - } - - m_value = outputValue; - - verify(); - } - - /** - * Sets the output set-point value. - * - *

The scale and the units depend on the mode the Jaguar is in.
In percentVbus mode, the - * outputValue is from -1.0 to 1.0 (same as PWM Jaguar).
In voltage mode, the outputValue is - * in volts.
In current mode, the outputValue is in amps.
In speed mode, the outputValue - * is in rotations/minute.
In position mode, the outputValue is in rotations. - * - * @param value The set-point to sent to the motor controller. - */ - @Override - public void set(double value) { - set(value, (byte) 0); - } - - /** - * Equivalent to {@link #set(double)}. Implements PIDInterface. - */ - @Override - public void setSetpoint(double value) { - set(value); - } - - @Override - public void reset() { - set(m_value); - disableControl(); - } - - /** - * Inverts the direction of rotation of the motor Only works in percentVbus, Speed, and Voltage - * mode. - * - * @param isInverted The state of inversion true is inverted - */ - @Override - public void setInverted(boolean isInverted) { - m_isInverted = isInverted; - } - - /** - * Common interface for the inverting direction of a speed controller. - * - * @return isInverted The state of inversion, true is inverted. - */ - @Override - public boolean getInverted() { - return m_isInverted; - } - - /** - * Check all unverified params and make sure they're equal to their local cached versions. If a - * value isn't available, it gets requested. If a value doesn't match up, it gets set again. - */ - protected void verify() { - byte[] data = new byte[8]; - - // If the Jaguar lost power, everything should be considered unverified - try { - getMessage(CANJNI.LM_API_STATUS_POWER, CANJNI.CAN_MSGID_FULL_M, data); - boolean powerCycled = data[0] != 0; - - if (powerCycled) { - // Clear the power cycled bit - data[0] = 1; - sendMessage(CANJNI.LM_API_STATUS_POWER, data, 1); - - // Mark everything as unverified - m_controlModeVerified = false; - m_speedRefVerified = false; - m_posRefVerified = false; - m_neutralModeVerified = false; - m_encoderCodesPerRevVerified = false; - m_potentiometerTurnsVerified = false; - m_forwardLimitVerified = false; - m_reverseLimitVerified = false; - m_limitModeVerified = false; - m_maxOutputVoltageVerified = false; - m_faultTimeVerified = false; - - if (m_controlMode == JaguarControlMode.PercentVbus - || m_controlMode == JaguarControlMode.Voltage) { - m_voltageRampRateVerified = false; - } else { - m_pVerified = false; - m_iVerified = false; - m_dVerified = false; - } - - // Verify periodic status messages again - m_receivedStatusMessage0 = false; - m_receivedStatusMessage1 = false; - m_receivedStatusMessage2 = false; - - // Remove any old values from netcomms. Otherwise, parameters - // are incorrectly marked as verified based on stale messages. - int[] messages = - new int[]{CANJNI.LM_API_SPD_REF, CANJNI.LM_API_POS_REF, CANJNI.LM_API_SPD_PC, - CANJNI.LM_API_POS_PC, CANJNI.LM_API_ICTRL_PC, CANJNI.LM_API_SPD_IC, - CANJNI.LM_API_POS_IC, CANJNI.LM_API_ICTRL_IC, CANJNI.LM_API_SPD_DC, - CANJNI.LM_API_POS_DC, CANJNI.LM_API_ICTRL_DC, CANJNI.LM_API_CFG_ENC_LINES, - CANJNI.LM_API_CFG_POT_TURNS, CANJNI.LM_API_CFG_BRAKE_COAST, - CANJNI.LM_API_CFG_LIMIT_MODE, CANJNI.LM_API_CFG_LIMIT_REV, - CANJNI.LM_API_CFG_MAX_VOUT, CANJNI.LM_API_VOLT_SET_RAMP, - CANJNI.LM_API_VCOMP_COMP_RAMP, CANJNI.LM_API_CFG_FAULT_TIME, - CANJNI.LM_API_CFG_LIMIT_FWD}; - - for (int message : messages) { - try { - getMessage(message, CANJNI.CAN_MSGID_FULL_M, data); - } catch (CANMessageNotFoundException ex) { - // no-op - } - } - } - } catch (CANMessageNotFoundException ex) { - requestMessage(CANJNI.LM_API_STATUS_POWER); - } - - // Verify that any recently set parameters are correct - if (!m_controlModeVerified && m_controlEnabled) { - try { - getMessage(CANJNI.LM_API_STATUS_CMODE, CANJNI.CAN_MSGID_FULL_M, data); - - JaguarControlMode mode = JaguarControlMode.values()[data[0]]; - - if (m_controlMode == mode) { - m_controlModeVerified = true; - } else { - // Enable control again to resend the control mode - enableControl(); - } - } catch (CANMessageNotFoundException ex) { - // Verification is needed but not available - request it again. - requestMessage(CANJNI.LM_API_STATUS_CMODE); - } - } - - if (!m_speedRefVerified) { - try { - getMessage(CANJNI.LM_API_SPD_REF, CANJNI.CAN_MSGID_FULL_M, data); - - int speedRef = data[0]; - - if (m_speedReference == speedRef) { - m_speedRefVerified = true; - } else { - // It's wrong - set it again - setSpeedReference(m_speedReference); - } - } catch (CANMessageNotFoundException ex) { - // Verification is needed but not available - request it again. - requestMessage(CANJNI.LM_API_SPD_REF); - } - } - - if (!m_posRefVerified) { - try { - getMessage(CANJNI.LM_API_POS_REF, CANJNI.CAN_MSGID_FULL_M, data); - - int posRef = data[0]; - - if (m_positionReference == posRef) { - m_posRefVerified = true; - } else { - // It's wrong - set it again - setPositionReference(m_positionReference); - } - } catch (CANMessageNotFoundException ex) { - // Verification is needed but not available - request it again. - requestMessage(CANJNI.LM_API_POS_REF); - } - } - - if (!m_pVerified) { - int message = 0; - - switch (m_controlMode) { - case Speed: - message = CANJNI.LM_API_SPD_PC; - break; - - case Position: - message = CANJNI.LM_API_POS_PC; - break; - - case Current: - message = CANJNI.LM_API_ICTRL_PC; - break; - - default: - break; - } - - try { - getMessage(message, CANJNI.CAN_MSGID_FULL_M, data); - - double punpacked = unpackFXP16_16(data); - - if (FXP16_EQ(m_p, punpacked)) { - m_pVerified = true; - } else { - // It's wrong - set it again - setP(m_p); - } - } catch (CANMessageNotFoundException ex) { - // Verification is needed but not available - request it again. - requestMessage(message); - } - } - - if (!m_iVerified) { - int message = 0; - - switch (m_controlMode) { - case Speed: - message = CANJNI.LM_API_SPD_IC; - break; - - case Position: - message = CANJNI.LM_API_POS_IC; - break; - - case Current: - message = CANJNI.LM_API_ICTRL_IC; - break; - - default: - break; - } - - try { - getMessage(message, CANJNI.CAN_MSGID_FULL_M, data); - - double iunpacked = unpackFXP16_16(data); - - if (FXP16_EQ(m_i, iunpacked)) { - m_iVerified = true; - } else { - // It's wrong - set it again - setI(m_i); - } - } catch (CANMessageNotFoundException ex) { - // Verification is needed but not available - request it again. - requestMessage(message); - } - } - - if (!m_dVerified) { - int message = 0; - - switch (m_controlMode) { - case Speed: - message = CANJNI.LM_API_SPD_DC; - break; - - case Position: - message = CANJNI.LM_API_POS_DC; - break; - - case Current: - message = CANJNI.LM_API_ICTRL_DC; - break; - - default: - break; - } - - try { - getMessage(message, CANJNI.CAN_MSGID_FULL_M, data); - - double dunpacked = unpackFXP16_16(data); - - if (FXP16_EQ(m_d, dunpacked)) { - m_dVerified = true; - } else { - // It's wrong - set it again - setD(m_d); - } - } catch (CANMessageNotFoundException ex) { - // Verification is needed but not available - request it again. - requestMessage(message); - } - } - - if (!m_neutralModeVerified) { - try { - getMessage(CANJNI.LM_API_CFG_BRAKE_COAST, CANJNI.CAN_MSGID_FULL_M, data); - - NeutralMode mode = NeutralMode.valueOf(data[0]); - - if (mode == m_neutralMode) { - m_neutralModeVerified = true; - } else { - // It's wrong - set it again - configNeutralMode(m_neutralMode); - } - } catch (CANMessageNotFoundException ex) { - // Verification is needed but not available - request it again. - requestMessage(CANJNI.LM_API_CFG_BRAKE_COAST); - } - } - - if (!m_encoderCodesPerRevVerified) { - try { - getMessage(CANJNI.LM_API_CFG_ENC_LINES, CANJNI.CAN_MSGID_FULL_M, data); - - short codes = unpackINT16(data); - - if (codes == m_encoderCodesPerRev) { - m_encoderCodesPerRevVerified = true; - } else { - // It's wrong - set it again - configEncoderCodesPerRev(m_encoderCodesPerRev); - } - } catch (CANMessageNotFoundException ex) { - // Verification is needed but not available - request it again. - requestMessage(CANJNI.LM_API_CFG_ENC_LINES); - } - } - - if (!m_potentiometerTurnsVerified) { - try { - getMessage(CANJNI.LM_API_CFG_POT_TURNS, CANJNI.CAN_MSGID_FULL_M, data); - - short turns = unpackINT16(data); - - if (turns == m_potentiometerTurns) { - m_potentiometerTurnsVerified = true; - } else { - // It's wrong - set it again - configPotentiometerTurns(m_potentiometerTurns); - } - } catch (CANMessageNotFoundException ex) { - // Verification is needed but not available - request it again. - requestMessage(CANJNI.LM_API_CFG_POT_TURNS); - } - } - - if (!m_limitModeVerified) { - try { - getMessage(CANJNI.LM_API_CFG_LIMIT_MODE, CANJNI.CAN_MSGID_FULL_M, data); - - LimitMode mode = LimitMode.valueOf(data[0]); - - if (mode == m_limitMode) { - m_limitModeVerified = true; - } else { - // It's wrong - set it again - configLimitMode(m_limitMode); - } - } catch (CANMessageNotFoundException ex) { - // Verification is needed but not available - request it again. - requestMessage(CANJNI.LM_API_CFG_LIMIT_MODE); - } - } - - if (!m_forwardLimitVerified) { - try { - getMessage(CANJNI.LM_API_CFG_LIMIT_FWD, CANJNI.CAN_MSGID_FULL_M, data); - - double limit = unpackFXP16_16(data); - - if (FXP16_EQ(limit, m_forwardLimit)) { - m_forwardLimitVerified = true; - } else { - // It's wrong - set it again - configForwardLimit(m_forwardLimit); - } - } catch (CANMessageNotFoundException ex) { - // Verification is needed but not available - request it again. - requestMessage(CANJNI.LM_API_CFG_LIMIT_FWD); - } - } - - if (!m_reverseLimitVerified) { - try { - getMessage(CANJNI.LM_API_CFG_LIMIT_REV, CANJNI.CAN_MSGID_FULL_M, data); - - double limit = unpackFXP16_16(data); - - if (FXP16_EQ(limit, m_reverseLimit)) { - m_reverseLimitVerified = true; - } else { - // It's wrong - set it again - configReverseLimit(m_reverseLimit); - } - } catch (CANMessageNotFoundException ex) { - // Verification is needed but not available - request it again. - requestMessage(CANJNI.LM_API_CFG_LIMIT_REV); - } - } - - if (!m_maxOutputVoltageVerified) { - try { - getMessage(CANJNI.LM_API_CFG_MAX_VOUT, CANJNI.CAN_MSGID_FULL_M, data); - - double voltage = unpackFXP8_8(data); - - // The returned max output voltage is sometimes slightly higher - // or lower than what was sent. This should not trigger - // resending the message. - if (Math.abs(voltage - m_maxOutputVoltage) < 0.1) { - m_maxOutputVoltageVerified = true; - } else { - // It's wrong - set it again - configMaxOutputVoltage(m_maxOutputVoltage); - } - - } catch (CANMessageNotFoundException ex) { - // Verification is needed but not available - request it again. - requestMessage(CANJNI.LM_API_CFG_MAX_VOUT); - } - } - - if (!m_voltageRampRateVerified) { - if (m_controlMode == JaguarControlMode.PercentVbus) { - try { - getMessage(CANJNI.LM_API_VOLT_SET_RAMP, CANJNI.CAN_MSGID_FULL_M, data); - - double rate = unpackPercentage(data); - - if (FXP16_EQ(rate, m_voltageRampRate)) { - m_voltageRampRateVerified = true; - } else { - // It's wrong - set it again - setVoltageRampRate(m_voltageRampRate); - } - - } catch (CANMessageNotFoundException ex) { - // Verification is needed but not available - request it again. - requestMessage(CANJNI.LM_API_VOLT_SET_RAMP); - } - } - } else if (m_controlMode == JaguarControlMode.Voltage) { - try { - getMessage(CANJNI.LM_API_VCOMP_COMP_RAMP, CANJNI.CAN_MSGID_FULL_M, data); - - double rate = unpackFXP8_8(data); - - if (FXP8_EQ(rate, m_voltageRampRate)) { - m_voltageRampRateVerified = true; - } else { - // It's wrong - set it again - setVoltageRampRate(m_voltageRampRate); - } - - } catch (CANMessageNotFoundException ex) { - // Verification is needed but not available - request it again. - requestMessage(CANJNI.LM_API_VCOMP_COMP_RAMP); - } - } - - if (!m_faultTimeVerified) { - try { - getMessage(CANJNI.LM_API_CFG_FAULT_TIME, CANJNI.CAN_MSGID_FULL_M, data); - - int faultTime = unpackINT16(data); - - if ((int) (m_faultTime * 1000.0) == faultTime) { - m_faultTimeVerified = true; - } else { - // It's wrong - set it again - configFaultTime(m_faultTime); - } - } catch (CANMessageNotFoundException ex) { - // Verification is needed but not available - request it again. - requestMessage(CANJNI.LM_API_CFG_FAULT_TIME); - } - } - - if (!m_receivedStatusMessage0 || !m_receivedStatusMessage1 || !m_receivedStatusMessage2) { - // If the periodic status messages haven't been verified as received, - // request periodic status messages again and attempt to unpack any - // available ones. - setupPeriodicStatus(); - getTemperature(); - getPosition(); - getFaults(); - } - } - - /** - * Common interface for disabling a motor. - * - * @deprecated Call {@link #disableControl()} instead. - */ - @Deprecated - @Override - public void disable() { - disableControl(); - } - - // PIDInterface interface - public void enable() { - enableControl(); - } - - // PIDOutput interface - @Override - public void pidWrite(double output) { - if (m_controlMode == JaguarControlMode.PercentVbus) { - set(output); - } else { - throw new IllegalStateException("PID only supported in PercentVbus mode"); - } - } - - /** - * Set the reference source device for speed controller mode. - * - *

Choose encoder as the source of speed feedback when in speed control mode. - * - * @param reference Specify a speed reference. - */ - private void setSpeedReference(int reference) { - sendMessage(CANJNI.LM_API_SPD_REF, new byte[]{(byte) reference}, 1); - - m_speedReference = reference; - m_speedRefVerified = false; - } - - /** - * Set the reference source device for position controller mode. - * - *

Choose between using and encoder and using a potentiometer as the source of position - * feedback when in position control mode. - * - * @param reference Specify a position reference. - */ - private void setPositionReference(int reference) { - sendMessage(CANJNI.LM_API_POS_REF, new byte[]{(byte) reference}, 1); - - m_positionReference = reference; - m_posRefVerified = false; - } - - /** - * Set the P constant for the closed loop modes. - * - * @param p The proportional gain of the Jaguar's PID controller. - */ - @SuppressWarnings("ParameterName") - public void setP(double p) { - byte[] data = new byte[8]; - byte dataSize = packFXP16_16(data, p); - - switch (m_controlMode) { - case Speed: - sendMessage(CANJNI.LM_API_SPD_PC, data, dataSize); - break; - - case Position: - sendMessage(CANJNI.LM_API_POS_PC, data, dataSize); - break; - - case Current: - sendMessage(CANJNI.LM_API_ICTRL_PC, data, dataSize); - break; - - default: - throw new IllegalStateException( - "PID constants only apply in Speed, Position, and Current mode"); - } - - m_p = p; - m_pVerified = false; - } - - /** - * Set the I constant for the closed loop modes. - * - * @param i The integral gain of the Jaguar's PID controller. - */ - @SuppressWarnings("ParameterName") - public void setI(double i) { - byte[] data = new byte[8]; - byte dataSize = packFXP16_16(data, i); - - switch (m_controlMode) { - case Speed: - sendMessage(CANJNI.LM_API_SPD_IC, data, dataSize); - break; - - case Position: - sendMessage(CANJNI.LM_API_POS_IC, data, dataSize); - break; - - case Current: - sendMessage(CANJNI.LM_API_ICTRL_IC, data, dataSize); - break; - - default: - throw new IllegalStateException( - "PID constants only apply in Speed, Position, and Current mode"); - } - - m_i = i; - m_iVerified = false; - } - - /** - * Set the D constant for the closed loop modes. - * - * @param d The derivative gain of the Jaguar's PID controller. - */ - @SuppressWarnings("ParameterName") - public void setD(double d) { - byte[] data = new byte[8]; - byte dataSize = packFXP16_16(data, d); - - switch (m_controlMode) { - case Speed: - sendMessage(CANJNI.LM_API_SPD_DC, data, dataSize); - break; - - case Position: - sendMessage(CANJNI.LM_API_POS_DC, data, dataSize); - break; - - case Current: - sendMessage(CANJNI.LM_API_ICTRL_DC, data, dataSize); - break; - - default: - throw new IllegalStateException( - "PID constants only apply in Speed, Position, and Current mode"); - } - - m_d = d; - m_dVerified = false; - } - - /** - * Set the P, I, and D constants for the closed loop modes. - * - * @param p The proportional gain of the Jaguar's PID controller. - * @param i The integral gain of the Jaguar's PID controller. - * @param d The differential gain of the Jaguar's PID controller. - */ - @SuppressWarnings("ParameterName") - public void setPID(double p, double i, double d) { - setP(p); - setI(i); - setD(d); - } - - /** - * Get the Proportional gain of the controller. - * - * @return The proportional gain. - */ - public double getP() { - if (m_controlMode.equals(JaguarControlMode.PercentVbus) || m_controlMode.equals( - JaguarControlMode.Voltage)) { - throw new IllegalStateException("PID does not apply in Percent or Voltage control modes"); - } - return m_p; - } - - /** - * Get the Integral gain of the controller. - * - * @return The integral gain. - */ - public double getI() { - if (m_controlMode.equals(JaguarControlMode.PercentVbus) || m_controlMode.equals( - JaguarControlMode.Voltage)) { - throw new IllegalStateException("PID does not apply in Percent or Voltage control modes"); - } - return m_i; - } - - /** - * Get the Derivative gain of the controller. - * - * @return The derivative gain. - */ - public double getD() { - if (m_controlMode.equals(JaguarControlMode.PercentVbus) || m_controlMode.equals( - JaguarControlMode.Voltage)) { - throw new IllegalStateException("PID does not apply in Percent or Voltage control modes"); - } - return m_d; - } - - /** - * Enable the closed loop controller. - * - *

Start actually controlling the output based on the feedback. If starting a position - * controller with an encoder reference, use the encoderInitialPosition parameter to initialize - * the encoder state. - * - * @param encoderInitialPosition Encoder position to set if position with encoder reference. - * Ignored otherwise. - */ - public void enableControl(double encoderInitialPosition) { - switch (m_controlMode) { - case PercentVbus: - sendMessage(CANJNI.LM_API_VOLT_T_EN, new byte[0], 0); - break; - - case Speed: - sendMessage(CANJNI.LM_API_SPD_T_EN, new byte[0], 0); - break; - - case Position: - byte[] data = new byte[8]; - int dataSize = packFXP16_16(data, encoderInitialPosition); - sendMessage(CANJNI.LM_API_POS_T_EN, data, dataSize); - break; - - case Current: - sendMessage(CANJNI.LM_API_ICTRL_T_EN, new byte[0], 0); - break; - - case Voltage: - sendMessage(CANJNI.LM_API_VCOMP_T_EN, new byte[0], 0); - break; - default: - break; - } - - m_controlEnabled = true; - } - - /** - * Enable the closed loop controller. - * - *

Start actually controlling the output based on the feedback. This is the same as calling - * CANJaguar.enableControl(double encoderInitialPosition) with - * encoderInitialPosition set to 0.0 - */ - public void enableControl() { - enableControl(0.0); - } - - /** - * Return whether the controller is enabled. - * - * @return true if enabled. - */ - public boolean isEnabled() { - return m_controlEnabled; - } - - /** - * Disable the closed loop controller. - * - *

Stop driving the output based on the feedback. - */ - public void disableControl() { - // Disable all control modes. - sendMessage(CANJNI.LM_API_VOLT_DIS, new byte[0], 0); - sendMessage(CANJNI.LM_API_SPD_DIS, new byte[0], 0); - sendMessage(CANJNI.LM_API_POS_DIS, new byte[0], 0); - sendMessage(CANJNI.LM_API_ICTRL_DIS, new byte[0], 0); - sendMessage(CANJNI.LM_API_VCOMP_DIS, new byte[0], 0); - - // Stop all periodic setpoints - sendMessage(CANJNI.LM_API_VOLT_T_SET, new byte[0], 0, CANJNI.CAN_SEND_PERIOD_STOP_REPEATING); - sendMessage(CANJNI.LM_API_SPD_T_SET, new byte[0], 0, CANJNI.CAN_SEND_PERIOD_STOP_REPEATING); - sendMessage(CANJNI.LM_API_POS_T_SET, new byte[0], 0, CANJNI.CAN_SEND_PERIOD_STOP_REPEATING); - sendMessage(CANJNI.LM_API_ICTRL_T_SET, new byte[0], 0, CANJNI.CAN_SEND_PERIOD_STOP_REPEATING); - sendMessage(CANJNI.LM_API_VCOMP_T_SET, new byte[0], 0, CANJNI.CAN_SEND_PERIOD_STOP_REPEATING); - - m_controlEnabled = false; - } - - /** - * Enable controlling the motor voltage as a percentage of the bus voltage without any position or - * speed feedback.
After calling this you must call {@link CANJaguar#enableControl()} or - * {@link CANJaguar#enableControl(double)} to enable the device. - */ - public void setPercentMode() { - changeControlMode(JaguarControlMode.PercentVbus); - setPositionReference(CANJNI.LM_REF_NONE); - setSpeedReference(CANJNI.LM_REF_NONE); - } - - /** - * Enable controlling the motor voltage as a percentage of the bus voltage, and enable speed - * sensing from a non-quadrature encoder.
After calling this you must call {@link - * CANJaguar#enableControl()} or {@link CANJaguar#enableControl(double)} to enable the device. - * - * @param tag The constant {@link CANJaguar#kEncoder} - * @param codesPerRev The counts per revolution on the encoder - */ - public void setPercentMode(EncoderTag tag, int codesPerRev) { - changeControlMode(JaguarControlMode.PercentVbus); - setPositionReference(CANJNI.LM_REF_NONE); - setSpeedReference(CANJNI.LM_REF_ENCODER); - configEncoderCodesPerRev(codesPerRev); - } - - /** - * Enable controlling the motor voltage as a percentage of the bus voltage, and enable position - * and speed sensing from a quadrature encoder.
After calling this you must call {@link - * CANJaguar#enableControl()} or {@link CANJaguar#enableControl(double)} to enable the device. - * - * @param tag The constant {@link CANJaguar#kQuadEncoder} - * @param codesPerRev The counts per revolution on the encoder - */ - public void setPercentMode(QuadEncoderTag tag, int codesPerRev) { - changeControlMode(JaguarControlMode.PercentVbus); - setPositionReference(CANJNI.LM_REF_ENCODER); - setSpeedReference(CANJNI.LM_REF_QUAD_ENCODER); - configEncoderCodesPerRev(codesPerRev); - } - - /** - * Enable controlling the motor voltage as a percentage of the bus voltage, and enable position - * sensing from a potentiometer and no speed feedback.
After calling this you must call {@link - * CANJaguar#enableControl()} or {@link CANJaguar#enableControl(double)} to enable the device. - * - * @param tag The constant {@link CANJaguar#kPotentiometer} - */ - public void setPercentMode(PotentiometerTag tag) { - changeControlMode(JaguarControlMode.PercentVbus); - setPositionReference(CANJNI.LM_REF_POT); - setSpeedReference(CANJNI.LM_REF_NONE); - configPotentiometerTurns(1); - } - - /** - * Enable controlling the motor current with a PID loop.
After calling this you must call - * {@link CANJaguar#enableControl()} or {@link CANJaguar#enableControl(double)} to enable the - * device. - * - * @param p The proportional gain of the Jaguar's PID controller. - * @param i The integral gain of the Jaguar's PID controller. - * @param d The differential gain of the Jaguar's PID controller. - */ - @SuppressWarnings("ParameterName") - public void setCurrentMode(double p, double i, double d) { - changeControlMode(JaguarControlMode.Current); - setPositionReference(CANJNI.LM_REF_NONE); - setSpeedReference(CANJNI.LM_REF_NONE); - setPID(p, i, d); - } - - /** - * Enable controlling the motor current with a PID loop, and enable speed sensing from a - * non-quadrature encoder.
After calling this you must call {@link CANJaguar#enableControl()} - * or {@link CANJaguar#enableControl(double)} to enable the device. - * - * @param tag The constant {@link CANJaguar#kEncoder} - * @param p The proportional gain of the Jaguar's PID controller. - * @param i The integral gain of the Jaguar's PID controller. - * @param d The differential gain of the Jaguar's PID controller. - */ - @SuppressWarnings("ParameterName") - public void setCurrentMode(EncoderTag tag, int codesPerRev, double p, double i, double d) { - changeControlMode(JaguarControlMode.Current); - setPositionReference(CANJNI.LM_REF_NONE); - setSpeedReference(CANJNI.LM_REF_NONE); - configEncoderCodesPerRev(codesPerRev); - setPID(p, i, d); - } - - /** - * Enable controlling the motor current with a PID loop, and enable speed and position sensing - * from a quadrature encoder.
After calling this you must call {@link - * CANJaguar#enableControl()} or {@link CANJaguar#enableControl(double)} to enable the device. - * - * @param tag The constant {@link CANJaguar#kQuadEncoder} - * @param p The proportional gain of the Jaguar's PID controller. - * @param i The integral gain of the Jaguar's PID controller. - * @param d The differential gain of the Jaguar's PID controller. - */ - @SuppressWarnings("ParameterName") - public void setCurrentMode(QuadEncoderTag tag, int codesPerRev, double p, double i, double d) { - changeControlMode(JaguarControlMode.Current); - setPositionReference(CANJNI.LM_REF_ENCODER); - setSpeedReference(CANJNI.LM_REF_QUAD_ENCODER); - configEncoderCodesPerRev(codesPerRev); - setPID(p, i, d); - } - - /** - * Enable controlling the motor current with a PID loop, and enable position sensing from a - * potentiometer.
After calling this you must call {@link CANJaguar#enableControl()} or {@link - * CANJaguar#enableControl(double)} to enable the device. - * - * @param tag The constant {@link CANJaguar#kPotentiometer} - * @param p The proportional gain of the Jaguar's PID controller. - * @param i The integral gain of the Jaguar's PID controller. - * @param d The differential gain of the Jaguar's PID controller. - */ - @SuppressWarnings("ParameterName") - public void setCurrentMode(PotentiometerTag tag, double p, double i, double d) { - changeControlMode(JaguarControlMode.Current); - setPositionReference(CANJNI.LM_REF_POT); - setSpeedReference(CANJNI.LM_REF_NONE); - configPotentiometerTurns(1); - setPID(p, i, d); - } - - /** - * Enable controlling the speed with a feedback loop from a non-quadrature encoder.
After - * calling this you must call {@link CANJaguar#enableControl()} or {@link - * CANJaguar#enableControl(double)} to enable the device. - * - * @param tag The constant {@link CANJaguar#kEncoder} - * @param codesPerRev The counts per revolution on the encoder - * @param p The proportional gain of the Jaguar's PID controller. - * @param i The integral gain of the Jaguar's PID controller. - * @param d The differential gain of the Jaguar's PID controller. - */ - @SuppressWarnings("ParameterName") - public void setSpeedMode(EncoderTag tag, int codesPerRev, double p, double i, double d) { - changeControlMode(JaguarControlMode.Speed); - setPositionReference(CANJNI.LM_REF_NONE); - setSpeedReference(CANJNI.LM_REF_ENCODER); - configEncoderCodesPerRev(codesPerRev); - setPID(p, i, d); - } - - /** - * Enable controlling the speed with a feedback loop from a quadrature encoder.
After calling - * this you must call {@link CANJaguar#enableControl()} or {@link CANJaguar#enableControl(double)} - * to enable the device. - * - * @param tag The constant {@link CANJaguar#kQuadEncoder} - * @param codesPerRev The counts per revolution on the encoder - * @param p The proportional gain of the Jaguar's PID controller. - * @param i The integral gain of the Jaguar's PID controller. - * @param d The differential gain of the Jaguar's PID controller. - */ - @SuppressWarnings("ParameterName") - public void setSpeedMode(QuadEncoderTag tag, int codesPerRev, double p, double i, double d) { - changeControlMode(JaguarControlMode.Speed); - setPositionReference(CANJNI.LM_REF_ENCODER); - setSpeedReference(CANJNI.LM_REF_QUAD_ENCODER); - configEncoderCodesPerRev(codesPerRev); - setPID(p, i, d); - } - - /** - * Enable controlling the position with a feedback loop using an encoder.
After calling this - * you must call {@link CANJaguar#enableControl()} or {@link CANJaguar#enableControl(double)} to - * enable the device. - * - * @param tag The constant {@link CANJaguar#kQuadEncoder} - * @param codesPerRev The counts per revolution on the encoder - * @param p The proportional gain of the Jaguar's PID controller. - * @param i The integral gain of the Jaguar's PID controller. - * @param d The differential gain of the Jaguar's PID controller. - */ - @SuppressWarnings("ParameterName") - public void setPositionMode(QuadEncoderTag tag, int codesPerRev, double p, double i, double d) { - changeControlMode(JaguarControlMode.Position); - setPositionReference(CANJNI.LM_REF_ENCODER); - configEncoderCodesPerRev(codesPerRev); - setPID(p, i, d); - } - - /** - * Enable controlling the position with a feedback loop using a potentiometer.
After calling - * this you must call {@link CANJaguar#enableControl()} or {@link CANJaguar#enableControl(double)} - * to enable the device. - * - * @param tag The constant {@link CANJaguar#kPotentiometer} - * @param p The proportional gain of the Jaguar's PID controller. - * @param i The integral gain of the Jaguar's PID controller. - * @param d The differential gain of the Jaguar's PID controller. - */ - @SuppressWarnings("ParameterName") - public void setPositionMode(PotentiometerTag tag, double p, double i, double d) { - changeControlMode(JaguarControlMode.Position); - setPositionReference(CANJNI.LM_REF_POT); - configPotentiometerTurns(1); - setPID(p, i, d); - } - - /** - * Enable controlling the motor voltage without any position or speed feedback.
After calling - * this you must call {@link CANJaguar#enableControl()} or {@link CANJaguar#enableControl(double)} - * to enable the device. - */ - public void setVoltageMode() { - changeControlMode(JaguarControlMode.Voltage); - setPositionReference(CANJNI.LM_REF_NONE); - setSpeedReference(CANJNI.LM_REF_NONE); - } - - /** - * Enable controlling the motor voltage with speed feedback from a non-quadrature encoder and no - * position feedback.
After calling this you must call {@link CANJaguar#enableControl()} or - * {@link CANJaguar#enableControl(double)} to enable the device. - * - * @param tag The constant {@link CANJaguar#kEncoder} - * @param codesPerRev The counts per revolution on the encoder - */ - public void setVoltageMode(EncoderTag tag, int codesPerRev) { - changeControlMode(JaguarControlMode.Voltage); - setPositionReference(CANJNI.LM_REF_NONE); - setSpeedReference(CANJNI.LM_REF_ENCODER); - configEncoderCodesPerRev(codesPerRev); - } - - /** - * Enable controlling the motor voltage with position and speed feedback from a quadrature - * encoder.
After calling this you must call {@link CANJaguar#enableControl()} or {@link - * CANJaguar#enableControl(double)} to enable the device. - * - * @param tag The constant {@link CANJaguar#kQuadEncoder} - * @param codesPerRev The counts per revolution on the encoder - */ - public void setVoltageMode(QuadEncoderTag tag, int codesPerRev) { - changeControlMode(JaguarControlMode.Voltage); - setPositionReference(CANJNI.LM_REF_ENCODER); - setSpeedReference(CANJNI.LM_REF_QUAD_ENCODER); - configEncoderCodesPerRev(codesPerRev); - } - - /** - * Enable controlling the motor voltage with position feedback from a potentiometer and no speed - * feedback. - * - * @param tag The constant {@link CANJaguar#kPotentiometer} - */ - public void setVoltageMode(PotentiometerTag tag) { - changeControlMode(JaguarControlMode.Voltage); - setPositionReference(CANJNI.LM_REF_POT); - setSpeedReference(CANJNI.LM_REF_NONE); - configPotentiometerTurns(1); - } - - /** - * Used internally. In order to set the control mode see the methods listed below. - * - *

Change the control mode of this Jaguar object. - * - *

After changing modes, configure any PID constants or other settings needed and then - * EnableControl() to actually change the mode on the Jaguar. - * - * @param controlMode The new mode. - * @see CANJaguar#setCurrentMode(double, double, double) - * @see CANJaguar#setCurrentMode(PotentiometerTag, double, double, double) - * @see CANJaguar#setCurrentMode(EncoderTag, int, double, double, double) - * @see CANJaguar#setCurrentMode(QuadEncoderTag, int, double, double, double) - * @see CANJaguar#setPercentMode() - * @see CANJaguar#setPercentMode(PotentiometerTag) - * @see CANJaguar#setPercentMode(EncoderTag, int) - * @see CANJaguar#setPercentMode(QuadEncoderTag, int) - * @see CANJaguar#setPositionMode(PotentiometerTag, double, double, double) - * @see CANJaguar#setPositionMode(QuadEncoderTag, int, double, double, double) - * @see CANJaguar#setSpeedMode(EncoderTag, int, double, double, double) - * @see CANJaguar#setSpeedMode(QuadEncoderTag, int, double, double, double) - * @see CANJaguar#setVoltageMode() - * @see CANJaguar#setVoltageMode(PotentiometerTag) - * @see CANJaguar#setVoltageMode(EncoderTag, int) - * @see CANJaguar#setVoltageMode(QuadEncoderTag, int) - */ - private void changeControlMode(JaguarControlMode controlMode) { - // Disable the previous mode - disableControl(); - - // Update the local mode - m_controlMode = controlMode; - m_controlModeVerified = false; - } - - /** - * Get the active control mode from the Jaguar. - * - *

Ask the Jagaur what mode it is in. - * - * @return JaguarControlMode that the Jag is in. - */ - public JaguarControlMode getControlMode() { - return m_controlMode; - } - - public void setControlMode(int mode) { - changeControlMode(JaguarControlMode.values()[mode]); - } - - /** - * Get the voltage at the battery input terminals of the Jaguar. - * - * @return The bus voltage in Volts. - */ - public double getBusVoltage() { - updatePeriodicStatus(); - - return m_busVoltage; - } - - /** - * Get the voltage being output from the motor terminals of the Jaguar. - * - * @return The output voltage in Volts. - */ - public double getOutputVoltage() { - updatePeriodicStatus(); - - return m_outputVoltage; - } - - /** - * Get the current through the motor terminals of the Jaguar. - * - * @return The output current in Amps. - */ - public double getOutputCurrent() { - updatePeriodicStatus(); - - return m_outputCurrent; - } - - /** - * Get the internal temperature of the Jaguar. - * - * @return The temperature of the Jaguar in degrees Celsius. - */ - public double getTemperature() { - updatePeriodicStatus(); - - return m_temperature; - } - - /** - * Get the position of the encoder or potentiometer. - * - * @return The position of the motor in rotations based on the configured feedback. - * @see CANJaguar#configPotentiometerTurns(int) - * @see CANJaguar#configEncoderCodesPerRev(int) - */ - public double getPosition() { - updatePeriodicStatus(); - - return m_position; - } - - /** - * Get the speed of the encoder. - * - * @return The speed of the motor in RPM based on the configured feedback. - */ - public double getSpeed() { - updatePeriodicStatus(); - - return m_speed; - } - - /** - * Get the status of the forward limit switch. - * - * @return true if the motor is allowed to turn in the forward direction. - */ - public boolean getForwardLimitOK() { - updatePeriodicStatus(); - - return (m_limits & kForwardLimit) != 0; - } - - /** - * Get the status of the reverse limit switch. - * - * @return true if the motor is allowed to turn in the reverse direction. - */ - public boolean getReverseLimitOK() { - updatePeriodicStatus(); - - return (m_limits & kReverseLimit) != 0; - } - - /** - * Get the status of any faults the Jaguar has detected. - * - * @return A bit-mask of faults defined by the "Faults" constants. - * @see #kCurrentFault - * @see #kBusVoltageFault - * @see #kTemperatureFault - * @see #kGateDriverFault - */ - public short getFaults() { - updatePeriodicStatus(); - - return m_faults; - } - - /** - * set the maximum voltage change rate. - * - *

When in PercentVbus or Voltage output mode, the rate at which the voltage changes can be - * limited to reduce current spikes. set this to 0.0 to disable rate limiting. - * - * @param rampRate The maximum rate of voltage change in Percent Voltage mode in V/s. - */ - public void setVoltageRampRate(double rampRate) { - byte[] data = new byte[8]; - int dataSize; - int message; - - switch (m_controlMode) { - case PercentVbus: - dataSize = packPercentage(data, rampRate / (m_maxOutputVoltage * kControllerRate)); - message = CANJNI.LM_API_VOLT_SET_RAMP; - break; - case Voltage: - dataSize = packFXP8_8(data, rampRate / kControllerRate); - message = CANJNI.LM_API_VCOMP_COMP_RAMP; - break; - default: - throw new IllegalStateException( - "Voltage ramp rate only applies in Percentage and Voltage modes"); - } - - sendMessage(message, data, dataSize); - } - - /** - * Get the version of the firmware running on the Jaguar. - * - * @return The firmware version. 0 if the device did not respond. - */ - public int getFirmwareVersion() { - return m_firmwareVersion; - } - - /** - * Get the version of the Jaguar hardware. - * - * @return The hardware version. 1: Jaguar, 2: Black Jaguar - */ - public byte getHardwareVersion() { - return m_hardwareVersion; - } - - /** - * Configure what the controller does to the H-Bridge when neutral (not driving the output). - * - *

This allows you to override the jumper configuration for brake or coast. - * - * @param mode Select to use the jumper setting or to override it to coast or brake. - */ - public void configNeutralMode(NeutralMode mode) { - sendMessage(CANJNI.LM_API_CFG_BRAKE_COAST, new byte[]{mode.value}, 1); - - m_neutralMode = mode; - m_neutralModeVerified = false; - } - - /** - * Configure how many codes per revolution are generated by your encoder. - * - * @param codesPerRev The number of counts per revolution in 1X mode. - */ - public void configEncoderCodesPerRev(int codesPerRev) { - byte[] data = new byte[8]; - - int dataSize = packINT16(data, (short) codesPerRev); - sendMessage(CANJNI.LM_API_CFG_ENC_LINES, data, dataSize); - - m_encoderCodesPerRev = (short) codesPerRev; - m_encoderCodesPerRevVerified = false; - } - - /** - * Configure the number of turns on the potentiometer. - * - *

There is no special support for continuous turn potentiometers. Only integer numbers of - * turns are supported. - * - * @param turns The number of turns of the potentiometer - */ - public void configPotentiometerTurns(int turns) { - byte[] data = new byte[8]; - - int dataSize = packINT16(data, (short) turns); - sendMessage(CANJNI.LM_API_CFG_POT_TURNS, data, dataSize); - - m_potentiometerTurns = (short) turns; - m_potentiometerTurnsVerified = false; - } - - /** - * Configure Soft Position Limits when in Position Controller mode.
- * - *

When controlling position, you can add additional limits on top of the limit switch inputs - * that are based on the position feedback. If the position limit is reached or the switch is - * opened, that direction will be disabled. - * - * @param forwardLimitPosition The position that, if exceeded, will disable the forward - * direction. - * @param reverseLimitPosition The position that, if exceeded, will disable the reverse - * direction. - */ - public void configSoftPositionLimits(double forwardLimitPosition, double reverseLimitPosition) { - configLimitMode(LimitMode.SoftPositionLimits); - configForwardLimit(forwardLimitPosition); - configReverseLimit(reverseLimitPosition); - } - - /** - * Disable Soft Position Limits if previously enabled.
- * - *

Soft Position Limits are disabled by default. - */ - public void disableSoftPositionLimits() { - configLimitMode(LimitMode.SwitchInputsOnly); - } - - /** - * Set the limit mode for position control mode.
- * - *

Use {@link #configSoftPositionLimits(double, double)} or {@link - * #disableSoftPositionLimits()} to set this automatically. - * - * @param mode The {@link LimitMode} to use to limit the rotation of the device. - * @see LimitMode#SwitchInputsOnly - * @see LimitMode#SoftPositionLimits - */ - public void configLimitMode(LimitMode mode) { - sendMessage(CANJNI.LM_API_CFG_LIMIT_MODE, new byte[]{mode.value}, 1); - } - - /** - * Set the position that, if exceeded, will disable the forward direction. - * - *

Use {@link #configSoftPositionLimits(double, double)} to set this and the {@link LimitMode} - * automatically. - * - * @param forwardLimitPosition The position that, if exceeded, will disable the forward - * direction. - */ - public void configForwardLimit(double forwardLimitPosition) { - byte[] data = new byte[8]; - - int dataSize = packFXP16_16(data, forwardLimitPosition); - data[dataSize++] = 1; - sendMessage(CANJNI.LM_API_CFG_LIMIT_FWD, data, dataSize); - - m_forwardLimit = forwardLimitPosition; - m_forwardLimitVerified = false; - } - - /** - * Set the position that, if exceeded, will disable the reverse direction. - * - *

Use {@link #configSoftPositionLimits(double, double)} to set this and the {@link LimitMode} - * automatically. - * - * @param reverseLimitPosition The position that, if exceeded, will disable the reverse - * direction. - */ - public void configReverseLimit(double reverseLimitPosition) { - byte[] data = new byte[8]; - - int dataSize = packFXP16_16(data, reverseLimitPosition); - data[dataSize++] = 1; - sendMessage(CANJNI.LM_API_CFG_LIMIT_REV, data, dataSize); - - m_reverseLimit = reverseLimitPosition; - m_reverseLimitVerified = false; - } - - /** - * Configure the maximum voltage that the Jaguar will ever output. - * - *

This can be used to limit the maximum output voltage in all modes so that motors which - * cannot withstand full bus voltage can be used safely. - * - * @param voltage The maximum voltage output by the Jaguar. - */ - public void configMaxOutputVoltage(double voltage) { - byte[] data = new byte[8]; - - int dataSize = packFXP8_8(data, voltage); - sendMessage(CANJNI.LM_API_CFG_MAX_VOUT, data, dataSize); - - m_maxOutputVoltage = voltage; - m_maxOutputVoltageVerified = false; - } - - /** - * Configure how long the Jaguar waits in the case of a fault before resuming operation. - * - *

Faults include over temerature, over current, and bus under voltage. The default is 3.0 - * seconds, but can be reduced to as low as 0.5 seconds. - * - * @param faultTime The time to wait before resuming operation, in seconds. - */ - public void configFaultTime(float faultTime) { - byte[] data = new byte[8]; - - if (faultTime < 0.5f) { - faultTime = 0.5f; - } else if (faultTime > 3.0f) { - faultTime = 3.0f; - } - - int dataSize = packINT16(data, (short) (faultTime * 1000.0)); - sendMessage(CANJNI.LM_API_CFG_FAULT_TIME, data, dataSize); - - m_faultTime = faultTime; - m_faultTimeVerified = false; - } - - byte m_deviceNumber; - double m_value = 0.0f; - - // Parameters/configuration - JaguarControlMode m_controlMode; - int m_speedReference = CANJNI.LM_REF_NONE; - int m_positionReference = CANJNI.LM_REF_NONE; - @SuppressWarnings("MemberName") - double m_p = 0.0; - @SuppressWarnings("MemberName") - double m_i = 0.0; - @SuppressWarnings("MemberName") - double m_d = 0.0; - NeutralMode m_neutralMode = NeutralMode.Jumper; - short m_encoderCodesPerRev = 0; - short m_potentiometerTurns = 0; - final LimitMode m_limitMode = LimitMode.SwitchInputsOnly; - double m_forwardLimit = 0.0; - double m_reverseLimit = 0.0; - double m_maxOutputVoltage = kApproxBusVoltage; - final double m_voltageRampRate = 0.0; - float m_faultTime = 0.0f; - - // Which parameters have been verified since they were last set? - boolean m_controlModeVerified = true; - boolean m_speedRefVerified = true; - boolean m_posRefVerified = true; - @SuppressWarnings("MemberName") - boolean m_pVerified = true; - @SuppressWarnings("MemberName") - boolean m_iVerified = true; - @SuppressWarnings("MemberName") - boolean m_dVerified = true; - boolean m_neutralModeVerified = true; - boolean m_encoderCodesPerRevVerified = true; - boolean m_potentiometerTurnsVerified = true; - boolean m_forwardLimitVerified = true; - boolean m_reverseLimitVerified = true; - boolean m_limitModeVerified = true; - boolean m_maxOutputVoltageVerified = true; - boolean m_voltageRampRateVerified = true; - boolean m_faultTimeVerified = true; - - // Status data - double m_busVoltage = 0.0f; - double m_outputVoltage = 0.0f; - double m_outputCurrent = 0.0f; - double m_temperature = 0.0f; - double m_position = 0.0; - double m_speed = 0.0; - byte m_limits = (byte) 0; - short m_faults = (short) 0; - int m_firmwareVersion = 0; - byte m_hardwareVersion = (byte) 0; - - // Which periodic status messages have we received at least once? - boolean m_receivedStatusMessage0 = false; - boolean m_receivedStatusMessage1 = false; - boolean m_receivedStatusMessage2 = false; - - static final int kReceiveStatusAttempts = 50; - - boolean m_controlEnabled = true; - boolean m_stopped = false; - - static void sendMessageHelper(int messageID, byte[] data, int dataSize, int period) - throws CANMessageNotFoundException { - final int[] kTrustedMessages = - {CANJNI.LM_API_VOLT_T_EN, CANJNI.LM_API_VOLT_T_SET, CANJNI.LM_API_SPD_T_EN, - CANJNI.LM_API_SPD_T_SET, CANJNI.LM_API_VCOMP_T_EN, CANJNI.LM_API_VCOMP_T_SET, - CANJNI.LM_API_POS_T_EN, CANJNI.LM_API_POS_T_SET, CANJNI.LM_API_ICTRL_T_EN, - CANJNI.LM_API_ICTRL_T_SET}; - - for (byte i = 0; i < kTrustedMessages.length; i++) { - if ((kFullMessageIDMask & messageID) == kTrustedMessages[i]) { - // Make sure the data will still fit after adjusting for the token. - if (dataSize > kMaxMessageDataSize - 2) { - throw new RuntimeException("CAN message has too much data."); - } - - ByteBuffer trustedBuffer = ByteBuffer.allocateDirect(dataSize + 2); - trustedBuffer.put(0, (byte) 0); - trustedBuffer.put(1, (byte) 0); - - for (byte j = 0; j < dataSize; j++) { - trustedBuffer.put(j + 2, data[j]); - } - - CANJNI.FRCNetCommCANSessionMuxSendMessage(messageID, trustedBuffer, period); - - return; - } - } - - // Use a null pointer for the data buffer if the given array is null - ByteBuffer buffer; - if (data != null) { - buffer = ByteBuffer.allocateDirect(dataSize); - for (byte i = 0; i < dataSize; i++) { - buffer.put(i, data[i]); - } - } else { - buffer = null; - } - - CANJNI.FRCNetCommCANSessionMuxSendMessage(messageID, buffer, period); - } - - /** - * Send a message to the Jaguar. - * - * @param messageID The messageID to be used on the CAN bus (device number is added internally) - * @param data The up to 8 bytes of data to be sent with the message - * @param dataSize Specify how much of the data in "data" to send - * @param period If positive, tell Network Communications to send the message every "period" - * milliseconds. - */ - protected void sendMessage(int messageID, byte[] data, int dataSize, int period) { - sendMessageHelper(messageID | m_deviceNumber, data, dataSize, period); - } - - /** - * Send a message to the Jaguar, non-periodically. - * - * @param messageID The messageID to be used on the CAN bus (device number is added internally) - * @param data The up to 8 bytes of data to be sent with the message - * @param dataSize Specify how much of the data in "data" to send - */ - protected void sendMessage(int messageID, byte[] data, int dataSize) { - sendMessage(messageID, data, dataSize, CANJNI.CAN_SEND_PERIOD_NO_REPEAT); - } - - /** - * Request a message from the Jaguar, but don't wait for it to arrive. - * - * @param messageID The message to request - * @param period If positive, tell Network Communications to request the message every "period" - * milliseconds. - */ - protected void requestMessage(int messageID, int period) { - sendMessageHelper(messageID | m_deviceNumber, null, 0, period); - } - - /** - * Request a message from the Jaguar, but don't wait for it to arrive. - * - * @param messageID The message to request - */ - protected void requestMessage(int messageID) { - requestMessage(messageID, CANJNI.CAN_SEND_PERIOD_NO_REPEAT); - } - - /** - * Get a previously requested message. - * - *

Jaguar always generates a message with the same message ID when replying. - * - * @param messageID The messageID to read from the CAN bus (device number is added internally) - * @param data The up to 8 bytes of data that was received with the message - * @throws CANMessageNotFoundException if there's not new message available - */ - protected void getMessage(int messageID, int messageMask, byte[] data) - throws CANMessageNotFoundException { - messageID |= m_deviceNumber; - messageID &= CANJNI.CAN_MSGID_FULL_M; - - ByteBuffer targetedMessageID = ByteBuffer.allocateDirect(4); - targetedMessageID.order(ByteOrder.LITTLE_ENDIAN); - targetedMessageID.asIntBuffer().put(0, messageID); - - ByteBuffer timeStamp = ByteBuffer.allocateDirect(4); - - // Get the data. - ByteBuffer dataBuffer = - CANJNI.FRCNetCommCANSessionMuxReceiveMessage(targetedMessageID.asIntBuffer(), - messageMask, timeStamp); - - if (data != null) { - for (int i = 0; i < dataBuffer.capacity(); i++) { - data[i] = dataBuffer.get(i); - } - } - } - - /** - * Enables periodic status updates from the Jaguar. - */ - protected void setupPeriodicStatus() { - byte[] data = new byte[8]; - int dataSize; - - // Message 0 returns bus voltage, output voltage, output current, and - // temperature. - final byte[] kMessage0Data = - new byte[]{CANJNI.LM_PSTAT_VOLTBUS_B0, CANJNI.LM_PSTAT_VOLTBUS_B1, - CANJNI.LM_PSTAT_VOLTOUT_B0, CANJNI.LM_PSTAT_VOLTOUT_B1, CANJNI.LM_PSTAT_CURRENT_B0, - CANJNI.LM_PSTAT_CURRENT_B1, CANJNI.LM_PSTAT_TEMP_B0, CANJNI.LM_PSTAT_TEMP_B1}; - - // Message 1 returns position and speed - final byte[] kMessage1Data = - new byte[]{CANJNI.LM_PSTAT_POS_B0, CANJNI.LM_PSTAT_POS_B1, CANJNI.LM_PSTAT_POS_B2, - CANJNI.LM_PSTAT_POS_B3, CANJNI.LM_PSTAT_SPD_B0, CANJNI.LM_PSTAT_SPD_B1, - CANJNI.LM_PSTAT_SPD_B2, CANJNI.LM_PSTAT_SPD_B3}; - - // Message 2 returns limits and faults - final byte[] kMessage2Data = - new byte[]{CANJNI.LM_PSTAT_LIMIT_CLR, CANJNI.LM_PSTAT_FAULT, CANJNI.LM_PSTAT_END, - (byte) 0, (byte) 0, (byte) 0, (byte) 0, (byte) 0,}; - - dataSize = packINT16(data, (short) (kSendMessagePeriod)); - sendMessage(CANJNI.LM_API_PSTAT_PER_EN_S0, data, dataSize); - sendMessage(CANJNI.LM_API_PSTAT_PER_EN_S1, data, dataSize); - sendMessage(CANJNI.LM_API_PSTAT_PER_EN_S2, data, dataSize); - - dataSize = 8; - sendMessage(CANJNI.LM_API_PSTAT_CFG_S0, kMessage0Data, dataSize); - sendMessage(CANJNI.LM_API_PSTAT_CFG_S1, kMessage1Data, dataSize); - sendMessage(CANJNI.LM_API_PSTAT_CFG_S2, kMessage2Data, dataSize); - } - - /** - * Check for new periodic status updates and unpack them into local variables. - */ - protected void updatePeriodicStatus() { - byte[] data = new byte[8]; - - // Check if a new bus voltage/output voltage/current/temperature message - // has arrived and unpack the values into the cached member variables - try { - getMessage(CANJNI.LM_API_PSTAT_DATA_S0, CANJNI.CAN_MSGID_FULL_M, data); - - m_busVoltage = unpackFXP8_8(new byte[]{data[0], data[1]}); - m_outputVoltage = unpackPercentage(new byte[]{data[2], data[3]}) * m_busVoltage; - m_outputCurrent = unpackFXP8_8(new byte[]{data[4], data[5]}); - m_temperature = unpackFXP8_8(new byte[]{data[6], data[7]}); - - m_receivedStatusMessage0 = true; - } catch (CANMessageNotFoundException ex) { - // no-op - } - - // Check if a new position/speed message has arrived and do the same - try { - getMessage(CANJNI.LM_API_PSTAT_DATA_S1, CANJNI.CAN_MSGID_FULL_M, data); - - m_position = unpackFXP16_16(new byte[]{data[0], data[1], data[2], data[3]}); - m_speed = unpackFXP16_16(new byte[]{data[4], data[5], data[6], data[7]}); - - m_receivedStatusMessage1 = true; - } catch (CANMessageNotFoundException ex) { - // no-op - } - - // Check if a new limits/faults message has arrived and do the same - try { - getMessage(CANJNI.LM_API_PSTAT_DATA_S2, CANJNI.CAN_MSGID_FULL_M, data); - m_limits = data[0]; - m_faults = data[1]; - - m_receivedStatusMessage2 = true; - } catch (CANMessageNotFoundException ex) { - // no-op - } - } - - /** - * Update all the motors that have pending sets in the syncGroup. - * - * @param syncGroup A bitmask of groups to generate synchronous output. - */ - public static void updateSyncGroup(byte syncGroup) { - byte[] data = new byte[8]; - - data[0] = syncGroup; - - sendMessageHelper(CANJNI.CAN_MSGID_API_SYNC, data, 1, CANJNI.CAN_SEND_PERIOD_NO_REPEAT); - } - - /* we are on ARM-LE now, not Freescale so no need to swap */ - @SuppressWarnings("ParameterName") - private static void swap16(int x, byte[] buffer) { - buffer[0] = (byte) (x & 0xff); - buffer[1] = (byte) ((x >> 8) & 0xff); - } - - @SuppressWarnings("ParameterName") - private static void swap32(int x, byte[] buffer) { - buffer[0] = (byte) (x & 0xff); - buffer[1] = (byte) ((x >> 8) & 0xff); - buffer[2] = (byte) ((x >> 16) & 0xff); - buffer[3] = (byte) ((x >> 24) & 0xff); - } - - private static byte packPercentage(byte[] buffer, double value) { - if (value < -1.0) { - value = -1.0; - } - if (value > 1.0) { - value = 1.0; - } - short intValue = (short) (value * 32767.0); - swap16(intValue, buffer); - return 2; - } - - private static byte packFXP8_8(byte[] buffer, double value) { - short intValue = (short) (value * 256.0); - swap16(intValue, buffer); - return 2; - } - - private static byte packFXP16_16(byte[] buffer, double value) { - int intValue = (int) (value * 65536.0); - swap32(intValue, buffer); - return 4; - } - - private static byte packINT16(byte[] buffer, short value) { - swap16(value, buffer); - return 2; - } - - private static byte packINT32(byte[] buffer, int value) { - swap32(value, buffer); - return 4; - } - - /** - * Unpack 16-bit data from a buffer in little-endian byte order. - * - * @param buffer The buffer to unpack from - * @param offset The offset into he buffer to unpack - * @return The data that was unpacked - */ - private static short unpack16(byte[] buffer, int offset) { - return (short) ((buffer[offset] & 0xFF) | (short) ((buffer[offset + 1] << 8)) & 0xFF00); - } - - /** - * Unpack 32-bit data from a buffer in little-endian byte order. - * - * @param buffer The buffer to unpack from - * @param offset The offset into he buffer to unpack - * @return The data that was unpacked - */ - private static int unpack32(byte[] buffer, int offset) { - return (buffer[offset] & 0xFF) | ((buffer[offset + 1] << 8) & 0xFF00) - | ((buffer[offset + 2] << 16) & 0xFF0000) | ((buffer[offset + 3] << 24) & 0xFF000000); - } - - private static double unpackPercentage(byte[] buffer) { - return unpack16(buffer, 0) / 32767.0; - } - - private static double unpackFXP8_8(byte[] buffer) { - return unpack16(buffer, 0) / 256.0; - } - - private static double unpackFXP16_16(byte[] buffer) { - return unpack32(buffer, 0) / 65536.0; - } - - private static short unpackINT16(byte[] buffer) { - return unpack16(buffer, 0); - } - - private static int unpackINT32(byte[] buffer) { - return unpack32(buffer, 0); - } - - /* Compare floats for equality as fixed point numbers */ - @SuppressWarnings({"ParameterName", "MethodName", "SummaryJavadoc"}) - public static boolean FXP8_EQ(double a, double b) { - return (int) (a * 256.0) == (int) (b * 256.0); - } - - /* Compare floats for equality as fixed point numbers */ - @SuppressWarnings({"ParameterName", "MethodName", "SummaryJavadoc"}) - public static boolean FXP16_EQ(double a, double b) { - return (int) (a * 65536.0) == (int) (b * 65536.0); - } - - @Override - public void setExpiration(double timeout) { - m_safetyHelper.setExpiration(timeout); - } - - @Override - public double getExpiration() { - return m_safetyHelper.getExpiration(); - } - - @Override - public boolean isAlive() { - return m_safetyHelper.isAlive(); - } - - @Override - public boolean isSafetyEnabled() { - return m_safetyHelper.isSafetyEnabled(); - } - - @Override - public void setSafetyEnabled(boolean enabled) { - m_safetyHelper.setSafetyEnabled(enabled); - } - - @Override - public String getDescription() { - return "CANJaguar ID " + m_deviceNumber; - } - - public int getDeviceID() { - return (int) m_deviceNumber; - } - - /** - * Common interface for stopping a motor. - * - * @deprecated Use disableControl instead. - */ - @Override - @Deprecated - public void stopMotor() { - disableControl(); - m_stopped = true; - } - - /* - * Live Window code, only does anything if live window is activated. - */ - - private ITable m_table = null; - private ITableListener m_tableListener = null; - - @Override - public void initTable(ITable subtable) { - m_table = subtable; - updateTable(); - } - - - @Override - public void updateTable() { - CANSpeedController.super.updateTable(); - } - - - @Override - public ITable getTable() { - return m_table; - } - - @Override - public void startLiveWindowMode() { - set(0); // Stop for safety - m_tableListener = createTableListener(); - m_table.addTableListener(m_tableListener, true); - } - - - @Override - public void stopLiveWindowMode() { - set(0); // Stop for safety - // TODO: See if this is still broken - m_table.removeTableListener(m_tableListener); - } -} diff --git a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/CANSpeedController.java b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/CANSpeedController.java index f448cab16c..f48a28db92 100644 --- a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/CANSpeedController.java +++ b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/CANSpeedController.java @@ -14,7 +14,7 @@ import edu.wpi.first.wpilibj.tables.ITableListener; public interface CANSpeedController extends SpeedController, PIDInterface, LiveWindowSendable { /** * Mode determines how the motor is controlled, used internally. This is meant to be subclassed by - * enums (see {@link CANTalon.TalonControlMode CANTalon.TalonControlMode}). + * enums * *

Note that the Jaguar does not support follower mode. */ @@ -146,11 +146,9 @@ public interface CANSpeedController extends SpeedController, PIDInterface, LiveW ITable table = getTable(); if (table != null) { table.putString("~TYPE~", SMART_DASHBOARD_TYPE); - table.putString("Type", getClass().getSimpleName()); // "CANTalon", "CANJaguar" + table.putString("Type", getClass().getSimpleName()); table.putNumber("Mode", getControlMode().getValue()); if (getControlMode().isPID()) { - // CANJaguar throws an exception if you try to get its PID constants - // when it's not in a PID-compatible mode table.putNumber("p", getP()); table.putNumber("i", getI()); table.putNumber("d", getD()); diff --git a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/Jaguar.java b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/Jaguar.java index bc7d35c44e..c5c0493625 100644 --- a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/Jaguar.java +++ b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/Jaguar.java @@ -13,8 +13,6 @@ import edu.wpi.first.wpilibj.livewindow.LiveWindow; /** * Texas Instruments / Vex Robotics Jaguar Speed Controller as a PWM device. - * - * @see CANJaguar CANJaguar for CAN control */ public class Jaguar extends PWMSpeedController { diff --git a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/can/CANJaguarVersionException.java b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/can/CANJaguarVersionException.java deleted file mode 100644 index 87c404eaec..0000000000 --- a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/can/CANJaguarVersionException.java +++ /dev/null @@ -1,42 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) FIRST 2008-2016. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package edu.wpi.first.wpilibj.can; - -/** - * Exception indicating that the CAN driver layer has not been initialized. This happens when an - * entry-point is called before a CAN driver plugin has been installed. - */ -public class CANJaguarVersionException extends RuntimeException { - - public static final int kMinLegalFIRSTFirmwareVersion = 101; - // 3330 was the first shipping RDK firmware version for the Jaguar - public static final int kMinRDKFirmwareVersion = 3330; - - public CANJaguarVersionException(int deviceNumber, int fwVersion) { - super(getString(deviceNumber, fwVersion)); - System.out.println("fwVersion[" + deviceNumber + "]: " + fwVersion); - } - - static String getString(int deviceNumber, int fwVersion) { - String msg; - if (fwVersion < kMinRDKFirmwareVersion) { - msg = - "Jaguar " + deviceNumber - + " firmware is too old. It must be updated to at least version " - + Integer.toString(kMinLegalFIRSTFirmwareVersion) - + " of the FIRST approved firmware!"; - } else { - msg = - "Jaguar " + deviceNumber - + " firmware is not FIRST approved. It must be updated to at least version " - + Integer.toString(kMinLegalFIRSTFirmwareVersion) - + " of the FIRST approved firmware!"; - } - return msg; - } -} diff --git a/wpilibj/src/sim/java/edu/wpi/first/wpilibj/simulation/SimSpeedController.java b/wpilibj/src/sim/java/edu/wpi/first/wpilibj/simulation/SimSpeedController.java index e78c8d7f81..be929bf185 100644 --- a/wpilibj/src/sim/java/edu/wpi/first/wpilibj/simulation/SimSpeedController.java +++ b/wpilibj/src/sim/java/edu/wpi/first/wpilibj/simulation/SimSpeedController.java @@ -26,22 +26,6 @@ public class SimSpeedController { m_pub = MainNode.advertise(topic, Msgs.Float64()); } - /** - * Set the PWM value. - * - *

he PWM value is set using a range of -1.0 to 1.0, appropriately scaling the value for the - * FPGA. - * - * @param speed The speed to set. Value should be between -1.0 and 1.0. - * @param syncGroup The update group to add this Set() to, pending UpdateSyncGroup(). If 0, - * update immediately. - * @deprecated For compatibility with CANJaguar - */ - @Deprecated - public void set(double speed, byte syncGroup) { - set(speed); - } - /** * Set the PWM value. * diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/AbstractCANTest.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/AbstractCANTest.java deleted file mode 100644 index 9437789113..0000000000 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/AbstractCANTest.java +++ /dev/null @@ -1,104 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) FIRST 2008-2016. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package edu.wpi.first.wpilibj.can; - -import org.junit.After; -import org.junit.Before; -import org.junit.rules.TestWatcher; -import org.junit.runner.Description; - -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.fixtures.CANMotorEncoderFixture; -import edu.wpi.first.wpilibj.test.AbstractComsSetup; -import edu.wpi.first.wpilibj.test.TestBench; - -/** - * Provides a base implementation for CAN Tests that forces a test of a particular mode to provide - * tests of a certain type. This also allows for a standardized base setup for each test. - */ -public abstract class AbstractCANTest extends AbstractComsSetup { - public static final double kMotorStopTime = 2; - public static final double kMotorTime = 3; - public static final double kMotorTimeSettling = 10; - public static final double kPotentiometerSettlingTime = 10.0; - public static final double kEncoderSettlingTime = 0.50; - public static final double kEncoderSpeedTolerance = 20.0; - public static final double kLimitSettlingTime = 20.0; // timeout in seconds - public static final double kStartupTime = 0.50; - public static final double kEncoderPositionTolerance = .75; - public static final double kPotentiometerPositionTolerance = 10.0 / 360.0; // +/-10 - // degrees - public static final double kCurrentTolerance = 0.1; - - - /** - * Stores the status value for the previous test. This is set immediately after a failure or - * success and before the me is torn down. - */ - private String m_status = ""; - - /** - * Extends the default test watcher in order to provide more information about a tests failure at - * runtime. - */ - public class CANTestWatcher extends DefaultTestWatcher { - @Override - protected void failed(Throwable exception, Description description) { - super.failed(exception, description, m_status); - } - } - - @Override - protected TestWatcher getOverridenTestWatcher() { - return new CANTestWatcher(); - } - - /** - * The Fixture under test. - */ - private CANMotorEncoderFixture m_me; - - /** - * Retrieves the CANMotorEncoderFixture. - * - * @return the CANMotorEncoderFixture for this test. - */ - public CANMotorEncoderFixture getME() { - return m_me; - } - - /** - * This runs BEFORE the setup of the inherited class. - */ - @Before - public final void preSetup() { - m_status = ""; - m_me = TestBench.getInstance().getCanJaguarPair(); - m_me.setup(); - m_me.getMotor().setSafetyEnabled(false); - } - - @After - public final void tearDown() throws Exception { - try { - // Stores the status data before tearing it down. - // If the test fails unexpectedly then this could cause an exception. - m_status = m_me.printStatus(); - } finally { - m_me.teardown(); - } - m_me = null; - } - - protected void setCANJaguar(double seconds, double value) { - for (int i = 0; i < 50; i++) { - getME().getMotor().set(value); - Timer.delay(seconds / 50.0); - } - } -} diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/CANCurrentQuadEncoderModeTest.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/CANCurrentQuadEncoderModeTest.java deleted file mode 100644 index f2ca7218ce..0000000000 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/CANCurrentQuadEncoderModeTest.java +++ /dev/null @@ -1,108 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) FIRST 2008-2016. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package edu.wpi.first.wpilibj.can; - -import org.junit.Before; -import org.junit.Ignore; -import org.junit.Test; - -import java.util.logging.Logger; - -import edu.wpi.first.wpilibj.CANJaguar; -import edu.wpi.first.wpilibj.Timer; - -import static org.junit.Assert.assertEquals; - -/** - * Tests the CAN Motor Controller in Current Quad Encoder mode. - */ -public class CANCurrentQuadEncoderModeTest extends AbstractCANTest { - private static Logger logger = Logger.getLogger(CANCurrentQuadEncoderModeTest.class.getName()); - private static final double kStoppedValue = 0; - private static final double kRunningValue = 3.0; - - /* - * (non-Javadoc) - * - * @see edu.wpi.first.wpilibj.can.AbstractCANTest#stopMotor() - */ - protected void stopMotor() { - getME().getMotor().set(kStoppedValue); - } - - /* - * (non-Javadoc) - * - * @see edu.wpi.first.wpilibj.can.AbstractCANTest#runMotorForward() - */ - protected void runMotorForward() { - getME().getMotor().set(kRunningValue); - } - - /* - * (non-Javadoc) - * - * @see edu.wpi.first.wpilibj.can.AbstractCANTest#runMotorReverse() - */ - protected void runMotorReverse() { - getME().getMotor().set(-kRunningValue); - } - - @Override - protected Logger getClassLogger() { - return logger; - } - - @Before - public void setUp() throws Exception { - getME().getMotor().setCurrentMode(CANJaguar.kQuadEncoder, 360, 10.0, 4.0, 1.0); - getME().getMotor().enableControl(); - getME().getMotor().set(0.0f); - /* The motor might still have momentum from the previous test. */ - Timer.delay(kStartupTime); - } - - - @Ignore - @Test - public void testDriveToCurrentPositive() { - double setpoint = 1.6f; - - /* It should get to the setpoint within 10 seconds */ - for (int i = 0; i < 10; i++) { - setCANJaguar(1.0, setpoint); - - if (Math.abs(getME().getMotor().getOutputCurrent() - setpoint) <= kCurrentTolerance) { - break; - } - } - - assertEquals("The desired output current was not reached", setpoint, getME().getMotor() - .getOutputCurrent(), kCurrentTolerance); - } - - @Ignore - @Test - public void testDriveToCurrentNegative() { - double setpoint = -1.6f; - - /* It should get to the setpoint within 10 seconds */ - for (int i = 0; i < 10; i++) { - setCANJaguar(1.0, setpoint); - - if (Math.abs(getME().getMotor().getOutputCurrent() - Math.abs(setpoint)) - <= kCurrentTolerance) { - break; - } - } - - assertEquals("The desired output current was not reached", Math.abs(setpoint), getME() - .getMotor().getOutputCurrent(), kCurrentTolerance); - } - -} diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/CANDefaultTest.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/CANDefaultTest.java deleted file mode 100644 index a7743d1aa8..0000000000 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/CANDefaultTest.java +++ /dev/null @@ -1,234 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) FIRST 2008-2016. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package edu.wpi.first.wpilibj.can; - -import com.googlecode.junittoolbox.PollingWait; -import com.googlecode.junittoolbox.RunnableAssert; - -import org.junit.Before; -import org.junit.Test; - -import java.util.concurrent.TimeUnit; -import java.util.logging.Logger; - -import edu.wpi.first.wpilibj.CANJaguar; -import edu.wpi.first.wpilibj.Timer; - -import static org.hamcrest.Matchers.greaterThan; -import static org.hamcrest.Matchers.is; -import static org.junit.Assert.assertEquals; -import static org.junit.Assert.assertFalse; -import static org.junit.Assert.assertThat; -import static org.junit.Assert.assertTrue; - -/** - * The default test set to run against the CAN Motor Controllers. - */ -public class CANDefaultTest extends AbstractCANTest { - private static final Logger logger = Logger.getLogger(CANDefaultTest.class.getName()); - private final PollingWait m_wait = new PollingWait().timeoutAfter(65, TimeUnit.MILLISECONDS) - .pollEvery(10, TimeUnit.MILLISECONDS); - - private static final double kSpikeTime = .5; - - @Override - protected Logger getClassLogger() { - return logger; - } - - @Before - public void setUp() throws Exception { - getME().getMotor().enableControl(); - getME().getMotor().set(0.0f); - /* The motor might still have momentum from the previous test. */ - Timer.delay(kStartupTime / 2); - } - - @Test - public void testDefaultGet() { - m_wait.until(new RunnableAssert("Waiting for CAN Jaguar get to return 0") { - @Override - public void run() { - assertEquals("CAN Jaguar did not initialize stopped", 0.0, getME().getMotor().get(), - .01f); - } - }); - } - - @Test - public void testDefaultBusVoltage() { - m_wait.until(new RunnableAssert("Waiting for default bus voltage to be correct") { - @Override - public void run() { - assertEquals("CAN Jaguar did not start at 14 volts", 14.0f, getME().getMotor() - .getBusVoltage(), 2.0f); - } - }); - } - - @Test - public void testDefaultOutputVoltage() { - m_wait.until(new RunnableAssert("Waiting for output voltage to be correct") { - @Override - public void run() { - assertEquals("CAN Jaguar did not start with an output voltage of 0", 0.0f, getME() - .getMotor().getOutputVoltage(), 0.3f); - } - }); - } - - @Test - public void testDefaultOutputCurrent() { - m_wait.until(new RunnableAssert("Waiting for output current to be correct") { - @Override - public void run() { - assertEquals("CAN Jaguar did not start with an output current of 0", 0.0f, getME() - .getMotor().getOutputCurrent(), 0.3f); - } - }); - } - - @Test - public void testDefaultTemperature() { - final double room_temp = 18.0f; - m_wait.until(new RunnableAssert("Waiting for temperature to be correct") { - @Override - public void run() { - assertThat( - "CAN Jaguar did not start with an initial temperature greater than " + room_temp, - getME().getMotor().getTemperature(), is(greaterThan(room_temp))); - } - }); - } - - @Test - public void testDefaultForwardLimit() { - getME().getMotor().configLimitMode(CANJaguar.LimitMode.SwitchInputsOnly); - m_wait.until(new RunnableAssert("Waiting for forward limit to not be set") { - @Override - public void run() { - getME().getMotor().set(0); - assertTrue("CAN Jaguar did not start with the Forward Limit Switch Off", getME() - .getMotor() - .getForwardLimitOK()); - } - }); - } - - @Test - public void testDefaultReverseLimit() { - getME().getMotor().configLimitMode(CANJaguar.LimitMode.SwitchInputsOnly); - m_wait.until(new RunnableAssert("Waiting for reverse limit to not be set") { - @Override - public void run() { - getME().getMotor().set(0); - assertTrue("CAN Jaguar did not start with the Reverse Limit Switch Off", getME() - .getMotor() - .getReverseLimitOK()); - } - }); - } - - @Test - public void testDefaultNoFaults() { - m_wait.until(new RunnableAssert("Waiting for no faults") { - @Override - public void run() { - assertEquals("CAN Jaguar initialized with Faults", 0, getME().getMotor().getFaults()); - } - }); - } - - - @Test - public void testFakeLimitSwitchForwards() { - // Given - getME().getMotor().configLimitMode(CANJaguar.LimitMode.SwitchInputsOnly); - getME().getMotor().enableControl(); - - // When - getME().getForwardLimit().set(true); - - // Then - PollingWait wait = - new PollingWait().timeoutAfter((long) kLimitSettlingTime, TimeUnit.SECONDS).pollEvery(1, - TimeUnit.MILLISECONDS); - wait.until(new RunnableAssert("Setting the CANJAguar forward limit switch high") { - @Override - public void run() throws Exception { - getME().getMotor().set(0); - assertFalse( - "Setting the forward limit switch high did not cause the forward limit switch to " - + "trigger", - getME().getMotor().getForwardLimitOK()); - } - }); - } - - - @Test - public void testFakeLimitSwitchReverse() { - // Given - getME().getMotor().configLimitMode(CANJaguar.LimitMode.SwitchInputsOnly); - getME().getMotor().enableControl(); - - // When - getME().getReverseLimit().set(true); - - // Then - PollingWait wait = - new PollingWait().timeoutAfter((long) kLimitSettlingTime, TimeUnit.SECONDS).pollEvery(1, - TimeUnit.MILLISECONDS); - wait.until(new RunnableAssert("Setting the CANJAguar reverse limit switch high") { - @Override - public void run() throws Exception { - getME().getMotor().set(0); - assertFalse( - "Setting the reverse limit switch high did not cause the forward limit switch to " - + "trigger", - getME().getMotor().getReverseLimitOK()); - } - }); - } - - @Test - public void testPositionModeVerifiesOnBrownOut() { - final double setpoint = 1.0; - - // Given - getME().getMotor().setPositionMode(CANJaguar.kQuadEncoder, 360, 10.0, 0.1, 0.0); - getME().getMotor().enableControl(); - setCANJaguar(kMotorTime, 0.0); - - getME().powerOn(); - - // When - /* Turn the spike off and on again */ - - getME().powerOff(); - Timer.delay(kSpikeTime); - getME().powerOn(); - Timer.delay(kSpikeTime); - - PollingWait wait = - new PollingWait().timeoutAfter(15, TimeUnit.SECONDS).pollEvery(1, TimeUnit.MILLISECONDS); - /* - * The jaguar should automatically get set to quad encoder position mode, so - * it should be able to reach a setpoint in a couple seconds. - */ - - wait.until(new RunnableAssert("Waiting for CANJaguar to reach set-point") { - @Override - public void run() throws Exception { - getME().getMotor().set(setpoint); - assertEquals("CANJaguar should have resumed PID control after power cycle", setpoint, - getME().getMotor().getPosition(), kEncoderPositionTolerance); - } - }); - } -} diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/CANJaguarInversionTest.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/CANJaguarInversionTest.java deleted file mode 100644 index a6fc64d4bc..0000000000 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/CANJaguarInversionTest.java +++ /dev/null @@ -1,88 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) FIRST 2016. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package edu.wpi.first.wpilibj.can; - -import org.junit.Test; - -import java.util.logging.Logger; - -import edu.wpi.first.wpilibj.CANJaguar; -import edu.wpi.first.wpilibj.Timer; - -import static org.junit.Assert.assertTrue; - -/** - * Tests the CAN Jaguar inverted. - * - *

Created by Patrick Murphy on 3/30/15. - */ -public class CANJaguarInversionTest extends AbstractCANTest { - private static final Logger logger = Logger.getLogger(CANJaguarInversionTest.class.getName()); - private static final double m_motorVoltage = 2.0; - private static final double m_motorPercent = 0.1; - private static final double m_motorSpeed = 10; - private static final double m_delayTime = 0.75; - private static final double m_speedModeDelayTime = 2.0; - - @Override - protected Logger getClassLogger() { - return logger; - } - - @Test - public void testInvertingVoltageMode() { - getME().getMotor().setVoltageMode(CANJaguar.kQuadEncoder, 360); - inversionTest(m_motorVoltage, m_delayTime); - } - - @Test - public void testInvertingPercentMode() { - getME().getMotor().setPercentMode(CANJaguar.kQuadEncoder, 360); - inversionTest(m_motorPercent, m_delayTime); - } - - @Test - public void testInvertingSpeedMode() { - getME().getMotor().setSpeedMode(CANJaguar.kQuadEncoder, 360, 0.1, 0.003, 0.01); - inversionTest(m_motorSpeed, m_speedModeDelayTime); - } - - /** - * Runs an inversion test To use set the jaguar to the proper mode(PercentVbus, voltage, speed). - * - * @param setPoint the speed/voltage/percent to set the motor to - * @param delayTime the amount of time to delay between starting a motor and checking the encoder - */ - private void inversionTest(double setPoint, double delayTime) { - final CANJaguar jag = getME().getMotor(); - jag.enableControl(); - jag.setInverted(false); - jag.set(setPoint); - Timer.delay(delayTime); - final double initialSpeed = jag.getSpeed(); - jag.set(0.0); - jag.setInverted(true); - jag.set(setPoint); - Timer.delay(delayTime); - jag.set(0.0); - final double finalSpeed = jag.getSpeed(); - assertTrue("Inverting with Positive value does not invert direction", - Math.signum(initialSpeed) != Math.signum(finalSpeed)); - jag.set(-setPoint); - Timer.delay(delayTime); - final double newInitialSpeed = jag.getSpeed(); - jag.set(0.0); - jag.setInverted(false); - jag.set(-setPoint); - Timer.delay(delayTime); - final double newFinalSpeed = jag.getSpeed(); - jag.set(0.0); - assertTrue("Inverting with Negative value does not invert direction", - Math.signum(newInitialSpeed) != Math.signum(newFinalSpeed)); - } -} diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/CANPercentQuadEncoderModeTest.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/CANPercentQuadEncoderModeTest.java deleted file mode 100644 index 0edd24b9cb..0000000000 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/CANPercentQuadEncoderModeTest.java +++ /dev/null @@ -1,397 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) FIRST 2008-2016. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package edu.wpi.first.wpilibj.can; - -import com.googlecode.junittoolbox.PollingWait; -import com.googlecode.junittoolbox.RunnableAssert; - -import org.junit.Before; -import org.junit.Ignore; -import org.junit.Test; - -import java.util.concurrent.TimeUnit; -import java.util.logging.Level; -import java.util.logging.Logger; - -import edu.wpi.first.wpilibj.CANJaguar; -import edu.wpi.first.wpilibj.Timer; - -import static org.hamcrest.Matchers.greaterThan; -import static org.hamcrest.Matchers.is; -import static org.hamcrest.Matchers.lessThan; -import static org.junit.Assert.assertEquals; -import static org.junit.Assert.assertFalse; -import static org.junit.Assert.assertThat; -import static org.junit.Assert.assertTrue; - -/** - * Tests the CAN motor in PercentQuadEncoderMode. - */ -@SuppressWarnings("AbbreviationAsWordInName") -public class CANPercentQuadEncoderModeTest extends AbstractCANTest { - private static final Logger logger = Logger.getLogger(CANPercentQuadEncoderModeTest.class - .getName()); - private static final double kStoppedValue = 0; - private static final double kRunningValue = 0.3; - - /* - * (non-Javadoc) - * - * @see edu.wpi.first.wpilibj.can.AbstractCANTest#stopMotor() - */ - protected void stopMotor() { - getME().getMotor().set(kStoppedValue); - } - - /* - * (non-Javadoc) - * - * @see edu.wpi.first.wpilibj.can.AbstractCANTest#runMotorForward() - */ - protected void runMotorForward() { - getME().getMotor().set(kRunningValue); - } - - /* - * (non-Javadoc) - * - * @see edu.wpi.first.wpilibj.can.AbstractCANTest#runMotorReverse() - */ - protected void runMotorReverse() { - getME().getMotor().set(-kRunningValue); - } - - @Override - protected Logger getClassLogger() { - return logger; - } - - @Before - public void setUp() { - getME().getMotor().setPercentMode(CANJaguar.kQuadEncoder, 360); - getME().getMotor().enableControl(); - getME().getMotor().set(0.0f); - /* The motor might still have momentum from the previous test. */ - Timer.delay(kStartupTime); - } - - @Test - public void testDisableStopsTheMotor() { - // given - getME().getMotor().enableControl(); - setCANJaguar(kMotorTime / 2, 1); - getME().getMotor().disableControl(); - // when - simpleLog(Level.FINER, "The motor should stop running now"); - setCANJaguar(kMotorTime / 2, 1); - final double initialPosition = getME().getMotor().getPosition(); - setCANJaguar(kMotorTime / 2, 1); - - // then - assertEquals("Speed did not go to zero when disabled in percent mode", 0, getME().getMotor() - .getSpeed(), kEncoderSpeedTolerance); - assertEquals(initialPosition, getME().getMotor().getPosition(), 10); - } - - @Test - public void testRotateForward() { - // Given - getME().getMotor().enableControl(); - final double initialPosition = getME().getMotor().getPosition(); - // When - /* Drive the speed controller briefly to move the encoder */ - runMotorForward(); - - // Then - PollingWait wait = - new PollingWait().timeoutAfter((long) kMotorTimeSettling, TimeUnit.SECONDS).pollEvery(1, - TimeUnit.MILLISECONDS); - wait.until(new RunnableAssert("CANJaguar position incrementing") { - @Override - public void run() throws Exception { - runMotorForward(); - assertThat("CANJaguar position should have increased after the motor moved", getME() - .getMotor().getPosition(), is(greaterThan(initialPosition))); - } - }); - - stopMotor(); - } - - @Test - public void testRotateReverse() { - // Given - getME().getMotor().enableControl(); - final double initialPosition = getME().getMotor().getPosition(); - // When - /* Drive the speed controller briefly to move the encoder */ - runMotorReverse(); - - // Then - PollingWait wait = - new PollingWait().timeoutAfter((long) kMotorTimeSettling, TimeUnit.SECONDS).pollEvery(1, - TimeUnit.MILLISECONDS); - wait.until(new RunnableAssert("CANJaguar position decrementing") { - @Override - public void run() throws Exception { - runMotorReverse(); - assertThat("CANJaguar position should have decreased after the motor moved", getME() - .getMotor().getPosition(), is(lessThan(initialPosition))); - } - }); - stopMotor(); - } - - /** - * Test if we can limit the Jaguar to not rotate forwards when the fake limit switch is tripped. - */ - @Test - public void shouldNotRotateForwards_WhenFakeLimitSwitchForwardsIsTripped() { - // Given - getME().getMotor().configLimitMode(CANJaguar.LimitMode.SwitchInputsOnly); - getME().getForwardLimit().set(true); - getME().getReverseLimit().set(false); - getME().getMotor().enableControl(); - - stopMotor(); - Timer.delay(kEncoderSettlingTime); - - PollingWait wait = - new PollingWait().timeoutAfter((long) kLimitSettlingTime, TimeUnit.SECONDS).pollEvery(1, - TimeUnit.MILLISECONDS); - /* Wait until the limits are recognized by the CANJaguar. */ - wait.until(new RunnableAssert( - "Waiting for the forward and reverse limit switches to be in the correct state") { - @Override - public void run() throws Exception { - stopMotor(); - assertFalse("[TEST SETUP] The forward limit switch is not in the correct state", getME() - .getMotor().getForwardLimitOK()); - assertTrue("[TEST SETUP]The reverse limit switch is not in the correct state", getME() - .getMotor().getReverseLimitOK()); - } - }); - - final double initialPosition = getME().getMotor().getPosition(); - - // When - /* - * Drive the speed controller briefly to move the encoder. If the limit - * switch is recognized, it shouldn't actually move. - */ - setCANJaguar(kMotorTime, 1); - stopMotor(); - - // Then - /* The position should be the same, since the limit switch was on. */ - assertEquals("CAN Jaguar should not have moved with the forward limit switch pressed", - initialPosition, getME().getMotor().getPosition(), kEncoderPositionTolerance); - } - - - /** - * Test if we can rotate in reverse when the limit switch. - */ - @Test - public void shouldRotateReverse_WhenFakeLimitSwitchForwardsIsTripped() { - // Given - getME().getMotor().configLimitMode(CANJaguar.LimitMode.SwitchInputsOnly); - getME().getForwardLimit().set(true); - getME().getReverseLimit().set(false); - getME().getMotor().enableControl(); - - stopMotor(); - Timer.delay(kEncoderSettlingTime); - - PollingWait limitWait = - new PollingWait().timeoutAfter((long) kLimitSettlingTime, TimeUnit.SECONDS).pollEvery(1, - TimeUnit.MILLISECONDS); - /* Wait until the limits are recognized by the CANJaguar. */ - limitWait.until(new RunnableAssert( - "Waiting for the forward and reverse limit switches to be in the correct state") { - @Override - public void run() throws Exception { - stopMotor(); - assertFalse("[TEST SETUP] The forward limit switch is not in the correct state", getME() - .getMotor().getForwardLimitOK()); - assertTrue("[TEST SETUP] The reverse limit switch is not in the correct state", getME() - .getMotor().getReverseLimitOK()); - } - }); - - final double initialPosition = getME().getMotor().getPosition(); - - // When - /* - * Drive the speed controller in the other direction. It should actually - * move, since only the forward switch is activated. - */ - setCANJaguar(kMotorTime, -1); - // Then - PollingWait wait = - new PollingWait().timeoutAfter((long) kMotorTimeSettling, TimeUnit.SECONDS).pollEvery(1, - TimeUnit.MILLISECONDS); - wait.until(new RunnableAssert("Waiting for the encoder to update") { - @Override - public void run() throws Exception { - runMotorReverse(); - assertThat("CAN Jaguar should have moved in reverse while the forward limit was on", - getME().getMotor().getPosition(), is(lessThan(initialPosition))); - } - - }); - stopMotor(); - - } - - /** - * Test if we can limit the Jaguar to only moving forwards with a fake limit switch. - */ - @Test - public void shouldNotRotateReverse_WhenFakeLimitSwitchReversesIsTripped() { - // Given - getME().getMotor().configLimitMode(CANJaguar.LimitMode.SwitchInputsOnly); - getME().getForwardLimit().set(false); - getME().getReverseLimit().set(true); - getME().getMotor().enableControl(); - - stopMotor(); - Timer.delay(kEncoderSettlingTime); - - PollingWait wait = - new PollingWait().timeoutAfter((long) kLimitSettlingTime, TimeUnit.SECONDS).pollEvery(1, - TimeUnit.MILLISECONDS); - /* Wait until the limits are recognized by the CANJaguar. */ - wait.until(new RunnableAssert( - "Waiting for the forward and reverse limit switches to be in the correct state") { - @Override - public void run() throws Exception { - stopMotor(); - assertTrue("[TEST SETUP] The forward limit switch is not in the correct state", getME() - .getMotor().getForwardLimitOK()); - assertFalse("[TEST SETUP] The reverse limit switch is not in the correct state", getME() - .getMotor().getReverseLimitOK()); - } - }); - - final double initialPosition = getME().getMotor().getPosition(); - - // When - /* - * Drive the speed controller backwards briefly to move the encoder. If the - * limit switch is recognized, it shouldn't actually move. - */ - setCANJaguar(kMotorTime, -1); - stopMotor(); - - // Then - /* The position should be the same, since the limit switch was on. */ - assertEquals("CAN Jaguar should not have moved with the limit switch pressed", initialPosition, - getME().getMotor().getPosition(), kEncoderPositionTolerance); - } - - /** - * Test if we can limit the Jaguar to only moving forwards with a fake limit - * switch. - */ - @Test - public void shouldRotateForward_WhenFakeLimitSwitchReversesIsTripped() { - // Given - getME().getMotor().configLimitMode(CANJaguar.LimitMode.SwitchInputsOnly); - getME().getForwardLimit().set(false); - getME().getReverseLimit().set(true); - getME().getMotor().enableControl(); - - PollingWait limitWait = - new PollingWait().timeoutAfter((long) kLimitSettlingTime, TimeUnit.SECONDS).pollEvery(1, - TimeUnit.MILLISECONDS); - /* Wait until the limits are recognized by the CANJaguar. */ - limitWait.until(new RunnableAssert( - "Waiting for the forward and reverse limit switches to be in the correct state") { - @Override - public void run() throws Exception { - stopMotor(); - assertTrue("[TEST SETUP] The forward limit switch is not in the correct state", getME() - .getMotor().getForwardLimitOK()); - assertFalse("[TEST SETUP] The reverse limit switch is not in the correct state", getME() - .getMotor().getReverseLimitOK()); - } - }); - - final double initialPosition = getME().getMotor().getPosition(); - - // When - /* - * Drive the speed controller in the other direction. It should actually - * move, since only the reverse switch is activated. - */ - setCANJaguar(kMotorTime, 1); - // Then - /* The position should have increased */ - PollingWait wait = - new PollingWait().timeoutAfter((long) kMotorTimeSettling, TimeUnit.SECONDS).pollEvery(1, - TimeUnit.MILLISECONDS); - wait.until(new RunnableAssert("Waiting for the encoder to update") { - @Override - public void run() throws Exception { - runMotorForward(); - assertThat("CAN Jaguar should have moved forwards while the reverse limit was on", getME() - .getMotor().getPosition(), is(greaterThan(initialPosition))); - } - - }); - stopMotor(); - } - - @Ignore("Encoder is not yet wired to the FPGA") - @Test - public void testRotateForwardEncoderToFPGA() { - getME().getMotor().enableControl(); - final double jagInitialPosition = getME().getMotor().getPosition(); - final double encoderInitialPosition = getME().getEncoder().get(); - getME().getMotor().set(1); - Timer.delay(kMotorStopTime); - getME().getMotor().set(0); - - delayTillInCorrectStateWithMessage(Level.FINE, kEncoderSettlingTime, - "Forward Encodeder settling", new BooleanCheck() { - @Override - public boolean getAsBoolean() { - return Math.abs((getME().getMotor().getPosition() - jagInitialPosition) - - (getME().getEncoder().get() - encoderInitialPosition)) - < kEncoderPositionTolerance; - } - }); - - assertEquals(getME().getMotor().getPosition() - jagInitialPosition, getME().getEncoder().get() - - encoderInitialPosition, kEncoderPositionTolerance); - } - - @Ignore("Encoder is not yet wired to the FPGA") - @Test - public void testRotateReverseEncoderToFPGA() { - getME().getMotor().enableControl(); - final double jagInitialPosition = getME().getMotor().getPosition(); - final double encoderInitialPosition = getME().getEncoder().get(); - getME().getMotor().set(-1); - Timer.delay(kMotorStopTime); - getME().getMotor().set(0); - - delayTillInCorrectStateWithMessage(Level.FINE, kEncoderSettlingTime, - "Forward Encodeder settling", new BooleanCheck() { - @Override - public boolean getAsBoolean() { - return Math.abs((getME().getMotor().getPosition() - jagInitialPosition) - - (getME().getEncoder().get() - encoderInitialPosition)) - < kEncoderPositionTolerance; - } - }); - assertEquals(getME().getMotor().getPosition() - jagInitialPosition, getME().getEncoder().get() - - encoderInitialPosition, kEncoderPositionTolerance); - } -} diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/CANPositionPotentiometerModeTest.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/CANPositionPotentiometerModeTest.java deleted file mode 100644 index 89a7fc16d7..0000000000 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/CANPositionPotentiometerModeTest.java +++ /dev/null @@ -1,171 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) FIRST 2008-2016. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package edu.wpi.first.wpilibj.can; - -import com.googlecode.junittoolbox.PollingWait; -import com.googlecode.junittoolbox.RunnableAssert; - -import org.junit.Before; -import org.junit.Ignore; -import org.junit.Test; - -import java.util.concurrent.TimeUnit; -import java.util.logging.Logger; - -import edu.wpi.first.wpilibj.CANJaguar; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.fixtures.MotorEncoderFixture; - -import static org.hamcrest.Matchers.greaterThan; -import static org.hamcrest.Matchers.is; -import static org.junit.Assert.assertEquals; -import static org.junit.Assert.assertThat; - -/** - * Tests the CAN Motor controller in Potentiometer Mode. - */ -public class CANPositionPotentiometerModeTest extends AbstractCANTest { - private static final Logger logger = Logger.getLogger(CANPositionPotentiometerModeTest.class - .getName()); - - private static final double kStoppedValue = 0; - - private static final int defaultPotAngle = 180; - private static final double maxPotVoltage = 3.0; - - /* - * (non-Javadoc) - * - * @see edu.wpi.first.wpilibj.can.AbstractCANTest#stopMotor() - */ - protected void stopMotor() { - getME().getMotor().set(.5); - } - - /* - * (non-Javadoc) - * - * @see edu.wpi.first.wpilibj.can.AbstractCANTest#runMotorForward() - */ - protected void runMotorForward() { - getME().getMotor().set(1); - } - - /* - * (non-Javadoc) - * - * @see edu.wpi.first.wpilibj.can.AbstractCANTest#runMotorReverse() - */ - protected void runMotorReverse() { - getME().getMotor().set(0); - } - - @Override - protected Logger getClassLogger() { - return logger; - } - - @Before - public void setUp() throws Exception { - getME().getMotor().setPositionMode(CANJaguar.kPotentiometer, 5.0, 0.1, 2.0); - // getME().getMotor().configPotentiometerTurns(rotationRange); - getME().getFakePot().setMaxVoltage(maxPotVoltage); - getME().getFakePot().setVoltage(1.5); - stopMotor(); - getME().getMotor().enableControl(); - /* The motor might still have momentum from the previous test. */ - Timer.delay(kStartupTime); - } - - - /** - * NOTICE: This is using the {@link MotorEncoderFixture#getEncoder()} instead of the one built - * into the CAN Jaguar. - */ - @Ignore("Encoder is not yet wired to the FPGA") - @Test - public void testRotateForward() { - final int initialPosition = getME().getEncoder().get(); - /* Drive the speed controller briefly to move the encoder */ - getME().getMotor().set(kStoppedValue); - Timer.delay(kMotorTimeSettling); - getME().getMotor().set(defaultPotAngle); - - /* The position should have increased */ - assertThat("CAN Jaguar position should have increased after the motor moved", getME() - .getEncoder().get(), is(greaterThan(initialPosition))); - - } - - /** - * NOTICE: This is using the {@link MotorEncoderFixture#getEncoder()} instead of the one built - * into the CAN Jaguar. - */ - @Ignore("Encoder is not yet wired to the FPGA") - @Test - public void testRotateReverse() { - final int initialPosition = getME().getEncoder().get(); - /* Drive the speed controller briefly to move the encoder */ - getME().getMotor().set(kStoppedValue); - Timer.delay(kMotorTimeSettling); - getME().getMotor().set(defaultPotAngle); - - /* The position should have increased */ - assertThat("CAN Jaguar position should have increased after the motor moved", getME() - .getEncoder().get(), is(greaterThan(initialPosition))); - - } - - /** - * Test if we can get a position in potentiometer mode, using an analog output as a fake - * potentiometer. - */ - @Test - public void testFakePotentiometerPosition() { - // TODO When https://github.com/Pragmatists/JUnitParams/issues/5 is resolved - // make this test parameterized - - // Given - PollingWait wait = - new PollingWait().timeoutAfter((long) kPotentiometerSettlingTime, TimeUnit.SECONDS) - .pollEvery(1, TimeUnit.MILLISECONDS); - RunnableAssert assertion = - new RunnableAssert("Waiting for potentiometer position to be correct") { - @Override - public void run() throws Exception { - getME().getMotor().set(0); - assertEquals( - "CAN Jaguar should have returned the potentiometer position set by the analog " - + "output", - getME().getFakePot().getVoltage(), getME().getMotor().getPosition() * 3, - kPotentiometerPositionTolerance * 3); - } - }; - - // When - getME().getFakePot().setVoltage(0.0); - // Then - wait.until(assertion); - - // When - getME().getFakePot().setVoltage(1.0); - // Then - wait.until(assertion); - - // When - getME().getFakePot().setVoltage(2.0); - // Then - wait.until(assertion); - - // When - getME().getFakePot().setVoltage(3.0); - // Then - wait.until(assertion); - } - -} diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/CANPositionQuadEncoderModeTest.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/CANPositionQuadEncoderModeTest.java deleted file mode 100644 index 2e7317b5a0..0000000000 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/CANPositionQuadEncoderModeTest.java +++ /dev/null @@ -1,118 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) FIRST 2008-2016. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package edu.wpi.first.wpilibj.can; - -import org.junit.Before; -import org.junit.Ignore; -import org.junit.Test; - -import java.util.logging.Level; -import java.util.logging.Logger; - -import edu.wpi.first.wpilibj.CANJaguar; -import edu.wpi.first.wpilibj.Timer; - -import static org.junit.Assert.assertEquals; - -/** - * Tests the CAN Motor Encoders in QuadEncoder mode. - */ -public class CANPositionQuadEncoderModeTest extends AbstractCANTest { - private static final Logger logger = Logger.getLogger(CANPositionQuadEncoderModeTest.class - .getName()); - - @Override - protected Logger getClassLogger() { - return logger; - } - - - /* - * (non-Javadoc) - * - * @see edu.wpi.first.wpilibj.can.AbstractCANTest#runMotorForward() - */ - protected void runMotorForward() { - double postion = getME().getMotor().getPosition(); - getME().getMotor().set(postion + 100); - } - - - /* - * (non-Javadoc) - * - * @see edu.wpi.first.wpilibj.can.AbstractCANTest#runMotorReverse() - */ - protected void runMotorReverse() { - double postion = getME().getMotor().getPosition(); - getME().getMotor().set(postion - 100); - } - - - @Before - public void setUp() throws Exception { - getME().getMotor().setPositionMode(CANJaguar.kQuadEncoder, 360, 10.0f, 0.01f, 0.0f); - getME().getMotor().enableControl(0); - /* The motor might still have momentum from the previous test. */ - Timer.delay(kStartupTime); - } - - @Ignore("The encoder initial position is not validated so is sometimes not set properly") - @Test - public void testSetEncoderInitialPositionWithEnable() { - // given - final double encoderValue = 4823; - // when - getME().getMotor().enableControl(encoderValue); - getME().getMotor().disableControl(); - delayTillInCorrectStateWithMessage(Level.FINE, kEncoderSettlingTime, "Encoder value settling", - new BooleanCheck() { - @Override - public boolean getAsBoolean() { - getME().getMotor().set(getME().getMotor().getPosition()); - return Math.abs(getME().getMotor().getPosition() - encoderValue) < 40; - } - }); - // then - assertEquals(encoderValue, getME().getMotor().getPosition(), 40); - } - - /** - * Test if we can set a position and reach that position with PID control on the Jaguar. - */ - @Test - public void testEncoderPositionPIDForward() { - - double setpoint = getME().getMotor().getPosition() + 1.0f; - - /* It should get to the setpoint within 10 seconds */ - getME().getMotor().set(setpoint); - setCANJaguar(kMotorTimeSettling, setpoint); - - assertEquals("CAN Jaguar should have reached setpoint with PID control", setpoint, getME() - .getMotor().getPosition(), kEncoderPositionTolerance); - } - - /** - * Test if we can set a position and reach that position with PID control on the Jaguar. - */ - @Test - public void testEncoderPositionPIDReverse() { - - double setpoint = getME().getMotor().getPosition() - 1.0f; - - /* It should get to the setpoint within 10 seconds */ - getME().getMotor().set(setpoint); - setCANJaguar(kMotorTimeSettling, setpoint); - - assertEquals("CAN Jaguar should have reached setpoint with PID control", setpoint, getME() - .getMotor().getPosition(), kEncoderPositionTolerance); - } - - -} diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/CANSpeedQuadEncoderModeTest.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/CANSpeedQuadEncoderModeTest.java deleted file mode 100644 index 94d7089b7f..0000000000 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/CANSpeedQuadEncoderModeTest.java +++ /dev/null @@ -1,90 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) FIRST 2008-2016. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package edu.wpi.first.wpilibj.can; - -import org.junit.Before; -import org.junit.Test; - -import java.util.logging.Logger; - -import edu.wpi.first.wpilibj.CANJaguar; -import edu.wpi.first.wpilibj.Timer; - -import static org.hamcrest.Matchers.greaterThan; -import static org.hamcrest.Matchers.is; -import static org.hamcrest.Matchers.lessThan; -import static org.junit.Assert.assertEquals; -import static org.junit.Assert.assertThat; - -/** - * Tests the CAN Speed controllers in quad mode. - */ -public class CANSpeedQuadEncoderModeTest extends AbstractCANTest { - private static final Logger logger = Logger.getLogger(CANPercentQuadEncoderModeTest.class - .getName()); - /** - * The stopped value in rev/min. - */ - private static final double kStoppedValue = 0; - /** - * The running value in rev/min. - */ - private static final double kRunningValue = 50; - - @Override - protected Logger getClassLogger() { - return logger; - } - - @Before - public void setUp() throws Exception { - getME().getMotor().setSpeedMode(CANJaguar.kQuadEncoder, 360, 0.1f, 0.003f, 0.01f); - getME().getMotor().enableControl(); - getME().getMotor().set(0.0f); - /* The motor might still have momentum from the previous test. */ - Timer.delay(kStartupTime); - } - - @Test - public void testDefaultSpeed() { - assertEquals("CAN Jaguar did not start with an initial speed of zero", 0.0f, getME() - .getMotor() - .getSpeed(), 0.3f); - } - - - /** - * Test if we can drive the motor forward in Speed mode and get a position back. - */ - @Test - public void testRotateForwardSpeed() { - double speed = 50.0f; - double initialPosition = getME().getMotor().getPosition(); - setCANJaguar(2 * kMotorTime, speed); - assertEquals("The motor did not reach the required speed in speed mode", speed, getME() - .getMotor().getSpeed(), kEncoderSpeedTolerance); - assertThat("The motor did not move forward in speed mode", getME().getMotor().getPosition(), - is(greaterThan(initialPosition))); - } - - /** - * Test if we can drive the motor backwards in Speed mode and get a position back. - */ - @Test - public void testRotateReverseSpeed() { - double speed = -50.0f; - double initialPosition = getME().getMotor().getPosition(); - setCANJaguar(2 * kMotorTime, speed); - assertEquals("The motor did not reach the required speed in speed mode", speed, getME() - .getMotor().getSpeed(), kEncoderSpeedTolerance); - assertThat("The motor did not move in reverse in speed mode", getME().getMotor() - .getPosition(), - is(lessThan(initialPosition))); - } - -} diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/CANTestSuite.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/CANTestSuite.java deleted file mode 100644 index e1bd2a395b..0000000000 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/CANTestSuite.java +++ /dev/null @@ -1,26 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) FIRST 2008-2016. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package edu.wpi.first.wpilibj.can; - -import org.junit.runner.RunWith; -import org.junit.runners.Suite; -import org.junit.runners.Suite.SuiteClasses; - -import edu.wpi.first.wpilibj.test.AbstractTestSuite; - -/** - * All of the tests to cover the CAN Motor Controllers. - */ -@RunWith(Suite.class) -@SuiteClasses({CANCurrentQuadEncoderModeTest.class, CANDefaultTest.class, - CANJaguarInversionTest.class, CANPercentQuadEncoderModeTest.class, - CANPositionPotentiometerModeTest.class, CANPositionQuadEncoderModeTest.class, - CANSpeedQuadEncoderModeTest.class, CANVoltageQuadEncoderModeTest.class}) -public class CANTestSuite extends AbstractTestSuite { - -} diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/CANVoltageQuadEncoderModeTest.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/CANVoltageQuadEncoderModeTest.java deleted file mode 100644 index d871571ce3..0000000000 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/CANVoltageQuadEncoderModeTest.java +++ /dev/null @@ -1,225 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) FIRST 2008-2016. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package edu.wpi.first.wpilibj.can; - -import com.googlecode.junittoolbox.PollingWait; -import com.googlecode.junittoolbox.RunnableAssert; - -import org.junit.Before; -import org.junit.Test; - -import java.util.concurrent.TimeUnit; -import java.util.logging.Logger; - -import edu.wpi.first.wpilibj.CANJaguar; -import edu.wpi.first.wpilibj.Timer; - -import static org.hamcrest.Matchers.greaterThan; -import static org.hamcrest.Matchers.is; -import static org.hamcrest.Matchers.lessThan; -import static org.junit.Assert.assertEquals; -import static org.junit.Assert.assertThat; - -/** - * Tests the CAN motor controllers in voltage mode work correctly. - */ -public class CANVoltageQuadEncoderModeTest extends AbstractCANTest { - private static final Logger logger = Logger.getLogger(CANVoltageQuadEncoderModeTest.class - .getName()); - /** - * The stopped value in volts. - */ - private static final double kStoppedValue = 0; - /** - * The running value in volts. - */ - private static final double kRunningValue = 4; - - private static final double kVoltageTolerance = .25; - - private static final PollingWait kWait = new PollingWait().timeoutAfter( - (long) kMotorTimeSettling, TimeUnit.SECONDS).pollEvery(1, TimeUnit.MILLISECONDS); - - /* - * (non-Javadoc) - * - * @see edu.wpi.first.wpilibj.can.AbstractCANTest#stopMotor() - */ - protected void stopMotor() { - getME().getMotor().set(kStoppedValue); - } - - /* - * (non-Javadoc) - * - * @see edu.wpi.first.wpilibj.can.AbstractCANTest#runMotorForward() - */ - protected void runMotorForward() { - getME().getMotor().set(kRunningValue); - } - - /* - * (non-Javadoc) - * - * @see edu.wpi.first.wpilibj.can.AbstractCANTest#runMotorReverse() - */ - protected void runMotorReverse() { - getME().getMotor().set(-kRunningValue); - } - - @Override - protected Logger getClassLogger() { - return logger; - } - - /** - * Sets up the test. - */ - @Before - public void setUp() { - getME().getMotor().setVoltageMode(CANJaguar.kQuadEncoder, 360); - getME().getMotor().set(kStoppedValue); - getME().getMotor().enableControl(); - - /* The motor might still have momentum from the previous test. */ - Timer.delay(kStartupTime); - } - - @Test - public void testRotateForwardToVoltage() { - setCANJaguar(kMotorTime, Math.PI); - assertEquals("The output voltage did not match the desired voltage set-point", Math.PI, getME() - .getMotor().getOutputVoltage(), kVoltageTolerance); - } - - @Test - public void testRotateReverseToVoltage() { - setCANJaguar(kMotorTime, -Math.PI); - assertEquals("The output voltage did not match the desired voltage set-point", -Math.PI, - getME().getMotor().getOutputVoltage(), kVoltageTolerance); - } - - - /** - * Sets up the test to have the CANJaguar running at the target voltage. - * - * @param targetValue the target voltage - * @param wait the PollingWait to to use to wait for the setup to complete with - */ - private void setupMotorVoltageForTest(final double targetValue, PollingWait wait) { - getME().getMotor().enableControl(); - setCANJaguar(1, targetValue); - wait.until(new RunnableAssert( - "[SETUP] Waiting for the output voltage to match the set output value") { - @Override - public void run() throws Exception { - getME().getMotor().set(targetValue); - assertEquals("[TEST SETUP] The output voltage should have matched the set value", - targetValue, getME().getMotor().getOutputVoltage(), 0.5); - assertEquals("[TEST SETUP] The set value did not match the get value", targetValue, getME() - .getMotor().get(), 0.5); - } - }); - } - - - @Test - public void testMaxOutputVoltagePositive() { - // given - double maxVoltage = 3; - - setupMotorVoltageForTest(kRunningValue, kWait); - - final double fastSpeed = getME().getMotor().getSpeed(); - - // when - getME().getMotor().configMaxOutputVoltage(maxVoltage); - - setCANJaguar(1, kRunningValue); - // Then - kWait.until(new RunnableAssert("Waiting for the speed to reduce using max output voltage") { - @Override - public void run() throws Exception { - runMotorForward(); - assertThat("Speed did not reduce when the max output voltage was set", fastSpeed, - is(greaterThan(getME().getMotor().getSpeed()))); - } - }); - - } - - @Test - public void testMaxOutputVoltagePositiveSetToZeroStopsMotor() { - // given - final double maxVoltage = 0; - - setupMotorVoltageForTest(kRunningValue, kWait); - // when - getME().getMotor().configMaxOutputVoltage(maxVoltage); - - setCANJaguar(1, kRunningValue); - // then - kWait.until(new RunnableAssert( - "Waiting for the speed to reduce to zero using max output voltage") { - @Override - public void run() throws Exception { - runMotorForward(); - assertEquals("Speed did not go to zero when the max output voltage was set to " - + maxVoltage, 0, getME().getMotor().getSpeed(), kEncoderSpeedTolerance); - } - }); - } - - - @Test - public void testMaxOutputVoltageNegative() { - // given - double maxVoltage = 3; - - setupMotorVoltageForTest(-kRunningValue, kWait); - final double fastSpeed = getME().getMotor().getSpeed(); - - // when - getME().getMotor().configMaxOutputVoltage(maxVoltage); - setCANJaguar(1, -kRunningValue); - - // then - kWait.until(new RunnableAssert("Waiting for the speed to reduce using max output voltage") { - - @Override - public void run() throws Exception { - runMotorReverse(); - assertThat("Speed did not reduce when the max output voltage was set", fastSpeed, - is(lessThan(getME().getMotor().getSpeed()))); - } - }); - } - - @Test - public void testMaxOutputVoltageNegativeSetToZeroStopsMotor() { - // given - final double maxVoltage = 0; - setupMotorVoltageForTest(-kRunningValue, kWait); - - // when - getME().getMotor().configMaxOutputVoltage(maxVoltage); - setCANJaguar(1, -kRunningValue); - - // Then - kWait.until(new RunnableAssert( - "Waiting for the speed to reduce to zero using max output voltage") { - @Override - public void run() throws Exception { - runMotorReverse(); - assertEquals("Speed did not go to zero when the max output voltage was set to " - + maxVoltage, 0, getME().getMotor().getSpeed(), kEncoderSpeedTolerance); - } - }); - } - -} diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/package-info.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/package-info.java deleted file mode 100644 index 0f1c9794f3..0000000000 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/package-info.java +++ /dev/null @@ -1,14 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) FIRST 2016. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -/** - * Provides a suite of tests to cover CANJaguar fully in all different control modes and with each - * supported positional input. Different setup parameters are provided in each Test class. All test - * classes that want to take advantage of the default test setup frameworks in place should extend - * {@link edu.wpi.first.wpilibj.can.AbstractCANTest AbstractCANTest}. - */ -package edu.wpi.first.wpilibj.can; diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/CANMotorEncoderFixture.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/CANMotorEncoderFixture.java deleted file mode 100644 index 2257e50fb1..0000000000 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/CANMotorEncoderFixture.java +++ /dev/null @@ -1,199 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) FIRST 2008-2016. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package edu.wpi.first.wpilibj.fixtures; - -import java.util.logging.Logger; - -import edu.wpi.first.wpilibj.CANJaguar; -import edu.wpi.first.wpilibj.DigitalOutput; -import edu.wpi.first.wpilibj.Relay; -import edu.wpi.first.wpilibj.Relay.Direction; -import edu.wpi.first.wpilibj.Relay.Value; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.mockhardware.FakePotentiometerSource; - -/** - * A fixture that wraps a {@link CANJaguar}. - */ -public abstract class CANMotorEncoderFixture extends MotorEncoderFixture implements - ITestFixture { - private static final Logger logger = Logger.getLogger(CANMotorEncoderFixture.class.getName()); - public static final double RELAY_POWER_UP_TIME = .75; - private FakePotentiometerSource m_potSource; - private DigitalOutput m_forwardLimit; - private DigitalOutput m_reverseLimit; - private Relay m_powerCycler; - private boolean m_initialized = false; - - protected abstract FakePotentiometerSource giveFakePotentiometerSource(); - - protected abstract DigitalOutput giveFakeForwardLimit(); - - protected abstract DigitalOutput giveFakeReverseLimit(); - - protected abstract Relay givePowerCycleRelay(); - - public CANMotorEncoderFixture() { - } - - private void initialize() { - synchronized (this) { - if (!m_initialized) { - m_initialized = true;// This ensures it is only initialized once - - m_powerCycler = givePowerCycleRelay(); - m_powerCycler.setDirection(Direction.kForward); - logger.fine("Turning on the power!"); - m_powerCycler.set(Value.kForward); - m_forwardLimit = giveFakeForwardLimit(); - m_reverseLimit = giveFakeReverseLimit(); - m_forwardLimit.set(false); - m_reverseLimit.set(false); - m_potSource = giveFakePotentiometerSource(); - Timer.delay(RELAY_POWER_UP_TIME); // Delay so the relay has time to boot - // up - } - } - } - - - @Override - public boolean setup() { - initialize(); // This initializes the Relay first - return super.setup(); - } - - @Override - public boolean reset() { - initialize(); - m_potSource.reset(); - m_forwardLimit.set(false); - m_reverseLimit.set(false); - getMotor().setPercentMode(); // Get the Jaguar into a mode where setting the - // speed means stop - return super.reset(); - } - - @Override - public boolean teardown() { - boolean wasNull = false; - if (m_potSource != null) { - m_potSource.free(); - m_potSource = null; - } else { - wasNull = true; - } - if (m_forwardLimit != null) { - m_forwardLimit.set(false); - m_forwardLimit.free(); - m_forwardLimit = null; - } else { - wasNull = true; - } - if (m_reverseLimit != null) { - m_reverseLimit.set(false); - m_reverseLimit.free(); - m_reverseLimit = null; - } else { - wasNull = true; - } - boolean superTornDown = false; - try { - superTornDown = super.teardown(); - } finally { - try { - if (getMotor() != null) { - getMotor().disableControl(); - getMotor().free(); - } else { - wasNull = true; - } - } finally { - if (m_powerCycler != null) { - m_powerCycler.free(); - m_powerCycler = null; - } else { - wasNull = true; - } - } - } - if (wasNull) { - throw new RuntimeException("CANMotorEncoderFixture had a null value at teardown"); - } - - return superTornDown; - } - - public FakePotentiometerSource getFakePot() { - initialize(); - return m_potSource; - } - - public DigitalOutput getForwardLimit() { - initialize(); - return m_forwardLimit; - } - - public DigitalOutput getReverseLimit() { - initialize(); - return m_reverseLimit; - } - - /** - * Prints the current status of the fixture. - */ - public String printStatus() { - StringBuilder status = new StringBuilder("CAN Motor Encoder Status: "); - if (getMotor() != null) { - status.append("\t" + getMotor().getDescription() + "\n"); - status.append("\tFault = " + getMotor().getFaults() + "\n"); - status.append("\tValue = " + getMotor().get() + "\n"); - status.append("\tOutputVoltage = " + getMotor().getOutputVoltage() + "\n"); - status.append("\tPosition = " + getMotor().getPosition() + "\n"); - status.append("\tForward Limit Ok = " + getMotor().getForwardLimitOK() + "\n"); - status.append("\tReverse Limit Ok = " + getMotor().getReverseLimitOK() + "\n"); - } else { - status.append("\t" + "CANJaguar Motor = null" + "\n"); - } - if (m_forwardLimit != null) { - status.append("\tForward Limit Output = " + m_forwardLimit.get() + "\n"); - } else { - status.append("\tForward Limit Output = null" + "\n"); - } - if (m_reverseLimit != null) { - status.append("\tReverse Limit Output = " + m_reverseLimit.get() + "\n"); - } else { - status.append("\tReverse Limit Output = null" + "\n"); - } - - return status.toString(); - } - - /** - * Browns out the fixture for a specific ammount of time. - * - * @param seconds The number of seconds to brown out for. - */ - public void brownOut(double seconds) { - initialize(); - powerOff(); - Timer.delay(seconds); - powerOn(); - } - - public void powerOff() { - initialize(); - m_powerCycler.set(Value.kOff); - } - - public void powerOn() { - initialize(); - m_powerCycler.set(Value.kForward); - } - -} diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/MotorEncoderFixture.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/MotorEncoderFixture.java index 04d98564f5..85569b0e31 100644 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/MotorEncoderFixture.java +++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/MotorEncoderFixture.java @@ -83,8 +83,7 @@ public abstract class MotorEncoderFixture implements m_counters[0] = new Counter(m_alphaSource); m_counters[1] = new Counter(m_betaSource); logger.fine("Creating the speed controller!"); - m_motor = giveSpeedController(); // CANJaguar throws an exception if it - // doesn't get the message + m_motor = giveSpeedController(); } } } diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/TestBench.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/TestBench.java index 0a58f53543..9a4f0ef5b5 100644 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/TestBench.java +++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/TestBench.java @@ -16,7 +16,6 @@ import java.util.List; import edu.wpi.first.wpilibj.AnalogGyro; import edu.wpi.first.wpilibj.AnalogInput; import edu.wpi.first.wpilibj.AnalogOutput; -import edu.wpi.first.wpilibj.CANJaguar; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.DigitalOutput; import edu.wpi.first.wpilibj.Jaguar; @@ -27,7 +26,6 @@ import edu.wpi.first.wpilibj.Talon; import edu.wpi.first.wpilibj.Victor; import edu.wpi.first.wpilibj.filters.LinearDigitalFilter; import edu.wpi.first.wpilibj.fixtures.AnalogCrossConnectFixture; -import edu.wpi.first.wpilibj.fixtures.CANMotorEncoderFixture; import edu.wpi.first.wpilibj.fixtures.DIOCrossConnectFixture; import edu.wpi.first.wpilibj.fixtures.FilterNoiseFixture; import edu.wpi.first.wpilibj.fixtures.FilterOutputFixture; @@ -63,14 +61,6 @@ public final class TestBench { public static final int kJaguarPDPChannel = 6; public static final int kVictorPDPChannel = 8; public static final int kTalonPDPChannel = 10; - public static final int kCANJaguarPDPChannel = 5; - - /* CAN ASSOICATED CHANNELS */ - public static final int kCANRelayPowerCycler = 1; - public static final int kFakeJaguarPotentiometer = 1; - public static final int kFakeJaguarForwardLimit = 20; - public static final int kFakeJaguarReverseLimit = 21; - public static final int kCANJaguarID = 2; // THESE MUST BE IN INCREMENTING ORDER public static final int DIOCrossConnectB2 = 9; @@ -196,67 +186,6 @@ public final class TestBench { return jagPair; } - public class BaseCANMotorEncoderFixture extends CANMotorEncoderFixture { - @Override - protected CANJaguar giveSpeedController() { - return new CANJaguar(kCANJaguarID); - } - - @Override - protected DigitalInput giveDigitalInputA() { - return new DigitalInput(18); - } - - @Override - protected DigitalInput giveDigitalInputB() { - return new DigitalInput(19); - } - - @Override - protected FakePotentiometerSource giveFakePotentiometerSource() { - return new FakePotentiometerSource(kFakeJaguarPotentiometer, 360); - } - - @Override - protected DigitalOutput giveFakeForwardLimit() { - return new DigitalOutput(kFakeJaguarForwardLimit); - } - - @Override - protected DigitalOutput giveFakeReverseLimit() { - return new DigitalOutput(kFakeJaguarReverseLimit); - } - - /* - * (non-Javadoc) - * - * @see - * edu.wpi.first.wpilibj.fixtures.CANMotorEncoderFixture#givePowerCycleRelay - * () - */ - @Override - protected Relay givePowerCycleRelay() { - return new Relay(kCANRelayPowerCycler); - } - - @Override - public int getPDPChannel() { - return kCANJaguarPDPChannel; - } - } - - /** - * Constructs a new set of objects representing a connected set of CANJaguar controlled Motors and - * an encoder
Note: The CANJaguar is not freshly allocated because the CANJaguar lacks a - * free() method. - * - * @return an existing CANJaguar and a freshly allocated Encoder - */ - public CANMotorEncoderFixture getCanJaguarPair() { - CANMotorEncoderFixture canPair = new BaseCANMotorEncoderFixture(); - return canPair; - } - /** * Constructs a new set of two Servo's and a Gyroscope. * diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/TestSuite.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/TestSuite.java index dd896e8797..00149f05c5 100644 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/TestSuite.java +++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/TestSuite.java @@ -27,7 +27,6 @@ import java.util.logging.Logger; import java.util.regex.Pattern; import edu.wpi.first.wpilibj.WpiLibJTestSuite; -import edu.wpi.first.wpilibj.can.CANTestSuite; import edu.wpi.first.wpilibj.command.CommandTestSuite; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboardTestSuite; @@ -38,8 +37,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboardTestSuite; */ @RunWith(Suite.class) // These are listed on separate lines to prevent merge conflicts -@SuiteClasses({WpiLibJTestSuite.class, CANTestSuite.class, CommandTestSuite.class, - SmartDashboardTestSuite.class}) +@SuiteClasses({WpiLibJTestSuite.class, CommandTestSuite.class, SmartDashboardTestSuite.class}) public class TestSuite extends AbstractTestSuite { static { // Sets up the logging output