merging the recent gyro board code updates in
I'm merging the wrong way in the middle because the merge is complicated
because there are changes to things that got split out into different
files.
diff --git a/aos/atom_code/output/motor_output.cc b/aos/atom_code/output/motor_output.cc
index b68dcad..48acc38 100644
--- a/aos/atom_code/output/motor_output.cc
+++ b/aos/atom_code/output/motor_output.cc
@@ -17,13 +17,19 @@
uint8_t MotorOutput::MotorControllerBounds::Map(double value) const {
if (value == 0.0) return kCenter;
+ if (value > 12.0) return Map(12.0);
+ if (value < -12.0) return Map(-12.0);
+ uint8_t r;
if (value > 0.0) {
- return static_cast<uint8_t>(kDeadbandMax + (value * (kMax - kDeadbandMax)) +
- 0.5);
+ r = static_cast<uint8_t>(kDeadbandMax + (value * (kMax - kDeadbandMax)) +
+ 0.5);
} else {
- return static_cast<uint8_t>(kDeadbandMin + (value * (kDeadbandMin - kMin)) +
- 0.5);
+ r = static_cast<uint8_t>(kDeadbandMin + (value * (kDeadbandMin - kMin)) +
+ 0.5);
}
+ if (r < kMin) return kMin;
+ if (r > kMax) return kMax;
+ return r;
}
MotorOutput::MotorOutput()
diff --git a/aos/build/aos.gypi b/aos/build/aos.gypi
index 6f3364f..9c74438 100644
--- a/aos/build/aos.gypi
+++ b/aos/build/aos.gypi
@@ -23,12 +23,8 @@
'conditions': [
['OS=="crio"', {
'make_global_settings': [
- ['CC', '<!(which powerpc-wrs-vxworks-gcc)'],
- ['CXX', '<!(which powerpc-wrs-vxworks-g++)'],
- ['LD', '<!(readlink -f <(AOS)/build/crio_link_out)'],
- #['LD', 'powerpc-wrs-vxworks-ld'],
- #['AR', '<!(which powerpc-wrs-vxworks-ar)'],
- #['NM', '<!(which powerpc-wrs-vxworks-nm)'],
+ ['CC', '<!(readlink -f <(AOS)/build/crio_cc)'],
+ ['CXX', '<!(readlink -f <(AOS)/build/crio_cxx)'],
],
'variables': {
'aos_target': 'static_library',
@@ -185,7 +181,10 @@
'-Wunused-local-typedefs',
# Give macro stack traces when they blow up.
- '-ftrack-macro-expansion',
+ # TODO(brians): Re-enable this once they fix the bug where it
+ # sometimes doesn't show you the top-most (aka most useful)
+ # line of code.
+ #'-ftrack-macro-expansion',
],
'cflags_cc': [
'-std=gnu++11',
diff --git a/aos/build/build.sh b/aos/build/build.sh
index eb639de..9ec745a 100755
--- a/aos/build/build.sh
+++ b/aos/build/build.sh
@@ -1,31 +1,37 @@
-#!/bin/bash -e
+#!/bin/bash
#set -x
+set -e
+
# This file should be called to build the code.
-# Usage: build.sh platform main_file.gyp debug [action]
+# Usage: build.sh platform main_file.gyp debug [action]...
PLATFORM=$1
GYP_MAIN=$2
DEBUG=$3
ACTION=$4
+shift 3
+shift || true # We might not have a 4th argument if ACTION is empty.
+
export WIND_BASE=${WIND_BASE:-"/usr/local/powerpc-wrs-vxworks/wind_base"}
-[ ${PLATFORM} == "crio" -o ${PLATFORM} == "atom" ] || ( echo Platform "(${PLATFORM})" must be '"crio" or "atom"'. ; exit 1 )
-[ ${DEBUG} == "yes" -o ${DEBUG} == "no" ] || ( echo Debug "(${DEBUG})" must be '"yes" or "no"'. ; exit 1 )
+[ "${PLATFORM}" == "crio" -o "${PLATFORM}" == "atom" ] || ( echo Platform "(${PLATFORM})" must be '"crio" or "atom"'. ; exit 1 )
+[ "${DEBUG}" == "yes" -o "${DEBUG}" == "no" ] || ( echo Debug "(${DEBUG})" must be '"yes" or "no"'. ; exit 1 )
AOS=`dirname $0`/..
-NINJA_DIR=${AOS}/externals/ninja
+NINJA_RELEASE=v1.4.0
+NINJA_DIR=${AOS}/externals/ninja-${NINJA_RELEASE}
NINJA=${NINJA_DIR}/ninja
# From chromium@154360:trunk/src/DEPS.
-GYP_REVISION=1488
+GYP_REVISION=1738
GYP_DIR=${AOS}/externals/gyp-${GYP_REVISION}
GYP=${GYP_DIR}/gyp
OUTDIR=${AOS}/../out_${PLATFORM}
BUILD_NINJA=${OUTDIR}/Default/build.ninja
-[ -d ${NINJA_DIR} ] || git clone --branch release https://github.com/martine/ninja.git ${NINJA_DIR}
+[ -d ${NINJA_DIR} ] || git clone --branch ${NINJA_RELEASE} https://github.com/martine/ninja.git ${NINJA_DIR}
[ -x ${NINJA} ] || ${NINJA_DIR}/bootstrap.py
[ -d ${GYP_DIR} ] || ( svn co http://gyp.googlecode.com/svn/trunk -r ${GYP_REVISION} ${GYP_DIR} && patch -p1 -d ${GYP_DIR} < ${AOS}/externals/gyp.patch )
${AOS}/build/download_externals.sh
@@ -56,19 +62,20 @@
fi
if [ "${ACTION}" == "clean" ]; then
- rm -r ${OUTDIR}
+ rm -r ${OUTDIR} || true
else
if [ "${ACTION}" != "deploy" -a "${ACTION}" != "tests" -a "${ACTION}" != "redeploy" ]; then
- GYP_ACTION=${ACTION}
+ NINJA_ACTION=${ACTION}
else
- GYP_ACTION=
+ NINJA_ACTION=
fi
- ${NINJA} -C ${OUTDIR}/Default ${GYP_ACTION}
+ ${NINJA} -C ${OUTDIR}/Default ${NINJA_ACTION} "$@"
if [[ ${ACTION} == deploy || ${ACTION} == redeploy ]]; then
[ ${PLATFORM} == atom ] && \
rsync --progress -c -r \
- ${OUTDIR}/Default/outputs/* \
- driver@`${AOS}/build/get_ip fitpc`:/home/driver/robot_code/bin
+ ${OUTDIR}/Default/outputs/* \
+ driver@`${AOS}/build/get_ip fitpc`:/home/driver/robot_code/bin
+ ssh driver@`${AOS}/build/get_ip fitpc` "sync; sync; sync"
[ ${PLATFORM} == crio ] && \
ncftpput `${AOS}/build/get_ip robot` / \
${OUTDIR}/Default/lib/FRC_UserProgram.out
diff --git a/aos/build/crio_cc b/aos/build/crio_cc
new file mode 100755
index 0000000..442d3fe
--- /dev/null
+++ b/aos/build/crio_cc
@@ -0,0 +1,7 @@
+#!/bin/bash
+
+# This is a helper script that gets called as a replacement for gcc. It just
+# passes all arguments on unless it is being called as a shared linker.
+
+[ $1 != '-shared' ] && exec powerpc-wrs-vxworks-gcc "$@"
+exec $(dirname $0)/crio_link_out "$@"
diff --git a/aos/build/crio_cxx b/aos/build/crio_cxx
new file mode 100755
index 0000000..ea68e58
--- /dev/null
+++ b/aos/build/crio_cxx
@@ -0,0 +1,7 @@
+#!/bin/bash
+
+# This is a helper script that gets called as a replacement for g++. It just
+# passes all arguments on unless it is being called as a shared linker.
+
+[ $1 != '-shared' ] && exec powerpc-wrs-vxworks-g++ "$@"
+exec $(dirname $0)/crio_link_out "$@"
diff --git a/aos/build/crio_link_out b/aos/build/crio_link_out
index e5a66e7..0630341 100755
--- a/aos/build/crio_link_out
+++ b/aos/build/crio_link_out
@@ -1,7 +1,11 @@
-#!/bin/bash -e
+#!/bin/bash
+#set -x
-# This is a helper script that compiles .out files for the cRIO. It is designed
-# to be called as a replacement for g++ being used as a linker.
+set -e
+
+# This is a helper script that compiles .out files for the cRIO. It gets called
+# by the gcc and g++ wrapper scripts if they detect that the tool is being used
+# as a shared linker.
# All the flags except -shared.
INPUTS_FLAGS=`echo "$@" | sed 's/-shared//g'`
@@ -9,7 +13,6 @@
OUTPUT=`echo ${INPUTS_FLAGS} | awk \
'BEGIN { RS=" " }; output { print ; output = 0 }; /-o/ { output = 1 }'`
# All arguments that don't start with a - and aren't ${OUTPUT}.
-#INPUTS=`echo ${INPUTS_FLAGS} | sed "s:-[^ ]*::g; s:${OUTPUT}::g;"`
INPUTS=`echo ${INPUTS_FLAGS} | awk \
'BEGIN { RS=" " }; /-Wl,--no-whole-archive/ { output = 0 }; \
output { print }; \
@@ -17,7 +20,8 @@
TEMPDIR=`dirname ${OUTPUT}`
AOS=`dirname $0`/..
powerpc-wrs-vxworks-nm ${INPUTS} | \
- tclsh ${WIND_BASE}/host/resource/hutils/tcl/munch.tcl -c ppc > ${TEMPDIR}/ctdt.c
+ tclsh ${WIND_BASE}/host/resource/hutils/tcl/munch.tcl -c ppc > \
+ ${TEMPDIR}/ctdt.c
powerpc-wrs-vxworks-gcc -I${AOS}/.. -c ${TEMPDIR}/ctdt.c -o ${TEMPDIR}/ctdt.o
powerpc-wrs-vxworks-g++ ${INPUTS_FLAGS} ${TEMPDIR}/ctdt.o
ln -f ${OUTPUT} `echo ${OUTPUT} | sed 's/lib\([A-Za-z0-9_]*\)\.so$/\1.out/'`
diff --git a/aos/build/queues/output/message_dec.rb b/aos/build/queues/output/message_dec.rb
index a643392..777e86b 100644
--- a/aos/build/queues/output/message_dec.rb
+++ b/aos/build/queues/output/message_dec.rb
@@ -35,7 +35,7 @@
format += ", "
format += elem.toPrintFormat()
if (elem.type == 'bool')
- args.push("#{elem.name} ? 't' : 'f'")
+ args.push("#{elem.name} ? 'T' : 'f'")
else
args.push(elem.name)
end
diff --git a/aos/common/logging/logging_impl_test.cc b/aos/common/logging/logging_impl_test.cc
index 16a0285..faaf1cd 100644
--- a/aos/common/logging/logging_impl_test.cc
+++ b/aos/common/logging/logging_impl_test.cc
@@ -125,7 +125,7 @@
expected << "-";
expected << (end_line + 1);
expected << ": ";
- expected << __PRETTY_FUNCTION__;
+ expected << __func__;
expected << ": first part second part (=19) third part last part 5\n";
EXPECT_TRUE(WasLogged(WARNING, expected.str()));
}
diff --git a/aos/externals/.gitignore b/aos/externals/.gitignore
index 80bd460..73cb3e2 100644
--- a/aos/externals/.gitignore
+++ b/aos/externals/.gitignore
@@ -7,13 +7,13 @@
/gccdist/
/gtest-1.6.0-p1/
/gtest-1.6.0.zip
-/gyp-1488/
+/gyp-1738/
/javacv-0.2-bin.zip
/javacv-bin/
/jpeg-8d/
/jpegsrc.v8d.tar.gz
/libjpeg/
-/ninja/
+/ninja-v1.4.0/
/one-jar-boot-0.97.jar
/gflags-2.0-prefix/
/gflags-2.0.tar.gz
diff --git a/aos/externals/gyp.patch b/aos/externals/gyp.patch
index 9019406..b09b67d 100644
--- a/aos/externals/gyp.patch
+++ b/aos/externals/gyp.patch
@@ -1,7 +1,7 @@
diff -rupN before/pylib/gyp/input.py after/pylib/gyp/input.py
--- before/pylib/gyp/input.py 2012-11-20 16:38:09.394784918 -0800
+++ after/pylib/gyp/input.py 2012-11-20 16:39:10.527105964 -0800
-@@ -2156,17 +2156,6 @@ def ValidateSourcesInTarget(target, targ
+@@ -2412,17 +2412,6 @@ def ValidateSourcesInTarget(target, targ
basename = os.path.basename(name) # Don't include extension.
basenames.setdefault(basename, []).append(source)
@@ -11,10 +11,10 @@
- error += ' %s: %s\n' % (basename, ' '.join(files))
-
- if error:
-- print ('static library %s has several files with the same basename:\n' %
-- target + error + 'Some build systems, e.g. MSVC08, '
-- 'cannot handle that.')
-- raise KeyError, 'Duplicate basenames in sources section, see list above'
+- print('static library %s has several files with the same basename:\n' %
+- target + error + 'Some build systems, e.g. MSVC08, '
+- 'cannot handle that.')
+- raise GypError('Duplicate basenames in sources section, see list above')
-
def ValidateRulesInTarget(target, target_dict, extra_sources_for_rules):
diff --git a/frc971/atom_code/atom_code.gyp b/frc971/atom_code/atom_code.gyp
index 0aceec0..9a111b0 100644
--- a/frc971/atom_code/atom_code.gyp
+++ b/frc971/atom_code/atom_code.gyp
@@ -32,7 +32,6 @@
{
'destination': '<(rsync_dir)',
'files': [
- 'scripts/aos_module.ko',
'scripts/start_list.txt',
],
},
diff --git a/frc971/atom_code/build.sh b/frc971/atom_code/build.sh
index d1dff19..b8129ab 100755
--- a/frc971/atom_code/build.sh
+++ b/frc971/atom_code/build.sh
@@ -1,3 +1,3 @@
#!/bin/bash
-../../aos/build/build.sh atom atom_code.gyp no $1
+../../aos/build/build.sh atom atom_code.gyp no "$@"
diff --git a/frc971/atom_code/scripts/aos_module.ko b/frc971/atom_code/scripts/aos_module.ko
deleted file mode 100644
index cdaddb7..0000000
--- a/frc971/atom_code/scripts/aos_module.ko
+++ /dev/null
Binary files differ
diff --git a/frc971/autonomous/auto.cc b/frc971/autonomous/auto.cc
index 424f5ef..e7c48a5 100644
--- a/frc971/autonomous/auto.cc
+++ b/frc971/autonomous/auto.cc
@@ -343,9 +343,9 @@
double WRIST_UP;
const double WRIST_DOWN = -0.580;
- const double WRIST_DOWN_TWO = WRIST_DOWN - 0.005;
- const double ANGLE_ONE = 0.520;
- const double ANGLE_TWO = 0.677;
+ const double WRIST_DOWN_TWO = WRIST_DOWN - 0.012;
+ const double ANGLE_ONE = 0.509;
+ const double ANGLE_TWO = 0.673;
ResetIndex();
SetWristGoal(1.0); // wrist must calibrate itself on power-up
@@ -440,12 +440,24 @@
if (ShouldExitAuto()) return;
WaitForIndex(); // ready to pick up discs
+ // How long we're going to drive in total.
static const double kDriveDistance = 2.8;
- static const double kFirstDrive = 0.27;
+ // How long to drive slowly to pick up the 2 disks under the pyramid.
+ static const double kFirstDrive = 0.4;
+ // How long to drive slowly to pick up the last 2 disks.
+ static const double kLastDrive = 0.3;
+ // How fast to drive when picking up disks.
+ static const double kPickupVelocity = 0.6;
+ // Where to take the second set of shots from.
static const double kSecondShootDistance = 2.0;
- SetDriveGoal(kFirstDrive, 0.6);
+
+ // Go slowly to grab the 2 disks under the pyramid.
+ SetDriveGoal(kFirstDrive, kPickupVelocity);
+ ::aos::time::SleepFor(::aos::time::Time::InSeconds(0.1));
+ SetDriveGoal(kDriveDistance - kFirstDrive - kLastDrive, 2.0);
SetWristGoal(WRIST_DOWN_TWO);
- SetDriveGoal(kDriveDistance - kFirstDrive, 2.0);
+ // Go slowly at the end to pick up the last 2 disks.
+ SetDriveGoal(kLastDrive, kPickupVelocity);
if (ShouldExitAuto()) return;
::aos::time::SleepFor(::aos::time::Time::InSeconds(0.5));
diff --git a/frc971/constants.cpp b/frc971/constants.cpp
index 147cb6e..df822e2 100644
--- a/frc971/constants.cpp
+++ b/frc971/constants.cpp
@@ -17,8 +17,10 @@
namespace {
// It has about 0.029043 of gearbox slop.
+// For purposes of moving the end up/down by a certain amount, the wrist is 18
+// inches long.
const double kCompWristHallEffectStartAngle = 1.285;
-const double kPracticeWristHallEffectStartAngle = 1.171;
+const double kPracticeWristHallEffectStartAngle = 1.168;
const double kCompWristHallEffectStopAngle = 100 * M_PI / 180.0;
const double kPracticeWristHallEffectStopAngle = 100 * M_PI / 180.0;
@@ -70,6 +72,9 @@
const int kCompCameraCenter = -2;
const int kPracticeCameraCenter = -5;
+const int kCompDrivetrainGearboxPinion = 19;
+const int kPracticeDrivetrainGearboxPinion = 17;
+
struct Values {
// Wrist hall effect positive and negative edges.
double wrist_hall_effect_start_angle;
@@ -107,6 +112,8 @@
// Deadband voltage.
double angle_adjust_deadband;
+ int drivetrain_gearbox_pinion;
+
// what camera_center returns
int camera_center;
};
@@ -139,6 +146,7 @@
kAngleAdjustZeroingSpeed,
kAngleAdjustZeroingOffSpeed,
kCompAngleAdjustDeadband,
+ kCompDrivetrainGearboxPinion,
kCompCameraCenter};
break;
case kPracticeTeamNumber:
@@ -159,6 +167,7 @@
kAngleAdjustZeroingSpeed,
kAngleAdjustZeroingOffSpeed,
kPracticeAngleAdjustDeadband,
+ kPracticeDrivetrainGearboxPinion,
kPracticeCameraCenter};
break;
default:
@@ -289,6 +298,13 @@
return true;
}
+bool drivetrain_gearbox_pinion(int *pinion) {
+ const Values *const values = GetValues();
+ if (values == NULL) return false;
+ *pinion = values->drivetrain_gearbox_pinion;
+ return true;
+}
+
bool camera_center(int *center) {
const Values *const values = GetValues();
if (values == NULL) return false;
diff --git a/frc971/constants.h b/frc971/constants.h
index 6a14e87..414d10d 100644
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -10,8 +10,8 @@
// store their output value into and assume that aos::robot_state->get() is
// not null and is correct. They return true on success.
-const uint16_t kCompTeamNumber = 971;
-const uint16_t kPracticeTeamNumber = 5971;
+const uint16_t kCompTeamNumber = 5971;
+const uint16_t kPracticeTeamNumber = 971;
// Sets *angle to how many radians from horizontal to the location of interest.
bool wrist_hall_effect_start_angle(double *angle);
@@ -43,6 +43,10 @@
// Returns the deadband voltage to use.
bool angle_adjust_deadband(double *voltage);
+// Sets *pinion to the number of teeth on the pinion that drives the drivetrain
+// wheels.
+bool drivetrain_gearbox_pinion(int *pinion);
+
// Sets *center to how many pixels off center the vertical line
// on the camera view is.
bool camera_center(int *center);
diff --git a/frc971/control_loops/index/index.cc b/frc971/control_loops/index/index.cc
index f8dbada..779e6a9 100644
--- a/frc971/control_loops/index/index.cc
+++ b/frc971/control_loops/index/index.cc
@@ -54,8 +54,10 @@
loader_up_(false),
disc_clamped_(false),
disc_ejected_(false),
+ is_shooting_(false),
last_bottom_disc_detect_(false),
last_top_disc_detect_(false),
+ hopper_clear_(true),
no_prior_position_(true),
missing_position_count_(0) {
}
@@ -106,9 +108,11 @@
const /*static*/ double IndexMotor::kTransferRollerRadius = 1.25 * 0.0254 / 2;
/*static*/ const int IndexMotor::kGrabbingDelay = 5;
-/*static*/ const int IndexMotor::kLiftingDelay = 30;
-/*static*/ const int IndexMotor::kShootingDelay = 5;
-/*static*/ const int IndexMotor::kLoweringDelay = 20;
+/*static*/ const int IndexMotor::kLiftingDelay = 2;
+/*static*/ const int IndexMotor::kLiftingTimeout = 100;
+/*static*/ const int IndexMotor::kShootingDelay = 10;
+/*static*/ const int IndexMotor::kLoweringDelay = 4;
+/*static*/ const int IndexMotor::kLoweringTimeout = 120;
// TODO(aschuh): Tune these.
/*static*/ const double
@@ -360,7 +364,7 @@
}
if (position->top_disc_posedge_count != last_top_disc_posedge_count_) {
- LOG(INFO, "Saw a posedge\n");
+ LOG(INFO, "Saw a top posedge\n");
const double index_position = wrist_loop_->X_hat(0, 0) -
position->index_position + position->top_disc_posedge_position;
// TODO(aschuh): Sanity check this number...
@@ -532,6 +536,7 @@
break;
case Goal::READY_LOWER:
case Goal::INTAKE:
+ hopper_clear_ = false;
{
if (position) {
// Posedge of the disc entering the beam break.
@@ -688,6 +693,7 @@
if (loader_state_ == LoaderState::GRABBED &&
safe_goal_ == Goal::SHOOT) {
loader_goal_ = LoaderGoal::SHOOT_AND_RESET;
+ is_shooting_ = true;
}
// Must wait until it has been grabbed to continue.
@@ -756,6 +762,7 @@
"Emptied the hopper out but there are still discs there\n");
hopper_disc_count_ = 0;
}
+ hopper_clear_ = true;
}
}
}
@@ -788,6 +795,7 @@
hopper_disc_count_);
hopper_disc_count_ = 0;
}
+ hopper_clear_ = true;
}
}
@@ -831,6 +839,7 @@
loader_up_ = false;
disc_clamped_ = false;
disc_ejected_ = false;
+ disk_stuck_in_loader_ = false;
if (loader_goal_ == LoaderGoal::GRAB ||
loader_goal_ == LoaderGoal::SHOOT_AND_RESET || goal->force_fire) {
if (goal->force_fire) {
@@ -864,13 +873,13 @@
disc_clamped_ = true;
disc_ejected_ = false;
if (loader_goal_ == LoaderGoal::SHOOT_AND_RESET || goal->force_fire) {
- shooter.status.FetchLatest();
- if (shooter.status.get()) {
+ if (shooter.status.FetchLatest() || shooter.status.get()) {
// TODO(aschuh): If we aren't shooting nicely, wait until the shooter
// is up to speed rather than just spinning.
if (shooter.status->average_velocity > 130 && shooter.status->ready) {
loader_state_ = LoaderState::LIFTING;
loader_countdown_ = kLiftingDelay;
+ loader_timeout_ = 0;
LOG(INFO, "Told to SHOOT_AND_RESET, moving on\n");
} else {
LOG(WARNING, "Told to SHOOT_AND_RESET, shooter too slow at %f\n",
@@ -881,6 +890,7 @@
LOG(ERROR, "Told to SHOOT_AND_RESET, no shooter data, moving on.\n");
loader_state_ = LoaderState::LIFTING;
loader_countdown_ = kLiftingDelay;
+ loader_timeout_ = 0;
}
} else if (loader_goal_ == LoaderGoal::READY) {
LOG(ERROR, "Can't go to ready when we have something grabbed.\n");
@@ -894,11 +904,25 @@
loader_up_ = true;
disc_clamped_ = true;
disc_ejected_ = false;
- if (loader_countdown_ > 0) {
- --loader_countdown_;
- break;
+ if (position->loader_top) {
+ if (loader_countdown_ > 0) {
+ --loader_countdown_;
+ loader_timeout_ = 0;
+ break;
+ } else {
+ loader_state_ = LoaderState::LIFTED;
+ }
} else {
- loader_state_ = LoaderState::LIFTED;
+ // Restart the countdown if it bounces back down or whatever.
+ loader_countdown_ = kLiftingDelay;
+ ++loader_timeout_;
+ if (loader_timeout_ > kLiftingTimeout) {
+ LOG(ERROR, "Loader timeout while LIFTING %d\n", loader_timeout_);
+ loader_state_ = LoaderState::LIFTED;
+ disk_stuck_in_loader_ = true;
+ } else {
+ break;
+ }
}
case LoaderState::LIFTED:
LOG(DEBUG, "Loader LIFTED\n");
@@ -928,6 +952,7 @@
disc_ejected_ = true;
loader_state_ = LoaderState::LOWERING;
loader_countdown_ = kLoweringDelay;
+ loader_timeout_ = 0;
--hopper_disc_count_;
++shot_disc_count_;
case LoaderState::LOWERING:
@@ -936,22 +961,42 @@
loader_up_ = false;
disc_clamped_ = false;
disc_ejected_ = true;
- if (loader_countdown_ > 0) {
- --loader_countdown_;
- break;
+ if (position->loader_bottom) {
+ if (loader_countdown_ > 0) {
+ --loader_countdown_;
+ loader_timeout_ = 0;
+ break;
+ } else {
+ loader_state_ = LoaderState::LOWERED;
+ }
} else {
- loader_state_ = LoaderState::LOWERED;
+ // Restart the countdown if it bounces back up or something.
+ loader_countdown_ = kLoweringDelay;
+ ++loader_timeout_;
+ if (loader_timeout_ > kLoweringTimeout) {
+ LOG(ERROR, "Loader timeout while LOWERING %d\n", loader_timeout_);
+ loader_state_ = LoaderState::LOWERED;
+ disk_stuck_in_loader_ = true;
+ } else {
+ break;
+ }
}
case LoaderState::LOWERED:
LOG(DEBUG, "Loader LOWERED\n");
- // The indexer is lowered.
loader_up_ = false;
- disc_clamped_ = false;
disc_ejected_ = false;
- loader_state_ = LoaderState::READY;
- // Once we have shot, we need to hang out in READY until otherwise
- // notified.
- loader_goal_ = LoaderGoal::READY;
+ is_shooting_ = false;
+ if (disk_stuck_in_loader_) {
+ disk_stuck_in_loader_ = false;
+ disc_clamped_ = true;
+ loader_state_ = LoaderState::GRABBED;
+ } else {
+ disc_clamped_ = false;
+ loader_state_ = LoaderState::READY;
+ // Once we have shot, we need to hang out in READY until otherwise
+ // notified.
+ loader_goal_ = LoaderGoal::READY;
+ }
break;
}
@@ -994,6 +1039,8 @@
status->total_disc_count = total_disc_count_;
status->shot_disc_count = shot_disc_count_;
status->preloaded = (loader_state_ != LoaderState::READY);
+ status->is_shooting = is_shooting_;
+ status->hopper_clear = hopper_clear_;
if (output) {
output->intake_voltage = intake_voltage;
diff --git a/frc971/control_loops/index/index.h b/frc971/control_loops/index/index.h
index a18d4c9..0be4801 100644
--- a/frc971/control_loops/index/index.h
+++ b/frc971/control_loops/index/index.h
@@ -136,12 +136,19 @@
// Time that it takes to grab the disc in cycles.
static const int kGrabbingDelay;
- // Time that it takes to lift the loader in cycles.
+ // Time that it takes to finish lifting the loader after the sensor is
+ // triggered in cycles.
static const int kLiftingDelay;
+ // Time until we give up lifting and move on in cycles.
+ static const int kLiftingTimeout;
// Time that it takes to shoot the disc in cycles.
static const int kShootingDelay;
- // Time that it takes to lower the loader in cycles.
+ // Time that it takes to finish lowering the loader after the sensor is
+ // triggered in cycles.
static const int kLoweringDelay;
+ // Time until we give up lowering and move on in cycles.
+ // It's a long time because we really don't want to ever hit this.
+ static const int kLoweringTimeout;
// Object representing a Frisbee tracked by the indexer.
class Frisbee {
@@ -309,19 +316,25 @@
// Loader goal, state, and counter.
LoaderGoal loader_goal_;
LoaderState loader_state_;
- int loader_countdown_;
+ int loader_countdown_, loader_timeout_;
+ // Whether or not we (might have) failed to shoot a disk that's now (probably)
+ // still in the loader.
+ bool disk_stuck_in_loader_;
// Current state of the pistons.
bool loader_up_;
bool disc_clamped_;
bool disc_ejected_;
+ bool is_shooting_;
+
// The frisbee that is flying through the transfer rollers.
Frisbee transfer_frisbee_;
// Bottom disc detect from the last valid packet for detecting edges.
bool last_bottom_disc_detect_;
bool last_top_disc_detect_;
+ bool hopper_clear_;
int32_t last_bottom_disc_posedge_count_;
int32_t last_bottom_disc_negedge_count_;
int32_t last_bottom_disc_negedge_wait_count_;
diff --git a/frc971/control_loops/index/index_motor.q b/frc971/control_loops/index/index_motor.q
index c14d459..b299b42 100644
--- a/frc971/control_loops/index/index_motor.q
+++ b/frc971/control_loops/index/index_motor.q
@@ -44,6 +44,11 @@
// and a count of how many edges have been seen.
int32_t top_disc_negedge_count;
double top_disc_negedge_position;
+
+ // Whether the hall effects for the loader are triggered (have a magnet in
+ // front of them).
+ bool loader_top;
+ bool loader_bottom;
};
message Output {
@@ -72,6 +77,12 @@
bool preloaded;
// Indexer ready to accept more discs.
bool ready_to_intake;
+ // True from when we're committed to shooting util after the disk is clear
+ // of the robot.
+ bool is_shooting;
+ // Goes false when we first get a disk and back true after we finish
+ // clearing.
+ bool hopper_clear;
};
queue Goal goal;
diff --git a/frc971/control_loops/shooter/shooter.cc b/frc971/control_loops/shooter/shooter.cc
index 981df5d..035880e 100644
--- a/frc971/control_loops/shooter/shooter.cc
+++ b/frc971/control_loops/shooter/shooter.cc
@@ -4,6 +4,7 @@
#include "aos/common/logging/logging.h"
#include "frc971/control_loops/shooter/shooter_motor_plant.h"
+#include "frc971/control_loops/index/index_motor.q.h"
namespace frc971 {
namespace control_loops {
@@ -13,7 +14,8 @@
loop_(new StateFeedbackLoop<2, 1, 1>(MakeShooterLoop())),
history_position_(0),
position_goal_(0.0),
- last_position_(0.0) {
+ last_position_(0.0),
+ last_velocity_goal_(0) {
memset(history_, 0, sizeof(history_));
}
@@ -26,11 +28,23 @@
const control_loops::ShooterLoop::Position *position,
::aos::control_loops::Output *output,
control_loops::ShooterLoop::Status *status) {
- const double velocity_goal = std::min(goal->velocity, kMaxSpeed);
+ double velocity_goal = std::min(goal->velocity, kMaxSpeed);
const double current_position =
(position == NULL ? loop_->X_hat(0, 0) : position->position);
double output_voltage = 0.0;
+ if (index_loop.status.FetchLatest() || index_loop.status.get()) {
+ if (index_loop.status->is_shooting) {
+ if (velocity_goal != last_velocity_goal_ &&
+ velocity_goal < 130) {
+ velocity_goal = last_velocity_goal_;
+ }
+ }
+ } else {
+ LOG(WARNING, "assuming index isn't shooting\n");
+ }
+ last_velocity_goal_ = velocity_goal;
+
// Track the current position if the velocity goal is small.
if (velocity_goal <= 1.0) {
position_goal_ = current_position;
diff --git a/frc971/control_loops/shooter/shooter.gyp b/frc971/control_loops/shooter/shooter.gyp
index b1371a4..41c33c1 100644
--- a/frc971/control_loops/shooter/shooter.gyp
+++ b/frc971/control_loops/shooter/shooter.gyp
@@ -29,6 +29,7 @@
'<(AOS)/common/common.gyp:controls',
'<(DEPTH)/frc971/frc971.gyp:common',
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
+ '<(DEPTH)/frc971/control_loops/index/index.gyp:index_loop',
],
'export_dependent_settings': [
'<(DEPTH)/frc971/control_loops/control_loops.gyp:state_feedback_loop',
diff --git a/frc971/control_loops/shooter/shooter.h b/frc971/control_loops/shooter/shooter.h
index 77c605b..7947f7a 100644
--- a/frc971/control_loops/shooter/shooter.h
+++ b/frc971/control_loops/shooter/shooter.h
@@ -45,6 +45,9 @@
double position_goal_;
double last_position_;
+ // For making sure it keeps spinning if we're shooting.
+ double last_velocity_goal_;
+
DISALLOW_COPY_AND_ASSIGN(ShooterMotor);
};
diff --git a/frc971/control_loops/wrist/wrist.cc b/frc971/control_loops/wrist/wrist.cc
index f472d55..0ca5caa 100644
--- a/frc971/control_loops/wrist/wrist.cc
+++ b/frc971/control_loops/wrist/wrist.cc
@@ -21,6 +21,12 @@
bool WristMotor::FetchConstants(
ZeroedJoint<1>::ConfigurationData *config_data) {
+ if (::aos::robot_state.get() == NULL) {
+ if (!::aos::robot_state.FetchNext()) {
+ LOG(ERROR, "Failed to fetch robot state to get constants.\n");
+ return false;
+ }
+ }
if (!constants::wrist_lower_limit(&config_data->lower_limit)) {
LOG(ERROR, "Failed to fetch the wrist lower limit constant.\n");
return false;
diff --git a/frc971/crio/build.sh b/frc971/crio/build.sh
index ec04978..b3fc031 100755
--- a/frc971/crio/build.sh
+++ b/frc971/crio/build.sh
@@ -1,3 +1,3 @@
#!/bin/bash
-../../aos/build/build.sh crio crio.gyp no $1
+../../aos/build/build.sh crio crio.gyp no "$@"
diff --git a/frc971/input/JoystickReader.cc b/frc971/input/JoystickReader.cc
index 31f0385..d9db969 100644
--- a/frc971/input/JoystickReader.cc
+++ b/frc971/input/JoystickReader.cc
@@ -65,6 +65,7 @@
virtual void RunIteration(const ::aos::input::driver_station::Data &data) {
static bool is_high_gear = false;
+ static double angle_adjust_goal = 0.42;
if (data.GetControlBit(ControlBit::kAutonomous)) {
if (data.PosEdge(ControlBit::kEnabled)){
@@ -136,6 +137,10 @@
is_high_gear = true;
}
+ // Whether we should change wrist positions to indicate that the hopper is
+ // clear.
+ bool hopper_clear = false;
+
// Where the wrist should be to pick up a frisbee.
// TODO(brians): Make these globally accessible and clean up auto.
static const double kWristPickup = -0.580;
@@ -143,15 +148,26 @@
// Where the wrist gets stored when up.
// All the way up is 1.5.
static const double kWristUp = 1.43;
+ static const double kWristCleared = kWristUp - 0.2;
static double wrist_down_position = kWristPickup;
double wrist_up_position = kWristUp;
+ double wrist_pickup_position = data.IsPressed(kIntake) ?
+ kWristPickup : kWristNearGround;
+ if (index_loop.status.FetchLatest() || index_loop.status.get()) {
+ if (index_loop.status->hopper_disc_count >= 4) {
+ wrist_down_position = kWristNearGround;
+ } else {
+ wrist_down_position = wrist_pickup_position;
+ }
+ hopper_clear = index_loop.status->hopper_clear;
+ }
::aos::ScopedMessagePtr<control_loops::ShooterLoop::Goal> shooter_goal =
shooter.goal.MakeMessage();
shooter_goal->velocity = 0;
- static double angle_adjust_goal = 0.42;
if (data.IsPressed(kPitShot1) && data.IsPressed(kPitShot2)) {
shooter_goal->velocity = 131;
+ if (hopper_clear) wrist_up_position = kWristCleared;
angle_adjust_goal = 0.70;
} else if (data.IsPressed(kLongShot)) {
#if 0
@@ -167,7 +183,7 @@
}
#endif
shooter_goal->velocity = 360;
- wrist_up_position = 1.23 - 0.4;
+ if (!hopper_clear) wrist_up_position = 1.23 - 0.4;
angle_adjust_goal = 0.596;
} else if (data.IsPressed(kMediumShot)) {
#if 0
@@ -177,24 +193,15 @@
#endif
// middle wheel on the back line (same as auto)
shooter_goal->velocity = 395;
- wrist_up_position = 1.23 - 0.4;
+ if (!hopper_clear) wrist_up_position = 1.23 - 0.4;
angle_adjust_goal = 0.520;
} else if (data.IsPressed(kShortShot)) {
shooter_goal->velocity = 375;
- angle_adjust_goal = 0.7267;
+ if (hopper_clear) wrist_up_position = kWristCleared;
+ angle_adjust_goal = 0.671;
}
angle_adjust.goal.MakeWithBuilder().goal(angle_adjust_goal).Send();
- double wrist_pickup_position = data.IsPressed(kIntake) ?
- kWristPickup : kWristNearGround;
- index_loop.status.FetchLatest();
- if (index_loop.status.get()) {
- if (index_loop.status->hopper_disc_count >= 4) {
- wrist_down_position = kWristNearGround;
- } else {
- wrist_down_position = wrist_pickup_position;
- }
- }
wrist.goal.MakeWithBuilder()
.goal(data.IsPressed(kWristDown) ?
wrist_down_position :
@@ -236,6 +243,7 @@
static int hanger_cycles = 0;
if (data.IsPressed(kDeployHangers)) {
++hanger_cycles;
+ angle_adjust_goal = 0.4;
} else {
hanger_cycles = 0;
}
diff --git a/frc971/input/gyro_board_reader.cc b/frc971/input/gyro_board_reader.cc
index e1e08d7..438fc30 100644
--- a/frc971/input/gyro_board_reader.cc
+++ b/frc971/input/gyro_board_reader.cc
@@ -15,6 +15,8 @@
#include "frc971/input/gyro_board_data.h"
#include "gyro_board/src/libusb-driver/libusb_wrap.h"
#include "frc971/queues/GyroAngle.q.h"
+#include "frc971/constants.h"
+#include "aos/common/messages/RobotState.q.h"
#ifndef M_PI
#define M_PI 3.14159265358979323846
@@ -31,8 +33,17 @@
namespace {
inline double drivetrain_translate(int32_t in) {
+ int pinion_size;
+ if (::aos::robot_state.get() == NULL) {
+ if (!::aos::robot_state.FetchNext()) {
+ LOG(WARNING, "couldn't fetch robot state\n");
+ return 0;
+ }
+ }
+ if (!constants::drivetrain_gearbox_pinion(&pinion_size)) return 0;
return static_cast<double>(in) / (256.0 /*cpr*/ * 4.0 /*quad*/) *
- (19.0 / 50.0) /*output reduction*/ * (64.0 / 24.0) /*encoder gears*/ *
+ (pinion_size / 50.0) /*output reduction*/ *
+ (64.0 / 24.0) /*encoder gears*/ *
(3.5 /*wheel diameter*/ * 2.54 / 100.0 * M_PI);
}
@@ -230,6 +241,8 @@
.bottom_disc_negedge_wait_position(index_translate(
data->main.capture_bottom_fall_delay))
.bottom_disc_negedge_wait_count(bottom_fall_delay_count_)
+ .loader_top(data->loader_top)
+ .loader_bottom(data->loader_bottom)
.Send();
}
diff --git a/frc971/input/gyro_sensor_receiver.cc b/frc971/input/gyro_sensor_receiver.cc
index 1e5343e..a0f5693 100644
--- a/frc971/input/gyro_sensor_receiver.cc
+++ b/frc971/input/gyro_sensor_receiver.cc
@@ -153,6 +153,8 @@
.bottom_disc_negedge_wait_position(index_translate(
data->main.capture_bottom_fall_delay))
.bottom_disc_negedge_wait_count(bottom_fall_delay_count_)
+ .loader_top(data->loader_top)
+ .loader_bottom(data->loader_bottom)
.Send();
}
diff --git a/frc971/input/input.gyp b/frc971/input/input.gyp
index 5e08a20..1758e95 100644
--- a/frc971/input/input.gyp
+++ b/frc971/input/input.gyp
@@ -61,6 +61,8 @@
'<(AOS)/build/aos.gyp:logging',
'<(AOS)/common/common.gyp:time',
'<(AOS)/common/common.gyp:timing',
+ '<(DEPTH)/frc971/frc971.gyp:common',
+ '<(AOS)/common/messages/messages.gyp:aos_queues',
],
},
{
diff --git a/gyro_board/src/usb/Makefile b/gyro_board/src/usb/Makefile
index c6050d6..424df84 100644
--- a/gyro_board/src/usb/Makefile
+++ b/gyro_board/src/usb/Makefile
@@ -61,7 +61,13 @@
$(FLASHER) -termonly -control $(NAME).hex $(PORT) $(SPEED) $(OSC)
deploy: all $(NAME).hex
- $(FLASHER) -hex -verify -control $(NAME).hex $(PORT) $(SPEED) $(OSC)
+ # TODO(aschuh): Figure out why the verify fails (or remove it) and then
+ # remove the -.
+ -$(FLASHER) -hex -verify -control $(NAME).hex $(PORT) $(SPEED) $(OSC)
+
+reset: deploy
+ # Echo an ESC into it to immediately exit the terminal.
+ `which echo` -e '\e' | $(FLASHER) -termonly -control $(PORT) $(SPEED) $(OSC)
cat:
@cd ../../bin; python serial_looper.py
diff --git a/gyro_board/src/usb/data_struct.h b/gyro_board/src/usb/data_struct.h
index ac86a1a..edccea3 100644
--- a/gyro_board/src/usb/data_struct.h
+++ b/gyro_board/src/usb/data_struct.h
@@ -41,6 +41,8 @@
uint8_t angle_adjust_bottom_hall_effect : 1;
uint8_t top_disc : 1;
uint8_t bottom_disc : 1;
+ uint8_t loader_top : 1;
+ uint8_t loader_bottom : 1;
};
uint16_t booleans;
};
diff --git a/gyro_board/src/usb/encoder.c b/gyro_board/src/usb/encoder.c
index ff0a282..136cefd 100644
--- a/gyro_board/src/usb/encoder.c
+++ b/gyro_board/src/usb/encoder.c
@@ -42,7 +42,7 @@
// ENC1A 2.11
void EINT1_IRQHandler(void) {
- // TODO(brians): figure out why this has to be up here too
+ SC->EXTPOLAR ^= 0x2;
SC->EXTINT = 0x2;
int fiopin = GPIO2->FIOPIN;
if (((fiopin >> 1) ^ fiopin) & 0x800) {
@@ -50,11 +50,10 @@
} else {
--encoder1_val;
}
- SC->EXTPOLAR ^= 0x2;
- SC->EXTINT = 0x2;
}
// ENC1B 2.12
void EINT2_IRQHandler(void) {
+ SC->EXTPOLAR ^= 0x4;
SC->EXTINT = 0x4;
int fiopin = GPIO2->FIOPIN;
if (((fiopin >> 1) ^ fiopin) & 0x800) {
@@ -62,38 +61,37 @@
} else {
++encoder1_val;
}
- SC->EXTPOLAR ^= 0x4;
- SC->EXTINT = 0x4;
}
-// GPIO Interrupt handlers
-static void NoGPIO() {}
-static void Encoder2ARise() {
- GPIOINT->IO0IntClr |= (1 << 22);
+// TODO(brians): Have this indicate some kind of error instead of just looping
+// infinitely in the ISR because it never clears it.
+static void NoGPIO(void) {}
+static void Encoder2ARise(void) {
+ GPIOINT->IO0IntClr = (1 << 22);
if (GPIO0->FIOPIN & (1 << 21)) {
++encoder2_val;
} else {
--encoder2_val;
}
}
-static void Encoder2AFall() {
- GPIOINT->IO0IntClr |= (1 << 22);
+static void Encoder2AFall(void) {
+ GPIOINT->IO0IntClr = (1 << 22);
if (GPIO0->FIOPIN & (1 << 21)) {
--encoder2_val;
} else {
++encoder2_val;
}
}
-static void Encoder2BRise() {
- GPIOINT->IO0IntClr |= (1 << 21);
+static void Encoder2BRise(void) {
+ GPIOINT->IO0IntClr = (1 << 21);
if (GPIO0->FIOPIN & (1 << 22)) {
--encoder2_val;
} else {
++encoder2_val;
}
}
-static void Encoder2BFall() {
- GPIOINT->IO0IntClr |= (1 << 21);
+static void Encoder2BFall(void) {
+ GPIOINT->IO0IntClr = (1 << 21);
if (GPIO0->FIOPIN & (1 << 22)) {
++encoder2_val;
} else {
@@ -101,32 +99,32 @@
}
}
-static void Encoder3ARise() {
- GPIOINT->IO0IntClr |= (1 << 20);
+static void Encoder3ARise(void) {
+ GPIOINT->IO0IntClr = (1 << 20);
if (GPIO0->FIOPIN & (1 << 19)) {
++encoder3_val;
} else {
--encoder3_val;
}
}
-static void Encoder3AFall() {
- GPIOINT->IO0IntClr |= (1 << 20);
+static void Encoder3AFall(void) {
+ GPIOINT->IO0IntClr = (1 << 20);
if (GPIO0->FIOPIN & (1 << 19)) {
--encoder3_val;
} else {
++encoder3_val;
}
}
-static void Encoder3BRise() {
- GPIOINT->IO0IntClr |= (1 << 19);
+static void Encoder3BRise(void) {
+ GPIOINT->IO0IntClr = (1 << 19);
if (GPIO0->FIOPIN & (1 << 20)) {
--encoder3_val;
} else {
++encoder3_val;
}
}
-static void Encoder3BFall() {
- GPIOINT->IO0IntClr |= (1 << 19);
+static void Encoder3BFall(void) {
+ GPIOINT->IO0IntClr = (1 << 19);
if (GPIO0->FIOPIN & (1 << 20)) {
++encoder3_val;
} else {
@@ -134,32 +132,32 @@
}
}
-static void Encoder4ARise() {
- GPIOINT->IO2IntClr |= (1 << 0);
+static void Encoder4ARise(void) {
+ GPIOINT->IO2IntClr = (1 << 0);
if (GPIO2->FIOPIN & (1 << 1)) {
++encoder4_val;
} else {
--encoder4_val;
}
}
-static void Encoder4AFall() {
- GPIOINT->IO2IntClr |= (1 << 0);
+static void Encoder4AFall(void) {
+ GPIOINT->IO2IntClr = (1 << 0);
if (GPIO2->FIOPIN & (1 << 1)) {
--encoder4_val;
} else {
++encoder4_val;
}
}
-static void Encoder4BRise() {
- GPIOINT->IO2IntClr |= (1 << 1);
+static void Encoder4BRise(void) {
+ GPIOINT->IO2IntClr = (1 << 1);
if (GPIO2->FIOPIN & (1 << 0)) {
--encoder4_val;
} else {
++encoder4_val;
}
}
-static void Encoder4BFall() {
- GPIOINT->IO2IntClr |= (1 << 1);
+static void Encoder4BFall(void) {
+ GPIOINT->IO2IntClr = (1 << 1);
if (GPIO2->FIOPIN & (1 << 0)) {
++encoder4_val;
} else {
@@ -167,32 +165,32 @@
}
}
-static void Encoder5ARise() {
- GPIOINT->IO2IntClr |= (1 << 2);
+static void Encoder5ARise(void) {
+ GPIOINT->IO2IntClr = (1 << 2);
if (GPIO2->FIOPIN & (1 << 3)) {
++encoder5_val;
} else {
--encoder5_val;
}
}
-static void Encoder5AFall() {
- GPIOINT->IO2IntClr |= (1 << 2);
+static void Encoder5AFall(void) {
+ GPIOINT->IO2IntClr = (1 << 2);
if (GPIO2->FIOPIN & (1 << 3)) {
--encoder5_val;
} else {
++encoder5_val;
}
}
-static void Encoder5BRise() {
- GPIOINT->IO2IntClr |= (1 << 3);
+static void Encoder5BRise(void) {
+ GPIOINT->IO2IntClr = (1 << 3);
if (GPIO2->FIOPIN & (1 << 2)) {
--encoder5_val;
} else {
++encoder5_val;
}
}
-static void Encoder5BFall() {
- GPIOINT->IO2IntClr |= (1 << 3);
+static void Encoder5BFall(void) {
+ GPIOINT->IO2IntClr = (1 << 3);
if (GPIO2->FIOPIN & (1 << 2)) {
++encoder5_val;
} else {
@@ -202,23 +200,23 @@
volatile int32_t capture_top_rise;
volatile int8_t top_rise_count;
-static void IndexerTopRise() {
- GPIOINT->IO0IntClr |= (1 << 5);
+static void IndexerTopRise(void) {
+ GPIOINT->IO0IntClr = (1 << 5);
// edge counting encoder capture
++top_rise_count;
capture_top_rise = encoder3_val;
}
volatile int32_t capture_top_fall;
volatile int8_t top_fall_count;
-static void IndexerTopFall() {
- GPIOINT->IO0IntClr |= (1 << 5);
+static void IndexerTopFall(void) {
+ GPIOINT->IO0IntClr = (1 << 5);
// edge counting encoder capture
++top_fall_count;
capture_top_fall = encoder3_val;
}
volatile int8_t bottom_rise_count;
-static void IndexerBottomRise() {
- GPIOINT->IO0IntClr |= (1 << 4);
+static void IndexerBottomRise(void) {
+ GPIOINT->IO0IntClr = (1 << 4);
// edge counting
++bottom_rise_count;
}
@@ -239,10 +237,10 @@
vTaskDelayUntil(&xSleepFrom, kBottomFallDelayTime / portTICK_RATE_MS);
- NVIC_DisableIRQ(EINT3_IRQn);
- ++bottom_fall_delay_count;
+ NVIC_DisableIRQ(USB_IRQn);
capture_bottom_fall_delay = encoder3_val;
- NVIC_EnableIRQ(EINT3_IRQn);
+ ++bottom_fall_delay_count;
+ NVIC_EnableIRQ(USB_IRQn);
} else {
NVIC_EnableIRQ(EINT3_IRQn);
vTaskDelayUntil(&xSleepFrom, 10 / portTICK_RATE_MS);
@@ -251,8 +249,8 @@
}
volatile int8_t bottom_fall_count;
-static void IndexerBottomFall() {
- GPIOINT->IO0IntClr |= (1 << 4);
+static void IndexerBottomFall(void) {
+ GPIOINT->IO0IntClr = (1 << 4);
++bottom_fall_count;
// edge counting start delayed capture
xDelayTimeFrom = xTaskGetTickCount();
@@ -260,16 +258,16 @@
}
volatile int32_t capture_wrist_rise;
volatile int8_t wrist_rise_count;
-static void WristHallRise() {
- GPIOINT->IO0IntClr |= (1 << 6);
+static void WristHallRise(void) {
+ GPIOINT->IO0IntClr = (1 << 6);
// edge counting encoder capture
++wrist_rise_count;
capture_wrist_rise = (int32_t)QEI->QEIPOS;
}
volatile int32_t capture_shooter_angle_rise;
volatile int8_t shooter_angle_rise_count;
-static void ShooterHallRise() {
- GPIOINT->IO0IntClr |= (1 << 7);
+static void ShooterHallRise(void) {
+ GPIOINT->IO0IntClr = (1 << 7);
// edge counting encoder capture
++shooter_angle_rise_count;
capture_shooter_angle_rise = encoder2_val;
@@ -327,12 +325,7 @@
table[index]();
}
void EINT3_IRQHandler(void) {
- // Have to disable it here or else it re-fires the interrupt while the code
- // reads to figure out which pin the interrupt is for.
- // TODO(brians): figure out details + look for an alternative
- NVIC_DisableIRQ(EINT3_IRQn);
IRQ_Dispatch();
- NVIC_EnableIRQ(EINT3_IRQn);
}
int32_t encoder_val(int chan) {
int32_t val;
@@ -476,8 +469,8 @@
packet->robot_id = 0;
packet->main.shooter = encoder1_val;
- packet->main.left_drive = encoder4_val;
- packet->main.right_drive = encoder5_val;
+ packet->main.left_drive = encoder5_val;
+ packet->main.right_drive = encoder4_val;
packet->main.shooter_angle = encoder2_val;
packet->main.indexer = encoder3_val;
@@ -496,7 +489,6 @@
packet->main.capture_top_rise = capture_top_rise;
packet->main.top_rise_count = top_rise_count;
-
packet->main.capture_top_fall = capture_top_fall;
packet->main.top_fall_count = top_fall_count;
packet->main.top_disc = !digital(2);
@@ -506,6 +498,9 @@
packet->main.bottom_fall_count = bottom_fall_count;
packet->main.bottom_disc = !digital(1);
+ packet->main.loader_top = !digital(5);
+ packet->main.loader_bottom = !digital(6);
+
packet->main.capture_shooter_angle_rise = capture_shooter_angle_rise;
packet->main.shooter_angle_rise_count = shooter_angle_rise_count;
packet->main.angle_adjust_bottom_hall_effect = !digital(4);