commit d6bc8bafb86dcb667934e30a2a159497ffb42a71
author Austin Schuh <austin.linux@gmail.com> Sun Sep 13 19:39:38 2015 +0000
committer Austin Schuh <austin.linux@gmail.com> Sun Sep 13 19:40:56 2015 +0000
tree 8d96642e67c34c31d275d55c7db51a272934f175
parent 3f748485bf46fbb06343ed79a2224a62bc48570e

Added tote sensor for autostack.

Change-Id: I85e795e890ddf0ec61c5eb6b53ea692cd0581641

bot3/control_loops/elevator/elevator.q

diff --git a/bot3/control_loops/elevator/elevator.q b/bot3/control_loops/elevator/elevator.q
index 6b2ba26..b6340e3 100644
--- a/bot3/control_loops/elevator/elevator.q
+++ b/bot3/control_loops/elevator/elevator.q
@@ -34,6 +34,9 @@
 
     // bottom hall effect sensor for zeroing purposes.
     bool bottom_hall_effect;
+
+    // True if there is a tote inside the elevator.
+    bool has_tote;
   };
 
   message Status {

bot3/wpilib/wpilib_interface.cc

diff --git a/bot3/wpilib/wpilib_interface.cc b/bot3/wpilib/wpilib_interface.cc
index 831c0e8..a43a84f 100644
--- a/bot3/wpilib/wpilib_interface.cc
+++ b/bot3/wpilib/wpilib_interface.cc
@@ -108,6 +108,10 @@
     zeroing_hall_effect_ = ::std::move(hall);
   }
 
+  void set_elevator_tote_sensor(::std::unique_ptr<AnalogInput> tote_sensor) {
+    tote_sensor_ = ::std::move(tote_sensor);
+  }
+
   void operator()() {
     LOG(INFO, "In sensor reader thread\n");
     ::aos::SetCurrentThreadName("SensorReader");
@@ -165,6 +169,7 @@
           elevator_translate(elevator_encoder_->GetRaw());
       elevator_message->bottom_hall_effect =
           zeroing_hall_effect_->Get();
+      elevator_message->has_tote = tote_sensor_->GetVoltage() > 2.5;
 
       elevator_message.Send();
     }
@@ -187,6 +192,7 @@
 
   ::std::unique_ptr<Encoder> left_encoder_, right_encoder_, elevator_encoder_;
   ::std::unique_ptr<HallEffect> zeroing_hall_effect_;
+  ::std::unique_ptr<AnalogInput> tote_sensor_;
 
   ::std::atomic<bool> run_{true};
   DigitalGlitchFilter filter_;
@@ -415,6 +421,7 @@
 
     reader.set_left_encoder(encoder(0));
     reader.set_right_encoder(encoder(1));
+    reader.set_elevator_tote_sensor(make_unique<AnalogInput>(0));
 
     ::std::thread reader_thread(::std::ref(reader));
     GyroSender gyro_sender;
@@ -442,12 +449,10 @@
     SolenoidWriter solenoid_writer(pcm);
     solenoid_writer.set_pressure_switch(make_unique<DigitalInput>(9));
     solenoid_writer.set_compressor_relay(make_unique<Relay>(0));
-    // TODO (jasmine): Find solenoid numbers
     solenoid_writer.set_elevator_passive_support(pcm->MakeSolenoid(0));
     solenoid_writer.set_elevator_can_support(pcm->MakeSolenoid(1));
     solenoid_writer.set_intake_claw(pcm->MakeSolenoid(2));
     ::std::thread solenoid_thread(::std::ref(solenoid_writer));
-    // TODO(comran): Find talon/encoder numbers ^^^
 
     // Wait forever. Not much else to do...
     PCHECK(select(0, nullptr, nullptr, nullptr, nullptr));