Merge branch 'devel' of robotics.mvla.net:git/brian/2013 into brian-devel
Pulled down Brian's latest changes. (Again)
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/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/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..65fb216 100644
--- a/frc971/constants.cpp
+++ b/frc971/constants.cpp
@@ -18,7 +18,7 @@
// It has about 0.029043 of gearbox slop.
const double kCompWristHallEffectStartAngle = 1.285;
-const double kPracticeWristHallEffectStartAngle = 1.171;
+const double kPracticeWristHallEffectStartAngle = 1.164;
const double kCompWristHallEffectStopAngle = 100 * M_PI / 180.0;
const double kPracticeWristHallEffectStopAngle = 100 * M_PI / 180.0;
@@ -70,6 +70,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 +110,8 @@
// Deadband voltage.
double angle_adjust_deadband;
+ int drivetrain_gearbox_pinion;
+
// what camera_center returns
int camera_center;
};
@@ -139,6 +144,7 @@
kAngleAdjustZeroingSpeed,
kAngleAdjustZeroingOffSpeed,
kCompAngleAdjustDeadband,
+ kCompDrivetrainGearboxPinion,
kCompCameraCenter};
break;
case kPracticeTeamNumber:
@@ -159,6 +165,7 @@
kAngleAdjustZeroingSpeed,
kAngleAdjustZeroingOffSpeed,
kPracticeAngleAdjustDeadband,
+ kPracticeDrivetrainGearboxPinion,
kPracticeCameraCenter};
break;
default:
@@ -289,6 +296,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..a1fc749 100644
--- a/frc971/constants.h
+++ b/frc971/constants.h
@@ -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..3932ab5 100644
--- a/frc971/control_loops/index/index.cc
+++ b/frc971/control_loops/index/index.cc
@@ -106,8 +106,9 @@
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::kLiftingDelay = 2;
+/*static*/ const int IndexMotor::kLiftingTimeout = 65;
+/*static*/ const int IndexMotor::kShootingDelay = 10;
/*static*/ const int IndexMotor::kLoweringDelay = 20;
// TODO(aschuh): Tune these.
@@ -871,6 +872,7 @@
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 +883,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,12 +897,22 @@
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_;
+ } 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;
+ }
}
+ break;
case LoaderState::LIFTED:
LOG(DEBUG, "Loader LIFTED\n");
// Disc lifted. Time to eject it out.
diff --git a/frc971/control_loops/index/index.h b/frc971/control_loops/index/index.h
index a18d4c9..8abb119 100644
--- a/frc971/control_loops/index/index.h
+++ b/frc971/control_loops/index/index.h
@@ -136,8 +136,11 @@
// 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.
@@ -309,7 +312,7 @@
// Loader goal, state, and counter.
LoaderGoal loader_goal_;
LoaderState loader_state_;
- int loader_countdown_;
+ int loader_countdown_, loader_timeout_;
// Current state of the pistons.
bool loader_up_;
diff --git a/frc971/control_loops/index/index_motor.q b/frc971/control_loops/index/index_motor.q
index c14d459..3956b18 100644
--- a/frc971/control_loops/index/index_motor.q
+++ b/frc971/control_loops/index/index_motor.q
@@ -44,6 +44,9 @@
// and a count of how many edges have been seen.
int32_t top_disc_negedge_count;
double top_disc_negedge_position;
+
+ // Whether the hall effect for the loader being at the top is triggered.
+ bool loader_top;
};
message Output {
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/input/JoystickReader.cc b/frc971/input/JoystickReader.cc
index 31f0385..3943350 100644
--- a/frc971/input/JoystickReader.cc
+++ b/frc971/input/JoystickReader.cc
@@ -181,7 +181,7 @@
angle_adjust_goal = 0.520;
} else if (data.IsPressed(kShortShot)) {
shooter_goal->velocity = 375;
- angle_adjust_goal = 0.7267;
+ angle_adjust_goal = 0.671;
}
angle_adjust.goal.MakeWithBuilder().goal(angle_adjust_goal).Send();
diff --git a/frc971/input/gyro_board_data.h b/frc971/input/gyro_board_data.h
index 10135e3..3650e96 100644
--- a/frc971/input/gyro_board_data.h
+++ b/frc971/input/gyro_board_data.h
@@ -41,6 +41,7 @@
uint8_t angle_adjust_bottom_hall_effect : 1;
uint8_t top_disc : 1;
uint8_t bottom_disc : 1;
+ uint8_t loader_top : 1;
};
uint32_t digitals;
};
diff --git a/frc971/input/gyro_board_reader.cc b/frc971/input/gyro_board_reader.cc
index 06e760c..817efa9 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);
}
@@ -224,6 +235,7 @@
.bottom_disc_negedge_wait_position(index_translate(
data->capture_bottom_fall_delay))
.bottom_disc_negedge_wait_count(bottom_fall_delay_count_)
+ .loader_top(data->loader_top)
.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 8e3cd3c..adec079 100644
--- a/gyro_board/src/usb/Makefile
+++ b/gyro_board/src/usb/Makefile
@@ -59,7 +59,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.
+ echo -e '\x1B' | $(FLASHER) -termonly -control $(PORT) $(SPEED) $(OSC)
cat:
@cd ../../bin; python serial_looper.py
diff --git a/gyro_board/src/usb/analog.c b/gyro_board/src/usb/analog.c
index 6e26e84..2b553c0 100644
--- a/gyro_board/src/usb/analog.c
+++ b/gyro_board/src/usb/analog.c
@@ -160,7 +160,6 @@
// ENC1A 2.11
void EINT1_IRQHandler(void) {
- // TODO(brians): figure out why this has to be up here too
SC->EXTINT = 0x2;
int fiopin = GPIO2->FIOPIN;
if (((fiopin >> 1) ^ fiopin) & 0x800) {
@@ -169,7 +168,6 @@
--encoder1_val;
}
SC->EXTPOLAR ^= 0x2;
- SC->EXTINT = 0x2;
}
// ENC1B 2.12
void EINT2_IRQHandler(void) {
@@ -181,7 +179,6 @@
++encoder1_val;
}
SC->EXTPOLAR ^= 0x4;
- SC->EXTINT = 0x4;
}
// GPIO Interrupt handlers
@@ -445,12 +442,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;
@@ -492,8 +484,8 @@
packet->gyro_angle = gyro_angle;
packet->shooter = encoder1_val;
- packet->left_drive = encoder4_val;
- packet->right_drive = encoder5_val;
+ packet->left_drive = encoder5_val;
+ packet->right_drive = encoder4_val;
packet->shooter_angle = encoder2_val;
packet->indexer = encoder3_val;
@@ -512,7 +504,6 @@
packet->capture_top_rise = capture_top_rise;
packet->top_rise_count = top_rise_count;
-
packet->capture_top_fall = capture_top_fall;
packet->top_fall_count = top_fall_count;
packet->top_disc = !digital(2);
@@ -522,6 +513,8 @@
packet->bottom_fall_count = bottom_fall_count;
packet->bottom_disc = !digital(1);
+ packet->loader_top = !digital(5);
+
packet->capture_shooter_angle_rise = capture_shooter_angle_rise;
packet->shooter_angle_rise_count = shooter_angle_rise_count;
packet->angle_adjust_bottom_hall_effect = !digital(4);
diff --git a/gyro_board/src/usb/analog.h b/gyro_board/src/usb/analog.h
index b72218b..8dc0963 100644
--- a/gyro_board/src/usb/analog.h
+++ b/gyro_board/src/usb/analog.h
@@ -38,6 +38,7 @@
uint8_t angle_adjust_bottom_hall_effect : 1;
uint8_t top_disc : 1;
uint8_t bottom_disc : 1;
+ uint8_t loader_top : 1;
};
uint32_t digitals;
};