Fliped default and put auto in high gear for y2014.
Change-Id: Iafb45ecac89a10eb9c05843626f20212488293d6
diff --git a/y2014/actors/drivetrain_actor.cc b/y2014/actors/drivetrain_actor.cc
index 3cf86bd..1f1a678 100644
--- a/y2014/actors/drivetrain_actor.cc
+++ b/y2014/actors/drivetrain_actor.cc
@@ -30,8 +30,8 @@
LOG(INFO, "Going to move %f and turn %f\n", yoffset, turn_offset);
// Measured conversion to get the distance right.
- ::aos::util::TrapezoidProfile profile(::aos::time::Time::InMS(5));
- ::aos::util::TrapezoidProfile turn_profile(::aos::time::Time::InMS(5));
+ ::aos::util::TrapezoidProfile profile(::aos::time::Time::InMS(10));
+ ::aos::util::TrapezoidProfile turn_profile(::aos::time::Time::InMS(10));
const double goal_velocity = 0.0;
const double epsilon = 0.01;
::Eigen::Matrix<double, 2, 1> left_goal_state, right_goal_state;
@@ -118,7 +118,7 @@
right_goal_state(0, 0) + params.right_initial_position);
control_loops::drivetrain_queue.goal.MakeWithBuilder()
.control_loop_driving(true)
- //.highgear(false)
+ .highgear(true)
.left_goal(left_goal_state(0, 0) + params.left_initial_position)
.right_goal(right_goal_state(0, 0) + params.right_initial_position)
.left_velocity_goal(left_goal_state(1, 0))
diff --git a/y2014/autonomous/auto.cc b/y2014/autonomous/auto.cc
index ab28323..fe4099a 100644
--- a/y2014/autonomous/auto.cc
+++ b/y2014/autonomous/auto.cc
@@ -43,6 +43,7 @@
LOG(INFO, "Stopping the drivetrain\n");
control_loops::drivetrain_queue.goal.MakeWithBuilder()
.control_loop_driving(true)
+ .highgear(true)
.left_goal(left_initial_position)
.left_velocity_goal(0)
.right_goal(right_initial_position)
@@ -55,7 +56,7 @@
LOG(INFO, "resetting the drivetrain\n");
control_loops::drivetrain_queue.goal.MakeWithBuilder()
.control_loop_driving(false)
- .highgear(false)
+ .highgear(true)
.steering(0.0)
.throttle(0.0)
.left_goal(left_initial_position)
@@ -87,6 +88,7 @@
theta * constants::GetValues().turn_width / 2.0);
control_loops::drivetrain_queue.goal.MakeWithBuilder()
.control_loop_driving(true)
+ .highgear(true)
.left_goal(left_goal)
.right_goal(right_goal)
.left_velocity_goal(0.0)
diff --git a/y2014/control_loops/drivetrain/drivetrain.cc b/y2014/control_loops/drivetrain/drivetrain.cc
index ecb8ed2..a326125 100644
--- a/y2014/control_loops/drivetrain/drivetrain.cc
+++ b/y2014/control_loops/drivetrain/drivetrain.cc
@@ -154,8 +154,8 @@
right_goal_(0.0),
raw_left_(0.0),
raw_right_(0.0) {
- // Low gear on both.
- loop_->set_controller_index(0);
+ // High gear on both.
+ loop_->set_controller_index(3);
}
void SetGoal(double left, double left_velocity, double right,
@@ -219,8 +219,8 @@
if (output) {
output->left_voltage = loop_->U(0, 0);
output->right_voltage = loop_->U(1, 0);
- output->left_high = false;
- output->right_high = false;
+ output->left_high = true;
+ output->right_high = true;
}
}
diff --git a/y2014/control_loops/python/drivetrain.py b/y2014/control_loops/python/drivetrain.py
index 5a2e478..14a3ecb 100755
--- a/y2014/control_loops/python/drivetrain.py
+++ b/y2014/control_loops/python/drivetrain.py
@@ -90,7 +90,7 @@
self.Gr = self.G_high
# Control loop time step
- self.dt = 0.005
+ self.dt = 0.010
# These describe the way that a given side of a robot will be influenced
# by the other side. Units of 1 / kg.
diff --git a/y2014/wpilib/wpilib_interface.cc b/y2014/wpilib/wpilib_interface.cc
index 20bef26..f636dee 100644
--- a/y2014/wpilib/wpilib_interface.cc
+++ b/y2014/wpilib/wpilib_interface.cc
@@ -74,7 +74,7 @@
return -static_cast<double>(in) /
(256.0 /*cpr*/ * 4.0 /*4x*/) *
constants::GetValues().drivetrain_encoder_ratio *
- (3.5 /*wheel diameter*/ * 2.54 / 100.0 * M_PI) * 2.0;
+ (3.5 /*wheel diameter*/ * 2.54 / 100.0 * M_PI) * 2.0 / 2.0;
}
float hall_translate(const constants::ShifterHallEffect &k, float in_low,
@@ -511,8 +511,8 @@
drivetrain_.FetchLatest();
if (drivetrain_.get()) {
LOG_STRUCT(DEBUG, "solenoids", *drivetrain_);
- drivetrain_left_->Set(drivetrain_->left_high);
- drivetrain_right_->Set(drivetrain_->right_high);
+ drivetrain_left_->Set(!drivetrain_->left_high);
+ drivetrain_right_->Set(!drivetrain_->right_high);
}
}