Merge changes Ifa6d446d,Ife280464
* changes:
Get all our k8 clang tools loading downloaded .sos
Use a (mostly) hermetic Python interpreter
diff --git a/motors/seems_reasonable/spring.cc b/motors/seems_reasonable/spring.cc
index 7cf80c1..1127aa9 100644
--- a/motors/seems_reasonable/spring.cc
+++ b/motors/seems_reasonable/spring.cc
@@ -29,7 +29,7 @@
}
void Spring::Iterate(bool unload, bool prime, bool fire, bool force_reset,
- bool encoder_valid, float angle) {
+ bool force_move, bool encoder_valid, float angle) {
// Angle is +- M_PI. So, we need to find the nearest angle to the previous
// one, and that's our new angle.
angle_ = ::frc971::zeroing::Wrap(angle_, angle, kTwoPi);
@@ -39,12 +39,21 @@
// Go to the previous unload from where we are.
goal_ = angle_;
goal_ = PreviousGoal(kUnloadGoal);
- if (prime && fire) {
+ if (force_move) {
+ ForceMove();
+ } else if (prime && fire) {
Unload();
}
break;
+ case State::FORCE_MOVE:
+ if (!force_move) {
+ state_ = State::UNINITIALIZED;
+ }
+ break;
case State::UNLOAD:
- if (!encoder_valid) {
+ if (force_move) {
+ ForceMove();
+ } else if (!encoder_valid) {
state_ = State::STUCK_UNLOAD;
} else if (!unload && prime && fire) {
// Go to the next goal from the current location. This handles if we
@@ -54,14 +63,19 @@
Load();
}
case State::STUCK_UNLOAD:
- if (force_reset && encoder_valid && state_ == State::STUCK_UNLOAD) {
+ if (force_move) {
+ ForceMove();
+ } else if (force_reset && encoder_valid &&
+ state_ == State::STUCK_UNLOAD) {
state_ = State::UNINITIALIZED;
} else if (timeout_ > 0) {
--timeout_;
}
break;
case State::LOAD:
- if (!encoder_valid) {
+ if (force_move) {
+ ForceMove();
+ } else if (!encoder_valid) {
goal_ = PreviousGoal(kUnloadGoal);
StuckUnload();
} else if (unload) {
@@ -79,7 +93,9 @@
}
break;
case State::PRIME:
- if (!encoder_valid) {
+ if (force_move) {
+ ForceMove();
+ } else if (!encoder_valid) {
goal_ = PreviousGoal(kUnloadGoal);
StuckUnload();
} else if (unload) {
@@ -101,7 +117,9 @@
break;
case State::FIRE:
- if (!encoder_valid) {
+ if (force_move) {
+ ForceMove();
+ } else if (!encoder_valid) {
goal_ = PreviousGoal(kUnloadGoal);
StuckUnload();
} else if (!Near()) {
@@ -120,7 +138,9 @@
}
break;
case State::WAIT_FOR_LOAD:
- if (!encoder_valid) {
+ if (force_move) {
+ ForceMove();
+ } else if (!encoder_valid) {
StuckUnload();
} else if (unload) {
// Goal is as good as it is going to get since unload is the fire
@@ -131,7 +151,9 @@
}
break;
case State::WAIT_FOR_LOAD_RELEASE:
- if (!encoder_valid) {
+ if (force_move) {
+ ForceMove();
+ } else if (!encoder_valid) {
StuckUnload();
} else if (unload) {
// Goal is as good as it is going to get since unload is the fire
@@ -159,6 +181,10 @@
}
break;
+ case State::FORCE_MOVE:
+ output_ = 1.0f;
+ break;
+
case State::LOAD:
case State::PRIME:
case State::FIRE:
diff --git a/motors/seems_reasonable/spring.h b/motors/seems_reasonable/spring.h
index c7c923a..90c9cd1 100644
--- a/motors/seems_reasonable/spring.h
+++ b/motors/seems_reasonable/spring.h
@@ -21,7 +21,7 @@
// If prime is true, go to primed state.
// If prime and fire are true, fire.
void Iterate(bool unload, bool prime, bool fire, bool force_reset,
- bool encoder_valid, float angle);
+ bool force_move, bool encoder_valid, float angle);
enum class State {
UNINITIALIZED = 0,
@@ -32,6 +32,7 @@
FIRE = 5,
WAIT_FOR_LOAD = 6,
WAIT_FOR_LOAD_RELEASE = 7,
+ FORCE_MOVE = 8,
};
// Returns the current to output to the spring motors.
@@ -58,13 +59,18 @@
state_ = State::PRIME;
}
+ void ForceMove() {
+ timeout_ = 0;
+ state_ = State::FORCE_MOVE;
+ }
+
void Unload() {
- timeout_ = 10 * 200;
+ timeout_ = 14 * 200;
state_ = State::UNLOAD;
}
void StuckUnload() {
- timeout_ = 10 * 200;
+ timeout_ = 14 * 200;
state_ = State::STUCK_UNLOAD;
}
@@ -86,7 +92,7 @@
// Note, these need to be (-M_PI, M_PI]
constexpr static float kLoadGoal = -0.345f;
constexpr static float kPrimeGoal = -0.269f;
- constexpr static float kFireGoal = -0.163f;
+ constexpr static float kFireGoal = -0.063f;
constexpr static float kUnloadGoal = kFireGoal;
float angle_ = 0.0f;
diff --git a/motors/simple_receiver.cc b/motors/simple_receiver.cc
index 20ca298..f8b38f0 100644
--- a/motors/simple_receiver.cc
+++ b/motors/simple_receiver.cc
@@ -533,15 +533,17 @@
++invalid_encoder_count;
}
- const bool lost_any_channel = lost_channel(0) || lost_channel(1) ||
- lost_channel(2) || lost_channel(3) ||
- lost_channel(4) ||
- (::std::abs(convert_input_width(4)) < 0.5f);
+ const bool lost_spring_channel = lost_channel(2) || lost_channel(3) ||
+ lost_channel(4) || lost_channel(5) ||
+ (convert_input_width(4) < 0.5f);
+
+ const bool lost_drive_channel = lost_channel(0) || lost_channel(1) ||
+ (::std::abs(convert_input_width(4)) < 0.5f);
if (polydrivetrain != nullptr && spring != nullptr) {
DrivetrainQueue_Goal goal;
goal.control_loop_driving = false;
- if (lost_any_channel) {
+ if (lost_drive_channel) {
goal.throttle = 0.0f;
goal.wheel = 0.0f;
} else {
@@ -562,16 +564,18 @@
vesc_set_duty(2, output.right_voltage / 12.0f);
vesc_set_duty(3, output.right_voltage / 12.0f);
- bool prime = convert_input_width(2) > 0.1f;
- bool fire = convert_input_width(3) > 0.1f;
+ const bool prime = convert_input_width(2) > 0.1f;
+ const bool fire = convert_input_width(3) > 0.1f;
+ const bool force_move =
+ (convert_input_width(5) > 0.1f) && !lost_spring_channel;
- bool unload = lost_any_channel;
+ bool unload = lost_spring_channel;
static bool was_lost = true;
- bool force_reset = !lost_any_channel && was_lost;
- was_lost = lost_any_channel;
+ bool force_reset = !lost_spring_channel && was_lost;
+ was_lost = lost_spring_channel;
- spring->Iterate(unload, prime, fire, force_reset, invalid_encoder_count <= 2,
- last_good_encoder);
+ spring->Iterate(unload, prime, fire, force_reset, force_move,
+ invalid_encoder_count <= 2, last_good_encoder);
float spring_output = spring->output();
@@ -597,16 +601,18 @@
++i;
if (i > 20) {
i = 0;
- if (lost_any_channel) {
- printf("200Hz loop, disabled %d %d %d %d %d\n",
+ if (lost_spring_channel || lost_drive_channel) {
+ printf("200Hz loop, disabled %d %d %d %d %d %d\n",
(int)(convert_input_width(0) * 1000),
(int)(convert_input_width(1) * 1000),
(int)(convert_input_width(2) * 1000),
(int)(convert_input_width(3) * 1000),
- (int)(convert_input_width(4) * 1000));
+ (int)(convert_input_width(4) * 1000),
+ (int)(convert_input_width(5) * 1000));
} else {
printf(
- "TODO(Austin): 200Hz loop %d %d %d %d %d, lr, %d, %d velocity %d "
+ "TODO(Austin): 200Hz loop %d %d %d %d %d %d, lr, %d, %d velocity %d
+ "
" state: %d, near %d angle %d goal %d to: %d ADC %" PRIu16
" %" PRIu16 " enc %d/1000 %s from %d\n",
(int)(convert_input_width(0) * 1000),
@@ -614,6 +620,7 @@
(int)(convert_input_width(2) * 1000),
(int)(convert_input_width(3) * 1000),
(int)(convert_input_width(4) * 1000),
+ (int)(convert_input_width(5) * 1000),
static_cast<int>(output.left_voltage * 100),
static_cast<int>(output.right_voltage * 100),
static_cast<int>(polydrivetrain->velocity() * 100),
@@ -772,7 +779,9 @@
NVIC_ENABLE_IRQ(IRQ_FTM0);
NVIC_ENABLE_IRQ(IRQ_FTM3);
NVIC_ENABLE_IRQ(IRQ_PIT_CH3);
+ printf("Done starting up2\n");
+ //DoReceiverTest2();
while (true) {
}
diff --git a/y2018/constants.cc b/y2018/constants.cc
index 42c8501..2ad0c97 100644
--- a/y2018/constants.cc
+++ b/y2018/constants.cc
@@ -118,13 +118,13 @@
right_intake->potentiometer_offset = 9.59 + 1.530320 - 3.620648;
right_intake->spring_offset = 0.255 + 0.008 - 0.09;
- arm_proximal->zeroing.measured_absolute_position = 0.1877 + 0.02 + 0.1;
- arm_proximal->potentiometer_offset = -1.242 - 0.03 - 0.1;
+ arm_proximal->zeroing.measured_absolute_position = -0.253183 + 1.0652774488034022 + 0.009566448803402405;
+ arm_proximal->potentiometer_offset = -1.242 - 0.03 - 0.1 - 1.0652;
arm_distal->zeroing.measured_absolute_position =
- 1.102987 - kDistalZeroingPosition + 0.12 + 0.0095;
+ 1.102987 - kDistalZeroingPosition + 0.12 + 0.0095 + 0.22300918279692628;
arm_distal->potentiometer_offset =
- 2.772210 + M_PI + 0.434 - 0.12 + 1.25 - 0.226 + 0.862067;
+ 2.772210 + M_PI + 0.434 - 0.12 + 1.25 - 0.226 + 0.862067 - 0.121925182796926;
break;
default: