Merge "remove no-position test because it hangs now"
diff --git a/aos/build/build.py b/aos/build/build.py
index ff85627..9f126c6 100755
--- a/aos/build/build.py
+++ b/aos/build/build.py
@@ -652,9 +652,6 @@
packages.add('clang-3.5')
packages.add('clang-format-3.5')
for platform in platforms:
- if platform.architecture() == 'arm':
- packages.add('gcc-4.7-arm-linux-gnueabihf')
- packages.add('g++-4.7-arm-linux-gnueabihf')
if (platform.compiler() == 'clang' or platform.compiler() == 'gcc_4.8' or
platform.compiler() == 'clang_frc'):
packages.add('clang-3.5')
@@ -665,7 +662,7 @@
if platform.compiler() == 'gcc' and platform.architecture() == 'amd64':
packages.add('gcc-4.7')
packages.add('g++-4.7')
- elif platform.compiler() == 'gcc_frc':
+ elif platform.compiler() == 'gcc_frc' || platform.compiler() == 'clang_frc':
packages.add('gcc-4.9-arm-frc-linux-gnueabi')
packages.add('g++-4.9-arm-frc-linux-gnueabi')
diff --git a/aos/externals/allwpilib/hal/include/HAL/Digital.hpp b/aos/externals/allwpilib/hal/include/HAL/Digital.hpp
index a860381..4d77976 100644
--- a/aos/externals/allwpilib/hal/include/HAL/Digital.hpp
+++ b/aos/externals/allwpilib/hal/include/HAL/Digital.hpp
@@ -93,6 +93,8 @@
void setEncoderSamplesToAverage(void* encoder_pointer, uint32_t samplesToAverage,
int32_t *status);
uint32_t getEncoderSamplesToAverage(void* encoder_pointer, int32_t *status);
+ void setEncoderIndexSource(void *encoder_pointer, uint32_t pin, bool analogTrigger, bool activeHigh,
+ bool edgeSensitive, int32_t *status);
uint16_t getLoopTiming(int32_t *status);
diff --git a/aos/externals/allwpilib/hal/lib/Athena/Digital.cpp b/aos/externals/allwpilib/hal/lib/Athena/Digital.cpp
index 006983f..4c12b17 100644
--- a/aos/externals/allwpilib/hal/lib/Athena/Digital.cpp
+++ b/aos/externals/allwpilib/hal/lib/Athena/Digital.cpp
@@ -12,6 +12,8 @@
#include "i2clib/i2c-lib.h"
#include "spilib/spi-lib.h"
+static_assert(sizeof(uint32_t) <= sizeof(void *), "This file shoves uint32_ts into pointers.");
+
static const uint32_t kExpectedLoopTiming = 40;
static const uint32_t kDigitalPins = 26;
static const uint32_t kPwmPins = 20;
@@ -1168,6 +1170,20 @@
}
/**
+ * Set an index source for an encoder, which is an input that resets the
+ * encoder's count.
+ */
+void setEncoderIndexSource(void *encoder_pointer, uint32_t pin, bool analogTrigger, bool activeHigh,
+ bool edgeSensitive, int32_t *status) {
+ Encoder* encoder = (Encoder*) encoder_pointer;
+ encoder->encoder->writeConfig_IndexSource_Channel((unsigned char)pin, status);
+ encoder->encoder->writeConfig_IndexSource_Module((unsigned char)0, status);
+ encoder->encoder->writeConfig_IndexSource_AnalogTrigger(analogTrigger, status);
+ encoder->encoder->writeConfig_IndexActiveHigh(activeHigh, status);
+ encoder->encoder->writeConfig_IndexEdgeSensitive(edgeSensitive, status);
+}
+
+/**
* Get the loop timing of the PWM system
*
* @return The loop time
diff --git a/aos/externals/allwpilib/hal/lib/Athena/Semaphore.cpp b/aos/externals/allwpilib/hal/lib/Athena/Semaphore.cpp
index df30f3c..6f3dd86 100644
--- a/aos/externals/allwpilib/hal/lib/Athena/Semaphore.cpp
+++ b/aos/externals/allwpilib/hal/lib/Athena/Semaphore.cpp
@@ -136,6 +136,9 @@
delete sem;
}
+/**
+ * @param timeout Not implemented.
+ */
int8_t takeMultiWait(MULTIWAIT_ID sem, MUTEX_ID m, int32_t timeout) {
lockMutex(m);
int8_t val = pthread_cond_wait(sem, m->native_handle());
diff --git a/aos/externals/allwpilib/hal/lib/Athena/Task.cpp b/aos/externals/allwpilib/hal/lib/Athena/Task.cpp
index 2be66f5..f39a7b5 100644
--- a/aos/externals/allwpilib/hal/lib/Athena/Task.cpp
+++ b/aos/externals/allwpilib/hal/lib/Athena/Task.cpp
@@ -37,6 +37,11 @@
return ret;
}
+/**
+ * @param priority Not implemented.
+ * @param options Not implemented.
+ * @param stackSize Not implemented.
+ */
TASK spawnTask(char * name, int priority, int options, int stackSize,
FUNCPTR entryPt, uint32_t arg0, uint32_t arg1, uint32_t arg2,
uint32_t arg3, uint32_t arg4, uint32_t arg5, uint32_t arg6,
diff --git a/aos/externals/allwpilib/wpilibc/wpilibC++/include/Utility.h b/aos/externals/allwpilib/wpilibc/wpilibC++/include/Utility.h
index 4fe325f..f081489 100644
--- a/aos/externals/allwpilib/wpilibc/wpilibC++/include/Utility.h
+++ b/aos/externals/allwpilib/wpilibc/wpilibC++/include/Utility.h
@@ -5,6 +5,10 @@
/*---------------------------------------------------------------------------*/
#pragma once
+/** @file
+ * Contains global utility functions
+ */
+
#include <stdint.h>
#include <string>
diff --git a/aos/externals/allwpilib/wpilibc/wpilibC++Devices/include/Encoder.h b/aos/externals/allwpilib/wpilibc/wpilibC++Devices/include/Encoder.h
index 45c4e5f..780d524 100644
--- a/aos/externals/allwpilib/wpilibc/wpilibC++Devices/include/Encoder.h
+++ b/aos/externals/allwpilib/wpilibc/wpilibC++Devices/include/Encoder.h
@@ -29,6 +29,7 @@
class Encoder : public SensorBase, public CounterBase, public PIDSource
{
public:
+ enum IndexingType { kResetWhileHigh, kResetWhileLow, kResetOnFallingEdge, kResetOnRisingEdge };
Encoder(uint32_t aChannel, uint32_t bChannel, bool reverseDirection = false,
EncodingType encodingType = k4X);
@@ -57,6 +58,10 @@
void SetPIDSourceParameter(PIDSourceParameter pidSource);
double PIDGet();
+ void SetIndexSource(uint32_t channel, IndexingType type = kResetOnRisingEdge);
+ void SetIndexSource(DigitalSource *source, IndexingType type = kResetOnRisingEdge);
+ void SetIndexSource(DigitalSource &source, IndexingType type = kResetOnRisingEdge);
+
int32_t GetFPGAIndex()
{
return m_index;
diff --git a/aos/externals/allwpilib/wpilibc/wpilibC++Devices/include/RobotDrive.h b/aos/externals/allwpilib/wpilibc/wpilibC++Devices/include/RobotDrive.h
index e6cd031..7286331 100644
--- a/aos/externals/allwpilib/wpilibc/wpilibC++Devices/include/RobotDrive.h
+++ b/aos/externals/allwpilib/wpilibc/wpilibC++Devices/include/RobotDrive.h
@@ -67,6 +67,7 @@
void SetInvertedMotor(MotorType motor, bool isInverted);
void SetSensitivity(float sensitivity);
void SetMaxOutput(double maxOutput);
+ void SetCANJaguarSyncGroup(uint8_t syncGroup);
void SetExpiration(float timeout);
float GetExpiration();
@@ -93,6 +94,7 @@
SpeedController *m_rearLeftMotor;
SpeedController *m_rearRightMotor;
MotorSafetyHelper *m_safetyHelper;
+ uint8_t m_syncGroup;
private:
int32_t GetNumMotors()
diff --git a/aos/externals/allwpilib/wpilibc/wpilibC++Devices/include/WPILib.h b/aos/externals/allwpilib/wpilibc/wpilibC++Devices/include/WPILib.h
index d0f6f92..5af18c9 100644
--- a/aos/externals/allwpilib/wpilibc/wpilibC++Devices/include/WPILib.h
+++ b/aos/externals/allwpilib/wpilibc/wpilibC++Devices/include/WPILib.h
@@ -19,23 +19,8 @@
#include "AnalogTrigger.h"
#include "AnalogTriggerOutput.h"
#include "BuiltInAccelerometer.h"
-#include "Buttons/InternalButton.h"
-#include "Buttons/JoystickButton.h"
-#include "Buttons/NetworkButton.h"
-#include "CameraServer.h"
#include "CANJaguar.h"
#include "CANTalon.h"
-#include "Commands/Command.h"
-#include "Commands/CommandGroup.h"
-#include "Commands/PIDCommand.h"
-#include "Commands/PIDSubsystem.h"
-#include "Commands/PrintCommand.h"
-#include "Commands/Scheduler.h"
-#include "Commands/StartCommand.h"
-#include "Commands/Subsystem.h"
-#include "Commands/WaitCommand.h"
-#include "Commands/WaitForChildren.h"
-#include "Commands/WaitUntilCommand.h"
#include "Compressor.h"
#include "ControllerPower.h"
#include "Counter.h"
@@ -57,10 +42,8 @@
#include "Jaguar.h"
#include "Joystick.h"
#include "Notifier.h"
-#include "PIDController.h"
#include "PIDOutput.h"
#include "PIDSource.h"
-#include "Preferences.h"
#include "PowerDistributionPanel.h"
#include "PWM.h"
#include "Relay.h"
@@ -68,11 +51,8 @@
#include "RobotBase.h"
#include "RobotDrive.h"
#include "SensorBase.h"
-#include "SerialPort.h"
#include "Servo.h"
#include "SampleRobot.h"
-#include "SmartDashboard/SendableChooser.h"
-#include "SmartDashboard/SmartDashboard.h"
#include "Solenoid.h"
#include "SpeedController.h"
#include "SPI.h"
diff --git a/aos/externals/allwpilib/wpilibc/wpilibC++Devices/src/CANTalon.cpp b/aos/externals/allwpilib/wpilibc/wpilibC++Devices/src/CANTalon.cpp
index 83cf5d2..0c20b55 100644
--- a/aos/externals/allwpilib/wpilibc/wpilibC++Devices/src/CANTalon.cpp
+++ b/aos/externals/allwpilib/wpilibc/wpilibC++Devices/src/CANTalon.cpp
@@ -66,9 +66,16 @@
}
}
-/**
- * TODO documentation (see CANJaguar.cpp)
- */
+ /**
+ * Gets the current status of the Talon (usually a sensor value).
+ *
+ * In Current mode: returns output current.
+ * In Speed mode: returns current speed.
+ * In Position mode: returns current sensor position.
+ * In PercentVbus and Follower modes: returns current applied throttle.
+ *
+ * @return The current sensor value of the Talon.
+ */
float CANTalon::Get()
{
int value;
@@ -84,6 +91,7 @@
m_impl->GetSensorPosition(value);
return value;
case kPercentVbus:
+ case kFollower:
default:
m_impl->GetAppliedThrottle(value);
return (float)value / 1023.0;
@@ -94,11 +102,12 @@
* Sets the appropriate output on the talon, depending on the mode.
*
* In PercentVbus, the output is between -1.0 and 1.0, with 0.0 as stopped.
- * In Voltage mode, outputValue is in volts.
- * In Current mode, outputValue is in amperes.
- * In Speed mode, outputValue is in position change / 10ms.
- * In Position mode, outputValue is in encoder ticks or an analog value,
+ * In Voltage mode, output value is in volts.
+ * In Current mode, output value is in amperes.
+ * In Speed mode, output value is in position change / 10ms.
+ * In Position mode, output value is in encoder ticks or an analog value,
* depending on the sensor.
+ * In Follower mode, the output value is the integer device ID of the talon to duplicate.
*
* @param outputValue The setpoint value, as described above.
* @see SelectProfileSlot to choose between the two sets of gains.
@@ -135,7 +144,9 @@
case CANSpeedController::kPosition:
status = m_impl->SetDemand(value);
break;
+ // XXX: What about CANSpeedController::kCurrent?
default:
+ status = CTR_InvalidParamValue;
break;
}
if (status != CTR_OKAY) {
@@ -188,10 +199,12 @@
}
}
-/**
- * TODO documentation (see CANJaguar.cpp)
- * @see SelectProfileSlot to choose between the two sets of gains.
- */
+ /**
+ * Set the integration constant of the currently selected profile.
+ *
+ * @param i Integration constant for the currently selected PID profile.
+ * @see SelectProfileSlot to choose between the two sets of gains.
+ */
void CANTalon::SetI(double i)
{
CTR_Code status = m_impl->SetIgain(m_profile, i);
@@ -201,7 +214,9 @@
}
/**
- * TODO documentation (see CANJaguar.cpp)
+ * Set the derivative constant of the currently selected profile.
+ *
+ * @param d Derivative constant for the currently selected PID profile.
* @see SelectProfileSlot to choose between the two sets of gains.
*/
void CANTalon::SetD(double d)
@@ -212,7 +227,9 @@
}
}
/**
+ * Set the feedforward value of the currently selected profile.
*
+ * @param f Feedforward constant for the currently selected PID profile.
* @see SelectProfileSlot to choose between the two sets of gains.
*/
void CANTalon::SetF(double f)
@@ -248,7 +265,11 @@
}
}
/**
- * TODO documentation (see CANJaguar.cpp)
+ * Sets control values for closed loop control.
+ *
+ * @param p Proportional constant.
+ * @param i Integration constant.
+ * @param d Differential constant.
* This function does not modify F-gain. Considerable passing a zero for f using
* the four-parameter function.
*/
@@ -258,6 +279,14 @@
SetI(i);
SetD(d);
}
+/**
+ * Sets control values for closed loop control.
+ *
+ * @param p Proportional constant.
+ * @param i Integration constant.
+ * @param d Differential constant.
+ * @param f Feedforward constant.
+ */
void CANTalon::SetPID(double p, double i, double d, double f)
{
SetP(p);
@@ -287,7 +316,9 @@
}
/**
- * TODO documentation (see CANJaguar.cpp)
+ * Get the current proportional constant.
+ *
+ * @return double proportional constant for current profile.
* @see SelectProfileSlot to choose between the two sets of gains.
*/
double CANTalon::GetP()
@@ -402,7 +433,7 @@
/**
* Returns the voltage coming in from the battery.
*
- * @return The input voltage in vols.
+ * @return The input voltage in volts.
*/
float CANTalon::GetBusVoltage()
{
@@ -415,7 +446,7 @@
}
/**
- * TODO documentation (see CANJaguar.cpp)
+ * @return The voltage being output by the Talon, in Volts.
*/
float CANTalon::GetOutputVoltage()
{
@@ -430,7 +461,7 @@
/**
- * TODO documentation (see CANJaguar.cpp)
+ * Returns the current going through the Talon, in Amperes.
*/
float CANTalon::GetOutputCurrent()
{
@@ -444,9 +475,9 @@
return current;
}
-/**
- * TODO documentation (see CANJaguar.cpp)
- */
+ /**
+ * Returns temperature of Talon, in degrees Celsius.
+ */
float CANTalon::GetTemperature()
{
double temp;
@@ -965,7 +996,7 @@
return brakeEn;
}
/**
- * TODO documentation (see CANJaguar.cpp)
+ * @deprecated not implemented
*/
void CANTalon::ConfigEncoderCodesPerRev(uint16_t codesPerRev)
{
@@ -973,7 +1004,7 @@
}
/**
- * TODO documentation (see CANJaguar.cpp)
+ * @deprecated not implemented
*/
void CANTalon::ConfigPotentiometerTurns(uint16_t turns)
{
@@ -981,7 +1012,7 @@
}
/**
- * TODO documentation (see CANJaguar.cpp)
+ * @deprecated not implemented
*/
void CANTalon::ConfigSoftPositionLimits(double forwardLimitPosition, double reverseLimitPosition)
{
diff --git a/aos/externals/allwpilib/wpilibc/wpilibC++Devices/src/DriverStation.cpp b/aos/externals/allwpilib/wpilibc/wpilibC++Devices/src/DriverStation.cpp
index 7821289..91fccd5 100644
--- a/aos/externals/allwpilib/wpilibc/wpilibC++Devices/src/DriverStation.cpp
+++ b/aos/externals/allwpilib/wpilibc/wpilibC++Devices/src/DriverStation.cpp
@@ -66,7 +66,13 @@
AddToSingletonList();
- if (!m_task.Start((int32_t)this))
+ // They need to be identical or it could lead to runtime stack corruption if
+ // the caller and callee push and pop different amounts of data on the stack.
+ static_assert(sizeof(this) == sizeof(uint32_t),
+ "We are passing a pointer through a uint32_t");
+ static_assert(alignof(this) <= alignof(uint32_t),
+ "We are passing a pointer through a uint32_t");
+ if (!m_task.Start((uint32_t)this))
{
wpi_setWPIError(DriverStationTaskError);
}
@@ -84,6 +90,8 @@
deleteMutex(m_waitForDataMutex);
}
+// XXX: This assumes that the calling convention treats pointers and uint32_ts
+// identical, which is not necessarily true.
void DriverStation::InitTask(DriverStation *ds)
{
ds->Run();
diff --git a/aos/externals/allwpilib/wpilibc/wpilibC++Devices/src/Encoder.cpp b/aos/externals/allwpilib/wpilibc/wpilibC++Devices/src/Encoder.cpp
index 310051d..4a2afb1 100644
--- a/aos/externals/allwpilib/wpilibc/wpilibC++Devices/src/Encoder.cpp
+++ b/aos/externals/allwpilib/wpilibc/wpilibC++Devices/src/Encoder.cpp
@@ -503,3 +503,44 @@
return 0.0;
}
}
+
+/**
+ * Set the index source for the encoder. When this source is activated, the encoder count automatically resets.
+ *
+ * @param channel A DIO channel to set as the encoder index
+ * @param type The state that will cause the encoder to reset
+ */
+void Encoder::SetIndexSource(uint32_t channel, Encoder::IndexingType type) {
+ int32_t status = 0;
+ bool activeHigh = (type == kResetWhileHigh) || (type == kResetOnRisingEdge);
+ bool edgeSensitive = (type == kResetOnFallingEdge) || (type == kResetOnRisingEdge);
+
+ setEncoderIndexSource(m_encoder, channel, false, activeHigh, edgeSensitive, &status);
+ wpi_setGlobalErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Set the index source for the encoder. When this source is activated, the encoder count automatically resets.
+ *
+ * @param channel A digital source to set as the encoder index
+ * @param type The state that will cause the encoder to reset
+ */
+void Encoder::SetIndexSource(DigitalSource *source, Encoder::IndexingType type) {
+ int32_t status = 0;
+ bool activeHigh = (type == kResetWhileHigh) || (type == kResetOnRisingEdge);
+ bool edgeSensitive = (type == kResetOnFallingEdge) || (type == kResetOnRisingEdge);
+
+ setEncoderIndexSource(m_encoder, source->GetChannelForRouting(), source->GetAnalogTriggerForRouting(), activeHigh,
+ edgeSensitive, &status);
+ wpi_setGlobalErrorWithContext(status, getHALErrorMessage(status));
+}
+
+/**
+ * Set the index source for the encoder. When this source is activated, the encoder count automatically resets.
+ *
+ * @param channel A digital source to set as the encoder index
+ * @param type The state that will cause the encoder to reset
+ */
+void Encoder::SetIndexSource(DigitalSource &source, Encoder::IndexingType type) {
+ SetIndexSource(&source, type);
+}
diff --git a/aos/externals/allwpilib/wpilibc/wpilibC++Devices/src/Joystick.cpp b/aos/externals/allwpilib/wpilibc/wpilibC++Devices/src/Joystick.cpp
index 790f0d7..46a9f08 100644
--- a/aos/externals/allwpilib/wpilibc/wpilibC++Devices/src/Joystick.cpp
+++ b/aos/externals/allwpilib/wpilibc/wpilibC++Devices/src/Joystick.cpp
@@ -107,6 +107,7 @@
/**
* Get the X value of the joystick.
* This depends on the mapping of the joystick connected to the current port.
+ * @param hand This parameter is ignored for the Joystick class and is only here to complete the GenericHID interface.
*/
float Joystick::GetX(JoystickHand hand)
{
@@ -116,6 +117,7 @@
/**
* Get the Y value of the joystick.
* This depends on the mapping of the joystick connected to the current port.
+ * @param hand This parameter is ignored for the Joystick class and is only here to complete the GenericHID interface.
*/
float Joystick::GetY(JoystickHand hand)
{
diff --git a/aos/externals/allwpilib/wpilibc/wpilibC++Devices/src/RobotDrive.cpp b/aos/externals/allwpilib/wpilibc/wpilibC++Devices/src/RobotDrive.cpp
index 8d44ca0..3eb90ff 100644
--- a/aos/externals/allwpilib/wpilibc/wpilibC++Devices/src/RobotDrive.cpp
+++ b/aos/externals/allwpilib/wpilibc/wpilibC++Devices/src/RobotDrive.cpp
@@ -41,6 +41,7 @@
m_maxOutput = 1.0;
m_safetyHelper = new MotorSafetyHelper(this);
m_safetyHelper->SetSafetyEnabled(true);
+ m_syncGroup = 0;
}
/**
@@ -507,14 +508,15 @@
Normalize(wheelSpeeds);
- uint8_t syncGroup = 0x80;
+ m_frontLeftMotor->Set(wheelSpeeds[kFrontLeftMotor] * m_invertedMotors[kFrontLeftMotor] * m_maxOutput, m_syncGroup);
+ m_frontRightMotor->Set(wheelSpeeds[kFrontRightMotor] * m_invertedMotors[kFrontRightMotor] * m_maxOutput, m_syncGroup);
+ m_rearLeftMotor->Set(wheelSpeeds[kRearLeftMotor] * m_invertedMotors[kRearLeftMotor] * m_maxOutput, m_syncGroup);
+ m_rearRightMotor->Set(wheelSpeeds[kRearRightMotor] * m_invertedMotors[kRearRightMotor] * m_maxOutput, m_syncGroup);
- m_frontLeftMotor->Set(wheelSpeeds[kFrontLeftMotor] * m_invertedMotors[kFrontLeftMotor] * m_maxOutput, syncGroup);
- m_frontRightMotor->Set(wheelSpeeds[kFrontRightMotor] * m_invertedMotors[kFrontRightMotor] * m_maxOutput, syncGroup);
- m_rearLeftMotor->Set(wheelSpeeds[kRearLeftMotor] * m_invertedMotors[kRearLeftMotor] * m_maxOutput, syncGroup);
- m_rearRightMotor->Set(wheelSpeeds[kRearRightMotor] * m_invertedMotors[kRearRightMotor] * m_maxOutput, syncGroup);
-
- CANJaguar::UpdateSyncGroup(syncGroup);
+ if (m_syncGroup != 0)
+ {
+ CANJaguar::UpdateSyncGroup(m_syncGroup);
+ }
m_safetyHelper->Feed();
}
@@ -556,14 +558,15 @@
Normalize(wheelSpeeds);
- uint8_t syncGroup = 0x80;
+ m_frontLeftMotor->Set(wheelSpeeds[kFrontLeftMotor] * m_invertedMotors[kFrontLeftMotor] * m_maxOutput, m_syncGroup);
+ m_frontRightMotor->Set(wheelSpeeds[kFrontRightMotor] * m_invertedMotors[kFrontRightMotor] * m_maxOutput, m_syncGroup);
+ m_rearLeftMotor->Set(wheelSpeeds[kRearLeftMotor] * m_invertedMotors[kRearLeftMotor] * m_maxOutput, m_syncGroup);
+ m_rearRightMotor->Set(wheelSpeeds[kRearRightMotor] * m_invertedMotors[kRearRightMotor] * m_maxOutput, m_syncGroup);
- m_frontLeftMotor->Set(wheelSpeeds[kFrontLeftMotor] * m_invertedMotors[kFrontLeftMotor] * m_maxOutput, syncGroup);
- m_frontRightMotor->Set(wheelSpeeds[kFrontRightMotor] * m_invertedMotors[kFrontRightMotor] * m_maxOutput, syncGroup);
- m_rearLeftMotor->Set(wheelSpeeds[kRearLeftMotor] * m_invertedMotors[kRearLeftMotor] * m_maxOutput, syncGroup);
- m_rearRightMotor->Set(wheelSpeeds[kRearRightMotor] * m_invertedMotors[kRearRightMotor] * m_maxOutput, syncGroup);
-
- CANJaguar::UpdateSyncGroup(syncGroup);
+ if (m_syncGroup != 0)
+ {
+ CANJaguar::UpdateSyncGroup(m_syncGroup);
+ }
m_safetyHelper->Feed();
}
@@ -595,17 +598,18 @@
{
wpi_assert(m_rearLeftMotor != NULL && m_rearRightMotor != NULL);
- uint8_t syncGroup = 0x80;
-
if (m_frontLeftMotor != NULL)
- m_frontLeftMotor->Set(Limit(leftOutput) * m_invertedMotors[kFrontLeftMotor] * m_maxOutput, syncGroup);
- m_rearLeftMotor->Set(Limit(leftOutput) * m_invertedMotors[kRearLeftMotor] * m_maxOutput, syncGroup);
+ m_frontLeftMotor->Set(Limit(leftOutput) * m_invertedMotors[kFrontLeftMotor] * m_maxOutput, m_syncGroup);
+ m_rearLeftMotor->Set(Limit(leftOutput) * m_invertedMotors[kRearLeftMotor] * m_maxOutput, m_syncGroup);
if (m_frontRightMotor != NULL)
- m_frontRightMotor->Set(-Limit(rightOutput) * m_invertedMotors[kFrontRightMotor] * m_maxOutput, syncGroup);
- m_rearRightMotor->Set(-Limit(rightOutput) * m_invertedMotors[kRearRightMotor] * m_maxOutput, syncGroup);
+ m_frontRightMotor->Set(-Limit(rightOutput) * m_invertedMotors[kFrontRightMotor] * m_maxOutput, m_syncGroup);
+ m_rearRightMotor->Set(-Limit(rightOutput) * m_invertedMotors[kRearRightMotor] * m_maxOutput, m_syncGroup);
- CANJaguar::UpdateSyncGroup(syncGroup);
+ if (m_syncGroup != 0)
+ {
+ CANJaguar::UpdateSyncGroup(m_syncGroup);
+ }
m_safetyHelper->Feed();
}
@@ -698,7 +702,15 @@
m_maxOutput = maxOutput;
}
-
+/**
+ * Set the number of the sync group for the motor controllers. If the motor controllers are {@link CANJaguar}s,
+ * then they will all be added to this sync group, causing them to update their values at the same time.
+ *
+ * @param syncGroup the update group to add the motor controllers to
+ */
+void RobotDrive::SetCANJaguarSyncGroup(uint8_t syncGroup) {
+ m_syncGroup = syncGroup;
+}
void RobotDrive::SetExpiration(float timeout)
{
diff --git a/aos/externals/allwpilib/wpilibc/wpilibC++Devices/src/Vision/BaeUtilities.cpp b/aos/externals/allwpilib/wpilibc/wpilibC++Devices/src/Vision/BaeUtilities.cpp
index 8804411..278fa59 100644
--- a/aos/externals/allwpilib/wpilibc/wpilibC++Devices/src/Vision/BaeUtilities.cpp
+++ b/aos/externals/allwpilib/wpilibc/wpilibC++Devices/src/Vision/BaeUtilities.cpp
@@ -16,7 +16,7 @@
#include "Servo.h"
#include "Timer.h"
-/**
+/** @file
* Utility functions
*/
diff --git a/aos/externals/allwpilib/wpilibc/wpilibC++Devices/src/Vision/VisionAPI.cpp b/aos/externals/allwpilib/wpilibc/wpilibC++Devices/src/Vision/VisionAPI.cpp
index 7ac3ac9..c72b321 100644
--- a/aos/externals/allwpilib/wpilibc/wpilibC++Devices/src/Vision/VisionAPI.cpp
+++ b/aos/externals/allwpilib/wpilibc/wpilibC++Devices/src/Vision/VisionAPI.cpp
@@ -14,7 +14,9 @@
int VisionAPI_debugFlag = 1;
#define DPRINTF if(VisionAPI_debugFlag)dprintf
-/* Image Management functions */
+/** @file
+ * Image Management functions
+ */
/**
* @brief Create an image object
diff --git a/frc971/control_loops/claw/claw.q b/frc971/control_loops/claw/claw.q
new file mode 100644
index 0000000..38252d7
--- /dev/null
+++ b/frc971/control_loops/claw/claw.q
@@ -0,0 +1,70 @@
+package frc971.control_loops;
+
+import "aos/common/controls/control_loops.q";
+
+queue_group Claw {
+ implements aos.control_loops.ControlLoop;
+
+ // NOTE: Unless otherwise specified, assume that all angle measures are in
+ // radians. An angle of zero means that the appendage is sticking straight out
+ // horizontally, pointing towards the front of the robot. Rotating the appendage
+ // up and towards the back of the robot increases the angle, moving it down
+ // and towards the back decreases it. (Think unit circle.) This rule holds
+ // true for both angle goals and encoder positions.
+ // Also note that unless otherwise specified, potentiometer readings are
+ // from 0V to 5V. As with the encoders, moving up and towards the back
+ // of the robot increases this value, moving down and towards the back
+ // decreases it.
+ // For all output voltage parameters, assume that a positive voltage moves
+ // the appendage in a direction that increases the value of the encoder, and
+ // vice versa. (For an output voltage parameter for something without an
+ // encoder, directions will be individually specified.)
+
+ message Goal {
+ // Angle of shoulder joint.
+ double angle;
+ // Voltage of intake rollers. Positive means sucking in, negative means
+ // spitting out.
+ double intake;
+
+ // Should claw be in open or closed position? (true means open.)
+ bool open;
+ };
+
+ message Position {
+ // Position of shoulder joint from encoder.
+ double encoder_pos;
+ // Reading from potentiometer.
+ double pot_pos;
+ // Position of encoder at last index pulse.
+ double last_index;
+ // Reading from potentiometer at last index pulse.
+ double last_index_pot;
+ // A count of how many index pulses we've seen on the shoulder encoder.
+ uint32_t index_pulses;
+ };
+
+ message Output {
+ // Voltage for intake motors. Positive is sucking in, negative is spitting
+ // out.
+ double intake_voltage;
+ // Voltage for shoulder motors.
+ double shoulder_voltage;
+ // Claw in opened or closed position. (true means open.)
+ bool open;
+ };
+
+ message Status {
+ // Is claw zeroed?
+ bool zeroed;
+ // Has claw zeroed and reached goal?
+ bool done;
+ };
+
+ queue Goal goal;
+ queue Position position;
+ queue Output output;
+ queue Status status;
+};
+
+queue_group Claw claw;
diff --git a/frc971/control_loops/fridge/fridge.q b/frc971/control_loops/fridge/fridge.q
new file mode 100644
index 0000000..9e623b3
--- /dev/null
+++ b/frc971/control_loops/fridge/fridge.q
@@ -0,0 +1,101 @@
+package frc971.control_loops;
+
+import "aos/common/controls/control_loops.q";
+
+queue_group Fridge {
+ implements aos.control_loops.ControlLoop;
+
+ // NOTE: Unless otherwise specified, assume that all angle measures are in
+ // radians. An angle of zero means that the appendage is sticking straight out
+ // horizontally, pointing towards the front of the robot. Rotating the appendage
+ // up and towards the back of the robot increases the angle, moving it down
+ // and towards the back decreases it. (Think unit circle.) This rule holds
+ // true for both angle goals and encoder positions.
+ // Also note that unless otherwise specified, potentiometer readings are
+ // from 0V to 5V. As with the encoders, moving up and towards the back
+ // of the robot increases this value, moving down and towards the back
+ // decreases it.
+ // For all output voltage parameters, assume that a positive voltage moves
+ // the appendage in a direction that increases the value of the encoder, and
+ // vice versa. (For an output voltage parameter for something without an
+ // encoder, directions will be individually specified.)
+
+ // NOTE: Elevator heights are defined as follows: The height of the elevator
+ // is the vertical distance (in meters) between the top of the frame
+ // (front and back), and the arm pivot axis. A constant specifies the minimum
+ // value for this distance.
+
+ message Goal {
+ // Position of the arm in radians.
+ double angle;
+ // Height of the elevator in meters.
+ double height;
+
+ // Should the grabbers be deployed?
+ bool grabbers_deployed;
+ };
+
+ message Position {
+ // Position of arm from encoder.
+ double arm_encoder_pos;
+ // Reading from arm potentiometer.
+ double arm_pot_pos;
+ // Position of arm encoder at last index pulse.
+ double arm_last_index;
+ // Reading from arm potentiometer at last index pulse.
+ double arm_last_index_pot;
+ // A count of how many index pulses we've seen on the arm encoder.
+ uint32_t arm_index_pulses;
+
+ // Height of left side from encoder.
+ double left_encoder_pos;
+ // Reading from left side potentiometer. Directions work the same for this
+ // as for the encoder.
+ double left_pot_pos;
+ // Position of left encoder at last index pulse.
+ double left_last_index;
+ // Reading from left potentiometer at last index pulse.
+ double left_last_index_pot;
+ // A count of how many index pulses we've seen on the left encoder.
+ uint32_t left_index_pulses;
+
+ // Height of right side from encoder. Directions are the same as
+ // for the left side.
+ double right_encoder_pos;
+ // Reading from right side potentiometer. Directions work the same for this
+ // as for the encoder.
+ double right_pot_pos;
+ // Position of right encoder at last index pulse.
+ double right_last_index;
+ // Reading from right potentiometer at last index pulse.
+ double right_last_index_pot;
+ // A count of how many index pulses we've seen on the right encoder.
+ uint32_t right_index_pulses;
+
+ };
+
+ message Status {
+ // Are both the arm and elevator zeroed?
+ bool zeroed;
+ // Are we zeroed and have reached our goal position on both the arm and
+ // elevator?
+ bool done;
+ };
+
+ message Output {
+ // Voltage of arm motor.
+ double arm_voltage;
+ // Voltage of left elevator motor.
+ double left_voltage;
+ // Voltage of right elevator motor.
+ double right_voltage;
+
+ // Are grabber pistons deployed?
+ bool grabbers_deployed;
+ };
+
+ queue Goal goal;
+ queue Position position;
+ queue Output output;
+ queue Status status;
+};