Merge "Add fast front to back moves."
diff --git a/aos/vision/events/udp.cc b/aos/vision/events/udp.cc
index 4c0437b..660342c 100644
--- a/aos/vision/events/udp.cc
+++ b/aos/vision/events/udp.cc
@@ -26,8 +26,8 @@
return send(fd_.get(), data, size, 0);
}
-RXUdpSocket::RXUdpSocket(int port)
- : fd_(PCHECK(socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP))) {
+int RXUdpSocket::SocketBindListenOnPort(int port) {
+ int fd = PCHECK(socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP));
sockaddr_in bind_address;
memset(&bind_address, 0, sizeof(bind_address));
@@ -35,10 +35,13 @@
bind_address.sin_port = htons(port);
bind_address.sin_addr.s_addr = htonl(INADDR_ANY);
- PCHECK(bind(fd_.get(), reinterpret_cast<sockaddr *>(&bind_address),
+ PCHECK(bind(fd, reinterpret_cast<sockaddr *>(&bind_address),
sizeof(bind_address)));
+ return fd;
}
+RXUdpSocket::RXUdpSocket(int port) : fd_(SocketBindListenOnPort(port)) {}
+
int RXUdpSocket::Recv(void *data, int size) {
return PCHECK(recv(fd_.get(), static_cast<char *>(data), size, 0));
}
diff --git a/aos/vision/events/udp.h b/aos/vision/events/udp.h
index ed4a8e7..9a23dab 100644
--- a/aos/vision/events/udp.h
+++ b/aos/vision/events/udp.h
@@ -30,6 +30,24 @@
DISALLOW_COPY_AND_ASSIGN(TXUdpSocket);
};
+// Send a protobuf. Not RT (mallocs on send).
+template <typename PB>
+class ProtoTXUdpSocket {
+ public:
+ ProtoTXUdpSocket(const std::string &ip_addr, int port)
+ : socket_(ip_addr, port) {}
+
+ void Send(const PB &pb) {
+ ::std::string serialized_data;
+ pb.SerializeToString(&serialized_data);
+ socket_.Send(serialized_data.data(), serialized_data.size());
+ }
+
+ private:
+ TXUdpSocket socket_;
+ DISALLOW_COPY_AND_ASSIGN(ProtoTXUdpSocket);
+};
+
// Simple wrapper around a receiving UDP socket.
//
// LOG(FATAL)s for all errors, including from Recv.
@@ -40,6 +58,8 @@
// Returns the number of bytes received.
int Recv(void *data, int size);
+ static int SocketBindListenOnPort(int port);
+
private:
ScopedFD fd_;
diff --git a/motors/fet12/BUILD b/motors/fet12/BUILD
new file mode 100644
index 0000000..a242f8e
--- /dev/null
+++ b/motors/fet12/BUILD
@@ -0,0 +1,22 @@
+load("//motors:macros.bzl", "hex_from_elf")
+load("//tools:environments.bzl", "mcu_cpus")
+
+cc_binary(
+ name = "power_wheels.elf",
+ srcs = [
+ "power_wheels.cc",
+ ],
+ restricted_to = mcu_cpus,
+ deps = [
+ "//motors:util",
+ "//motors/core",
+ "//motors/peripheral:adc",
+ "//motors/usb",
+ "//motors/usb:cdc",
+ ],
+)
+
+hex_from_elf(
+ name = "power_wheels",
+ restricted_to = mcu_cpus,
+)
diff --git a/motors/fet12/power_wheels.cc b/motors/fet12/power_wheels.cc
new file mode 100644
index 0000000..4920c9b
--- /dev/null
+++ b/motors/fet12/power_wheels.cc
@@ -0,0 +1,346 @@
+#include "motors/core/kinetis.h"
+
+#include <inttypes.h>
+#include <math.h>
+#include <stdio.h>
+
+#include <atomic>
+
+#include "motors/core/time.h"
+#include "motors/peripheral/adc.h"
+#include "motors/usb/cdc.h"
+#include "motors/usb/usb.h"
+#include "motors/util.h"
+
+namespace frc971 {
+namespace motors {
+namespace {
+
+struct Fet12AdcReadings {
+ // 1100 off, 3160 floored
+ uint16_t throttle;
+};
+
+void AdcInitFet12() {
+ AdcInitCommon();
+
+ // EI2C_SCL (end pin) ADC0_SE13
+ PORTB_PCR3 = PORT_PCR_MUX(0);
+}
+
+Fet12AdcReadings AdcReadFet12(const DisableInterrupts &) {
+ Fet12AdcReadings r;
+
+ ADC0_SC1A = 13;
+ while (!(ADC0_SC1A & ADC_SC1_COCO)) {
+ }
+ r.throttle = ADC0_RA;
+
+ return r;
+}
+
+bool ReadButton() { return PERIPHERAL_BITBAND(GPIOB_PDIR, 2); }
+
+::std::atomic<teensy::AcmTty *> global_stdout{nullptr};
+
+extern "C" {
+
+void *__stack_chk_guard = (void *)0x67111971;
+void __stack_chk_fail(void) {
+ while (true) {
+ GPIOC_PSOR = (1 << 5);
+ printf("Stack corruption detected\n");
+ delay(1000);
+ GPIOC_PCOR = (1 << 5);
+ delay(1000);
+ }
+}
+
+int _write(int /*file*/, char *ptr, int len) {
+ teensy::AcmTty *const tty = global_stdout.load(::std::memory_order_acquire);
+ if (tty != nullptr) {
+ return tty->Write(ptr, len);
+ }
+ return 0;
+}
+
+void __stack_chk_fail(void);
+
+extern char *__brkval;
+extern uint32_t __bss_ram_start__[];
+extern uint32_t __heap_start__[];
+extern uint32_t __stack_end__[];
+
+} // extern "C"
+
+constexpr int kOutputCounts = 37500;
+constexpr int kOutputPrescalerShift = 4;
+
+void SetOutputWidth(float ms) {
+ static constexpr float kScale = static_cast<float>(
+ static_cast<double>(kOutputCounts) / 10.0 /* milliseconds per period */);
+ const int width = static_cast<int>(ms * kScale + 0.5f);
+ FTM3->C6V = width - 1;
+ FTM3->PWMLOAD = FTM_PWMLOAD_LDOK;
+}
+
+} // namespace
+
+extern "C" int main(void) {
+ // for background about this startup delay, please see these conversations
+ // https://forum.pjrc.com/threads/36606-startup-time-(400ms)?p=113980&viewfull=1#post113980
+ // https://forum.pjrc.com/threads/31290-Teensey-3-2-Teensey-Loader-1-24-Issues?p=87273&viewfull=1#post87273
+ delay(400);
+
+ // Set all interrupts to the second-lowest priority to start with.
+ for (int i = 0; i < NVIC_NUM_INTERRUPTS; i++) NVIC_SET_SANE_PRIORITY(i, 0xD);
+
+ // Now set priorities for all the ones we care about. They only have meaning
+ // relative to each other, which means centralizing them here makes it a lot
+ // more manageable.
+ NVIC_SET_SANE_PRIORITY(IRQ_USBOTG, 0x7);
+ NVIC_SET_SANE_PRIORITY(IRQ_FTM0, 0x3);
+
+ // Set the LED's pin to output mode.
+ PERIPHERAL_BITBAND(GPIOC_PDDR, 5) = 1;
+ PORTC_PCR5 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+
+ // EI2C_SCL (not end) PTB3
+ PORTB_PCR2 = PORT_PCR_MUX(1);
+
+#if 0
+ PERIPHERAL_BITBAND(GPIOA_PDDR, 15) = 1;
+ PORTA_PCR15 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+#endif
+
+ DMA_CR = DMA_CR_EMLM;
+
+ teensy::UsbDevice usb_device(0, 0x16c0, 0x0490);
+ usb_device.SetManufacturer("FRC 971 Spartan Robotics");
+ usb_device.SetProduct("Pistol Grip Controller debug");
+ teensy::AcmTty tty1(&usb_device);
+ teensy::AcmTty tty2(&usb_device);
+ global_stdout.store(&tty1, ::std::memory_order_release);
+ usb_device.Initialize();
+
+ AdcInitFet12();
+ delay(1000);
+
+#if 0
+ GPIOD_PCOR = 1 << 3;
+ PERIPHERAL_BITBAND(GPIOD_PDDR, 3) = 1;
+ PORTD_PCR3 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+ delay(1000);
+ GPIOD_PSOR = 1 << 3;
+ delay(1000);
+ GPIOD_PCOR = 1 << 3;
+ delay(1000);
+#endif
+
+ delay(1000);
+
+ // Index pin
+ PORTA_PCR7 = PORT_PCR_MUX(1);
+ // FTM1_QD_PH{A,B}
+ PORTB_PCR0 = PORT_PCR_MUX(6);
+ PORTB_PCR1 = PORT_PCR_MUX(6);
+
+ // FTM3_CH6 for PWM_IN (used as output)
+ PORTE_PCR11 = PORT_PCR_MUX(6);
+
+ auto *const encoder_ftm = FTM1;
+ // PWMSYNC doesn't matter because we set SYNCMODE down below.
+ encoder_ftm->MODE = FTM_MODE_WPDIS;
+ encoder_ftm->MODE = FTM_MODE_WPDIS | FTM_MODE_FTMEN;
+ encoder_ftm->SC =
+ FTM_SC_CLKS(1) /* Use the system clock (not sure it matters) */ |
+ FTM_SC_PS(0) /* Don't prescale the clock (not sure it matters) */;
+
+ encoder_ftm->MOD = 1023;
+
+ // I think you have to set this to something other than 0 for the quadrature
+ // encoder mode to actually work? This is "input capture on rising edge only",
+ // which should be fine.
+ encoder_ftm->C0SC = FTM_CSC_ELSA;
+ encoder_ftm->C1SC = FTM_CSC_ELSA;
+
+ encoder_ftm->FILTER = FTM_FILTER_CH0FVAL(0) /* No filter */ |
+ FTM_FILTER_CH1FVAL(0) /* No filter */;
+
+ // Could set PHAFLTREN and PHBFLTREN here to enable the filters.
+ encoder_ftm->QDCTRL = FTM_QDCTRL_QUADEN;
+
+ encoder_ftm->SYNCONF =
+ FTM_SYNCONF_SWWRBUF /* Software trigger flushes MOD */ |
+ FTM_SYNCONF_SWRSTCNT /* Software trigger resets the count */ |
+ FTM_SYNCONF_SYNCMODE /* Use the new synchronization mode */;
+
+ encoder_ftm->SYNC = FTM_SYNC_SWSYNC /* Flush everything out right now */;
+ // Wait for the software synchronization to finish.
+ while (encoder_ftm->SYNC & FTM_SYNC_SWSYNC) {
+ }
+
+ auto *const pwm_ftm = FTM3;
+ // PWMSYNC doesn't matter because we set SYNCMODE down below.
+ pwm_ftm->MODE = FTM_MODE_WPDIS;
+ pwm_ftm->MODE = FTM_MODE_WPDIS | FTM_MODE_FTMEN;
+ pwm_ftm->SC = FTM_SC_CLKS(0) /* Disable counting for now */ |
+ FTM_SC_PS(kOutputPrescalerShift);
+
+ pwm_ftm->CNTIN = 0;
+ pwm_ftm->CNT = 0;
+ pwm_ftm->MOD = kOutputCounts - 1;
+
+ // High-true edge-aligned mode (turns on at start, off at match).
+ pwm_ftm->C0SC = FTM_CSC_MSB | FTM_CSC_ELSB;
+ pwm_ftm->C1SC = FTM_CSC_MSB | FTM_CSC_ELSB;
+ pwm_ftm->C2SC = FTM_CSC_MSB | FTM_CSC_ELSB;
+ pwm_ftm->C3SC = FTM_CSC_MSB | FTM_CSC_ELSB;
+ pwm_ftm->C4SC = FTM_CSC_MSB | FTM_CSC_ELSB;
+ pwm_ftm->C5SC = FTM_CSC_MSB | FTM_CSC_ELSB;
+ pwm_ftm->C6SC = FTM_CSC_MSB | FTM_CSC_ELSB;
+ pwm_ftm->C7SC = FTM_CSC_MSB | FTM_CSC_ELSB;
+
+ pwm_ftm->COMBINE = FTM_COMBINE_SYNCEN3 /* Synchronize updates usefully */ |
+ FTM_COMBINE_SYNCEN2 /* Synchronize updates usefully */ |
+ FTM_COMBINE_SYNCEN1 /* Synchronize updates usefully */ |
+ FTM_COMBINE_SYNCEN0 /* Synchronize updates usefully */;
+
+ // Initialize all the channels to 0.
+ pwm_ftm->OUTINIT = 0;
+
+ // All of the channels are active high.
+ pwm_ftm->POL = 0;
+
+ pwm_ftm->SYNCONF =
+ FTM_SYNCONF_HWWRBUF /* Hardware trigger flushes switching points */ |
+ FTM_SYNCONF_SWWRBUF /* Software trigger flushes switching points */ |
+ FTM_SYNCONF_SWRSTCNT /* Software trigger resets the count */ |
+ FTM_SYNCONF_SYNCMODE /* Use the new synchronization mode */;
+
+ // Don't want any intermediate loading points.
+ pwm_ftm->PWMLOAD = 0;
+
+ // This has to happen after messing with SYNCONF, and should happen after
+ // messing with various other things so the values can get flushed out of the
+ // buffers.
+ pwm_ftm->SYNC = FTM_SYNC_SWSYNC /* Flush everything out right now */ |
+ FTM_SYNC_CNTMAX /* Load new values at the end of the cycle */;
+ // Wait for the software synchronization to finish.
+ while (pwm_ftm->SYNC & FTM_SYNC_SWSYNC) {
+ }
+
+ // Don't let any memory accesses sneak past here, because we actually
+ // need everything to be starting up.
+ __asm__("" :: : "memory");
+
+ // Give everything a chance to get going.
+ delay(100);
+
+ printf("Ram start: %p\n", __bss_ram_start__);
+ printf("Heap start: %p\n", __heap_start__);
+ printf("Heap end: %p\n", __brkval);
+ printf("Stack start: %p\n", __stack_end__);
+
+ encoder_ftm->MODE &= ~FTM_MODE_WPDIS;
+ pwm_ftm->SC = FTM_SC_TOIE /* Interrupt on overflow */ |
+ FTM_SC_CLKS(1) /* Use the system clock */ |
+ FTM_SC_PS(kOutputPrescalerShift);
+ pwm_ftm->MODE &= ~FTM_MODE_WPDIS;
+
+ GPIOC_PSOR = 1 << 5;
+
+ uint16_t old_encoder = FTM1->CNT;
+ uint32_t start_time = micros();
+ while (true) {
+ const uint32_t end_time = start_time + UINT32_C(500);
+ while (micros() < end_time) {
+ }
+ start_time = end_time;
+
+ Fet12AdcReadings adc_readings;
+ {
+ DisableInterrupts disable_interrupts;
+ adc_readings = AdcReadFet12(disable_interrupts);
+ }
+ const float pedal_position = ::std::min(
+ 1.0f,
+ ::std::max(0.0f, static_cast<float>(adc_readings.throttle - 1200) /
+ static_cast<float>(3120 - 1200)));
+
+ const uint16_t new_encoder = FTM1->CNT;
+ // Full speed is ~418.
+ // Low gear is positive.
+ int16_t encoder_delta =
+ static_cast<int16_t>(new_encoder) - static_cast<int16_t>(old_encoder);
+ if (encoder_delta < -512) {
+ encoder_delta += 1024;
+ }
+ if (encoder_delta > 512) {
+ encoder_delta -= 1024;
+ }
+ old_encoder = new_encoder;
+
+ // Positive -> low gear
+ float speed = ::std::min(
+ 1.0f, ::std::max(-1.0f, static_cast<float>(encoder_delta) / 418.0f));
+
+ float out_command;
+ if (ReadButton()) {
+ out_command = pedal_position;
+ } else {
+ out_command = -pedal_position;
+ }
+
+ static constexpr float kMaxCurrentFull = 0.155f;
+ static constexpr float kMaxCurrentStopped = 0.29f;
+ float abs_speed;
+ if (speed > 0.0f) {
+ abs_speed = speed;
+ } else {
+ abs_speed = -speed;
+ }
+ float max_current =
+ abs_speed * (kMaxCurrentFull - kMaxCurrentStopped) + kMaxCurrentStopped;
+ if (abs_speed < 0.06f) {
+ max_current = 0.27f;
+ }
+ if (speed > 0.0f) {
+ out_command =
+ ::std::min(speed + max_current,
+ ::std::max(speed - 2.0f * max_current, out_command));
+ } else {
+ out_command = ::std::min(speed + 2.0f * max_current,
+ ::std::max(speed - max_current, out_command));
+ }
+
+ static float slew_limited_command = 0.0f;
+ constexpr float kMaxChangePerCycle = 1.0f / 150.0f;
+
+ if (out_command < slew_limited_command - kMaxChangePerCycle) {
+ out_command = slew_limited_command - kMaxChangePerCycle;
+ } else if (out_command > slew_limited_command + kMaxChangePerCycle) {
+ out_command = slew_limited_command + kMaxChangePerCycle;
+ }
+
+ slew_limited_command = out_command;
+
+ const float pwm_out = 1.5f + -slew_limited_command / 2.0f;
+ SetOutputWidth(pwm_out);
+
+ static int i = 0;
+ if (i == 100) {
+ i = 0;
+ printf("enc %" PRIu32 " throttle %" PRIu16 " %d out %d %d %d\n",
+ FTM1->CNT, adc_readings.throttle, ReadButton(),
+ (int)(pwm_out * 1000), (int)encoder_delta,
+ (int)(abs_speed * 1000));
+ }
+ ++i;
+ }
+
+ return 0;
+}
+
+} // namespace motors
+} // namespace frc971
diff --git a/motors/packages/4SMD_3.2_2.5.fp b/motors/packages/4SMD_3.2_2.5.fp
index 580cba6..59d9a58 100644
--- a/motors/packages/4SMD_3.2_2.5.fp
+++ b/motors/packages/4SMD_3.2_2.5.fp
@@ -3,8 +3,8 @@
# for a 3.20mm x 2.50mm 4-SMD, No Lead package.
Element[0x0 "" "" "" 0 0 0 0 0 100 0x0]
(
- Pad[-1.025mm -0.925mm -1.275mm -0.925mm 1.05mm 2000 1.2mm "1" "1" "square"]
- Pad[1.025mm -0.925mm 1.275mm -0.925mm 1.05mm 2000 1.2mm "2" "2" "square"]
- Pad[1.025mm 0.925mm 1.275mm 0.925mm 1.05mm 2000 1.2mm "3" "3" "square"]
- Pad[-1.025mm 0.925mm -1.275mm 0.925mm 1.05mm 2000 1.2mm "4" "4" "square"]
+ Pad[-1.025mm -0.925mm -1.275mm -0.925mm 1.05mm 2000 1.2mm "4" "4" "square"]
+ Pad[1.025mm -0.925mm 1.275mm -0.925mm 1.05mm 2000 1.2mm "3" "3" "square"]
+ Pad[1.025mm 0.925mm 1.275mm 0.925mm 1.05mm 2000 1.2mm "2" "2" "square"]
+ Pad[-1.025mm 0.925mm -1.275mm 0.925mm 1.05mm 2000 1.2mm "1" "1" "square"]
)
diff --git a/motors/packages/CAP_D.fp b/motors/packages/CAP_D.fp
index 0c91538..abf38ab 100644
--- a/motors/packages/CAP_D.fp
+++ b/motors/packages/CAP_D.fp
@@ -1,16 +1,18 @@
# This is the footprint at
-# http://www.nichicon.co.jp/english/products/pdfs/e-ch_ref.pdf
+# https://web.archive.org/web/20131102032848/http://industrial.panasonic.com/www-data/pdf/ABA0000/ABA0000PE251.pdf
# for a 6.3mm diameter capacitor (size D).
Element[0x0 "" "" "" 0 0 0 0 0 100 0x0]
(
- Pad[0 1.75mm 0 3.65mm 1.6mm 2000 1.85mm "1" "1" "square"]
- Pad[0 -1.75mm 0 -3.65mm 1.6mm 2000 1.85mm "2" "2" "square"]
+ Pad[0 1.7mm 0 3.3mm 1.6mm 2000 1.9mm "1" "1" "square"]
+ Pad[0 -1.7mm 0 -3.3mm 1.6mm 2000 1.9mm "2" "2" "square"]
- ElementLine[-3.55mm -3.9mm -1.5mm -3.9mm 2000]
- ElementLine[3.55mm -3.9mm 1.5mm -3.9mm 2000]
+ ElementLine[-3.7mm -3.55mm -1.5mm -3.55mm 0.5mm]
+ ElementLine[3.7mm -3.55mm 1.5mm -3.55mm 0.5mm]
- ElementLine[-3.8mm 2.95mm -3.8mm -3.2mm 800]
- ElementLine[3.8mm 2.95mm 3.8mm -3.2mm 800]
- ElementLine[3.8mm 2.95mm 2.6mm 4.15mm 800]
- ElementLine[-3.8mm 2.95mm -2.6mm 4.15mm 800]
+ ElementLine[-3.825mm 2.475mm -3.825mm -3mm 0.25mm]
+ ElementLine[3.825mm 2.475mm 3.825mm -3mm 0.25mm]
+ ElementLine[3.825mm 2.475mm 2.625mm 3.675mm 0.25mm]
+ ElementLine[-3.825mm 2.475mm -2.625mm 3.675mm 0.25mm]
+ ElementLine[2.625mm 3.675mm 1.5mm 3.675mm 0.25mm]
+ ElementLine[-2.625mm 3.675mm -1.5mm 3.675mm 0.25mm]
)
diff --git a/motors/packages/CAP_F.fp b/motors/packages/CAP_F.fp
index 74addf8..e020aa9 100644
--- a/motors/packages/CAP_F.fp
+++ b/motors/packages/CAP_F.fp
@@ -1,15 +1,22 @@
# This is the footprint at
# https://web.archive.org/web/20131102032848/http://industrial.panasonic.com/www-data/pdf/ABA0000/ABA0000PE251.pdf
-# for a 8x10.2 capacitor (size F).
+# for a 8x10.2 capacitor (size E or F).
Element[0x0 "" "" "" 0 0 0 0 0 100 0x0]
(
- Pad[0 2.55mm 0 4.55mm 2mm 2000 9874 "1" "1" "square"]
- Pad[0 -2.55mm 0 -4.55mm 2mm 2000 9874 "2" "2" "square"]
+ Pad[0 2.55mm 0 4.55mm 2mm 2000 2.3mm "1" "1" "square"]
+ Pad[0 -2.55mm 0 -4.55mm 2mm 2000 2.3mm "2" "2" "square"]
- ElementLine[-10000 -24000 10000 -24000 1200]
+ ElementLine[-4mm -5mm -1.75mm -5mm 0.5mm]
+ ElementLine[4mm -5mm 1.75mm -5mm 0.5mm]
- ElementLine[-4.25mm 4.25mm -4.25mm -4.25mm 800]
- ElementLine[4.25mm 4.25mm 4.25mm -4.25mm 800]
- ElementLine[4.25mm 4.25mm 2.75mm 5.75mm 800]
- ElementLine[-4.25mm 4.25mm -2.75mm 5.75mm 800]
+ #ElementLine[-4.25mm 4.25mm -4.25mm -4.25mm 800]
+ #ElementLine[4.25mm 4.25mm 4.25mm -4.25mm 800]
+ #ElementLine[4.25mm 4.25mm 2.75mm 5.75mm 800]
+ #ElementLine[-4.25mm 4.25mm -2.75mm 5.75mm 800]
+ ElementLine[-4.75mm 4.325mm -4.75mm -4mm 0.25mm]
+ ElementLine[4.75mm 4.325mm 4.75mm -4mm 0.25mm]
+ ElementLine[4.75mm 4.325mm 3.95mm 5.125mm 0.25mm]
+ ElementLine[-4.75mm 4.325mm -3.95mm 5.125mm 0.25mm]
+ ElementLine[3.95mm 5.125mm 1.8mm 5.125mm 0.25mm]
+ ElementLine[-3.95mm 5.125mm -1.8mm 5.125mm 0.25mm]
)
diff --git a/motors/packages/CAP_G.fp b/motors/packages/CAP_G.fp
index fb24fef..30b0225 100644
--- a/motors/packages/CAP_G.fp
+++ b/motors/packages/CAP_G.fp
@@ -1,16 +1,18 @@
# This is the footprint at
-# http://www.nichicon.co.jp/english/products/pdfs/e-ch_ref.pdf
+# https://web.archive.org/web/20131102032848/http://industrial.panasonic.com/www-data/pdf/ABA0000/ABA0000PE251.pdf
# for a 10mm diameter capacitor (size G).
Element[0x0 "" "" "" 0 0 0 0 0 100 0x0]
(
- Pad[0 5.25mm 0 6.75mm 2.5mm 2000 10743 "1" "1" "square"]
- Pad[0 -5.25mm 0 -6.75mm 2.5mm 2000 10743 "2" "2" "square"]
+ Pad[0 3.3mm 0 5.4mm 2.0mm 2000 2.3mm "1" "1" "square"]
+ Pad[0 -3.3mm 0 -5.4mm 2.0mm 2000 2.3mm "2" "2" "square"]
- ElementLine[-5mm -7.5mm -2mm -7.5mm 2000]
- ElementLine[5mm -7.5mm 2mm -7.5mm 2000]
+ ElementLine[-5mm -5.85mm -2mm -5.85mm 0.5mm]
+ ElementLine[5mm -5.85mm 2mm -5.85mm 0.5mm]
- ElementLine[-5.25mm 5.25mm -5.25mm -5.25mm 800]
- ElementLine[5.25mm 5.25mm 5.25mm -5.25mm 800]
- ElementLine[5.25mm 5.25mm 2.75mm 7.75mm 800]
- ElementLine[-5.25mm 5.25mm -2.75mm 7.75mm 800]
+ ElementLine[-5.75mm 4.275mm -5.75mm -5mm 0.25mm]
+ ElementLine[5.75mm 4.275mm 5.75mm -5mm 0.25mm]
+ ElementLine[5.75mm 4.275mm 4.05mm 5.975mm 0.25mm]
+ ElementLine[-5.75mm 4.275mm -4.05mm 5.975mm 0.25mm]
+ ElementLine[4.05mm 5.975mm 2mm 5.975mm 0.25mm]
+ ElementLine[-4.05mm 5.975mm -2mm 5.975mm 0.25mm]
)
diff --git a/motors/packages/TI_PWP.fp b/motors/packages/TI_PWP.fp
index 0ac3860..34ae3f3 100644
--- a/motors/packages/TI_PWP.fp
+++ b/motors/packages/TI_PWP.fp
@@ -22,8 +22,8 @@
Pad[3.425mm -2.275mm 2.375mm -2.275mm 0.45mm 2000 0.55mm "16" "16" "square"]
# The big power pad in the middle, and the vias in it.
- Pad[0 0.44mm 0 -0.44mm 1000 0 2.41mm "17" "17" "square"]
- Pad[0 0.8mm 0 -0.8mm 3.4mm 2000 1000 "17" "17" "square"]
+ Pad[0 0.44mm 0 -0.44mm 2.41mm 0 2.41mm "17" "17" "square"]
+ Pad[0 0.8mm 0 -0.8mm 3.4mm 2000 1000 "17" "17" "square,nopaste"]
Pin[-1.425mm 1.9mm 31.81mil 20mil 0 11.81mil "17" "17" ""]
Pin[-0.475mm 1.9mm 31.81mil 20mil 0 11.81mil "17" "17" ""]
Pin[0.475mm 1.9mm 31.81mil 20mil 0 11.81mil "17" "17" ""]
diff --git a/motors/packages/WDFN10_4_EP.fp b/motors/packages/WDFN10_4_EP.fp
index 26692f1..e3f00be 100644
--- a/motors/packages/WDFN10_4_EP.fp
+++ b/motors/packages/WDFN10_4_EP.fp
@@ -16,20 +16,20 @@
Pin[-0.75mm -0.75mm 0.75mm 2000 100 0.3mm "11" "11" ""]
# The top row of pads.
- Pad[-1.6mm -1.725mm -1.6mm -2.225mm 0.35mm 0.18mm 0.53mm "1" "1" ""]
- Pad[-0.8mm -1.725mm -0.8mm -2.225mm 0.35mm 0.18mm 0.53mm "2" "2" ""]
- Pad[0mm -1.725mm 0mm -2.225mm 0.35mm 0.18mm 0.53mm "3" "3" ""]
- Pad[0.8mm -1.725mm 0.8mm -2.225mm 0.35mm 0.18mm 0.53mm "4" "4" ""]
- Pad[1.6mm -1.725mm 1.6mm -2.225mm 0.35mm 0.18mm 0.53mm "5" "5" ""]
+ Pad[-1.6mm -1.725mm -1.6mm -2.225mm 0.35mm 0.18mm 0.53mm "10" "10" ""]
+ Pad[-0.8mm -1.725mm -0.8mm -2.225mm 0.35mm 0.18mm 0.53mm "9" "9" ""]
+ Pad[0mm -1.725mm 0mm -2.225mm 0.35mm 0.18mm 0.53mm "8" "8" ""]
+ Pad[0.8mm -1.725mm 0.8mm -2.225mm 0.35mm 0.18mm 0.53mm "7" "7" ""]
+ Pad[1.6mm -1.725mm 1.6mm -2.225mm 0.35mm 0.18mm 0.53mm "6" "6" ""]
# The bottom row of pads.
- Pad[-1.6mm 1.725mm -1.6mm 2.225mm 0.35mm 0.18mm 0.53mm "10" "10" ""]
- Pad[-0.8mm 1.725mm -0.8mm 2.225mm 0.35mm 0.18mm 0.53mm "9" "9" ""]
- Pad[0mm 1.725mm 0mm 2.225mm 0.35mm 0.18mm 0.53mm "8" "8" ""]
- Pad[0.8mm 1.725mm 0.8mm 2.225mm 0.35mm 0.18mm 0.53mm "7" "7" ""]
- Pad[1.6mm 1.725mm 1.6mm 2.225mm 0.35mm 0.18mm 0.53mm "6" "6" ""]
+ Pad[-1.6mm 1.725mm -1.6mm 2.225mm 0.35mm 0.18mm 0.53mm "1" "1" ""]
+ Pad[-0.8mm 1.725mm -0.8mm 2.225mm 0.35mm 0.18mm 0.53mm "2" "2" ""]
+ Pad[0mm 1.725mm 0mm 2.225mm 0.35mm 0.18mm 0.53mm "3" "3" ""]
+ Pad[0.8mm 1.725mm 0.8mm 2.225mm 0.35mm 0.18mm 0.53mm "4" "4" ""]
+ Pad[1.6mm 1.725mm 1.6mm 2.225mm 0.35mm 0.18mm 0.53mm "5" "5" ""]
ElementLine[-2.05mm -2.05mm -2.05mm 2.05mm 800]
ElementLine[2.05mm -2.05mm 2.05mm 2.05mm 800]
- ElementArc[-2.05mm -1.9mm 0.2mm 0.2mm 270 180 800]
+ ElementArc[-2.05mm 1.9mm 0.2mm 0.2mm 270 180 800]
)
diff --git a/y2018/BUILD b/y2018/BUILD
index 67ad583..4edb57e 100644
--- a/y2018/BUILD
+++ b/y2018/BUILD
@@ -1,5 +1,6 @@
load("//aos/downloader:downloader.bzl", "aos_downloader")
load("//aos/build:queues.bzl", "queue_library")
+load("//tools/build_rules:protobuf.bzl", "proto_cc_library")
aos_downloader(
name = "download",
@@ -10,6 +11,7 @@
start_srcs = [
":joystick_reader",
":wpilib_interface",
+ "//y2018/vision:vision_status",
"//aos:prime_start_binaries",
"//y2018/control_loops/drivetrain:drivetrain",
"//y2018/actors:autonomous_action",
@@ -26,6 +28,7 @@
start_srcs = [
":joystick_reader.stripped",
":wpilib_interface.stripped",
+ "//y2018/vision:vision_status.stripped",
"//aos:prime_start_binaries_stripped",
"//y2018/actors:autonomous_action.stripped",
"//y2018/control_loops/drivetrain:drivetrain.stripped",
@@ -39,7 +42,8 @@
"joystick_reader.cc",
],
deps = [
- ":status_light",
+ ":vision_proto",
+ "//aos/common:stl_mutex",
"//aos/common:time",
"//aos/common/actions:action_lib",
"//aos/common/logging",
@@ -47,6 +51,7 @@
"//aos/input:drivetrain_input",
"//aos/input:joystick_input",
"//aos/linux_code:init",
+ "//aos/vision/events:udp",
"//frc971/autonomous:auto_queue",
"//frc971/autonomous:base_autonomous_actor",
"//frc971/control_loops/drivetrain:drivetrain_queue",
@@ -116,6 +121,7 @@
"//third_party/Phoenix-frc-lib:phoenix",
"//y2018:constants",
"//y2018/control_loops/superstructure:superstructure_queue",
+ "//y2018/vision:vision_queue",
],
)
@@ -124,4 +130,11 @@
srcs = [
"status_light.q",
],
+ visibility = ["//visibility:public"],
+)
+
+proto_cc_library(
+ name = "vision_proto",
+ src = "vision.proto",
+ visibility = ["//visibility:public"],
)
diff --git a/y2018/constants.cc b/y2018/constants.cc
index 8180bdc..42c8501 100644
--- a/y2018/constants.cc
+++ b/y2018/constants.cc
@@ -90,20 +90,20 @@
left_intake->zeroing.measured_absolute_position = 0.213653;
left_intake->potentiometer_offset = -5.45258 + 1.299206;
- left_intake->spring_offset = -0.25 - 0.009 + 0.029;
+ left_intake->spring_offset = -0.25 - 0.009 + 0.029 - 0.025;
right_intake->zeroing.measured_absolute_position = 0.37022 - 0.04;
right_intake->potentiometer_offset = 3.739919 + 1.087098 + 0.825;
- right_intake->spring_offset = 0.25 + 0.015;
+ right_intake->spring_offset = 0.25 + 0.015 - 0.025;
arm_proximal->zeroing.measured_absolute_position =
- 0.067941 + 1.047 - 0.116 + 0.06 - 0.004 + 0.009;
+ 0.067941 + 1.047 - 0.116 + 0.06 - 0.004 + 0.009 + 0.0938;
arm_proximal->potentiometer_offset =
- 1.047 - 3.653298 + -0.078 + 0.9455 + 0.265;
+ 1.047 - 3.653298 + -0.078 + 0.9455 + 0.265 - 0.36;
arm_distal->zeroing.measured_absolute_position =
- -0.870445 + 5.209807817203074 + 0.118 - 0.004 + 0.407;
- arm_distal->potentiometer_offset = 5.209807817203074 + 1.250476 + 0.110;
+ -0.870445 + 5.209807817203074 + 0.118 - 0.004 + 0.407 - 0.53;
+ arm_distal->potentiometer_offset = 5.209807817203074 + 1.250476 + 0.110 + 0.52;
break;
case kPracticeTeamNumber:
diff --git a/y2018/control_loops/superstructure/BUILD b/y2018/control_loops/superstructure/BUILD
index 1d52ab4..cc179d5 100644
--- a/y2018/control_loops/superstructure/BUILD
+++ b/y2018/control_loops/superstructure/BUILD
@@ -25,9 +25,12 @@
":superstructure_queue",
"//aos/common/controls:control_loop",
"//frc971/control_loops:queues",
+ "//frc971/control_loops/drivetrain:drivetrain_queue",
"//y2018:constants",
+ "//y2018:status_light",
"//y2018/control_loops/superstructure/arm",
"//y2018/control_loops/superstructure/intake",
+ "//y2018/vision:vision_queue",
],
)
@@ -61,3 +64,24 @@
"//aos/linux_code:init",
],
)
+
+cc_library(
+ name = "debouncer",
+ hdrs = [
+ "debouncer.h",
+ ],
+ srcs = [
+ "debouncer.cc",
+ ],
+)
+
+cc_test(
+ name = "debouncer_test",
+ srcs = [
+ "debouncer_test.cc",
+ ],
+ deps = [
+ ":debouncer",
+ "//aos/testing:googletest",
+ ],
+)
diff --git a/y2018/control_loops/superstructure/arm/graph.cc b/y2018/control_loops/superstructure/arm/graph.cc
index 6544a8e..54ec79d 100644
--- a/y2018/control_loops/superstructure/arm/graph.cc
+++ b/y2018/control_loops/superstructure/arm/graph.cc
@@ -9,7 +9,7 @@
namespace arm {
SearchGraph::SearchGraph(size_t num_vertexes, std::initializer_list<Edge> edges)
- : SearchGraph(num_vertexes, ::std::move(::std::vector<Edge>(edges))) {}
+ : SearchGraph(num_vertexes, ::std::vector<Edge>(edges)) {}
SearchGraph::SearchGraph(size_t num_vertexes, std::vector<Edge> &&edges)
: edges_(::std::move(edges)) {
diff --git a/y2018/control_loops/superstructure/debouncer.cc b/y2018/control_loops/superstructure/debouncer.cc
new file mode 100644
index 0000000..40b730d
--- /dev/null
+++ b/y2018/control_loops/superstructure/debouncer.cc
@@ -0,0 +1,25 @@
+#include "y2018/control_loops/superstructure/debouncer.h"
+
+namespace y2018 {
+namespace control_loops {
+namespace superstructure {
+
+void Debouncer::Update(bool new_state) {
+ // If the incoming state is different from the one we have stored, increment
+ // the counter.
+ if (new_state != current_state_) {
+ consistent_count_++;
+ } else {
+ consistent_count_ = 0;
+ }
+
+ // If we have reached the number required to change the state, change it.
+ if (consistent_count_ >= inputs_before_change_) {
+ current_state_ = new_state;
+ consistent_count_ = 0;
+ }
+}
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2018
diff --git a/y2018/control_loops/superstructure/debouncer.h b/y2018/control_loops/superstructure/debouncer.h
new file mode 100644
index 0000000..ad46b51
--- /dev/null
+++ b/y2018/control_loops/superstructure/debouncer.h
@@ -0,0 +1,44 @@
+#ifndef Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_DEBOUNCER_H_
+#define Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_DEBOUNCER_H_
+
+namespace y2018 {
+namespace control_loops {
+namespace superstructure {
+
+// Ensures that a certain number of states of a certain type are recieved before
+// the actual state is changed.
+class Debouncer {
+ public:
+ // Parameters:
+ // - initial_state: the initial state of the debouncer. (assigned to
+ // current_state)
+ // - inputs_before_change: the number of inputs of the same type (true or
+ // false) required before the debouncer state is changed.
+ Debouncer(bool initial_state, int inputs_before_change)
+ : current_state_(initial_state),
+ inputs_before_change_(inputs_before_change) {}
+
+ // Updates the debounder state with a new input value.
+ void Update(bool new_state);
+
+ // Retrieves the current debouncer state.
+ bool current_state() const { return current_state_; }
+
+ private:
+ // Stores the current debouncer state.
+ bool current_state_;
+
+ // Stores the number of inputs of the same type (true or false) required
+ // before the debouncer state changes.
+ const int inputs_before_change_;
+
+ // Stores the temporary count of inputs of the same type. When this number
+ // reaches inputs_before_change_, the debouncer state changes.
+ int consistent_count_ = 0;
+};
+
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2018
+
+#endif // Y2018_CONTROL_LOOPS_SUPERSTRUCTURE_DEBOUNCER_H_
diff --git a/y2018/control_loops/superstructure/debouncer_test.cc b/y2018/control_loops/superstructure/debouncer_test.cc
new file mode 100644
index 0000000..2f56d56
--- /dev/null
+++ b/y2018/control_loops/superstructure/debouncer_test.cc
@@ -0,0 +1,59 @@
+#include "y2018/control_loops/superstructure/debouncer.h"
+
+#include "gtest/gtest.h"
+
+namespace y2018 {
+namespace control_loops {
+namespace superstructure {
+namespace testing {
+
+// Tests that the debouncer behaves as it should. This tests the following:
+// - The debouncer changes its internal state after the desired number of
+// repeated inputs.
+// - The debouncer doesn't change its internal state before the desired number
+// of repeated inputs.
+TEST(DebouncerTest, Debouncer) {
+ Debouncer bouncer(false, 2);
+
+ bouncer.Update(true);
+ bouncer.Update(true);
+ EXPECT_EQ(true, bouncer.current_state());
+
+ bouncer.Update(false);
+
+ // Only one false, state shouldn't have changed.
+ EXPECT_EQ(true, bouncer.current_state());
+
+ bouncer.Update(false);
+ // Now there are two falses in a row, the state should've changed.
+ EXPECT_EQ(false, bouncer.current_state());
+}
+
+// Test that the debouncer will hold its state through a short-lived state
+// change.
+TEST(DebouncerTest, DebouncerLongSequence) {
+ Debouncer bouncer(false, 2);
+
+ bouncer.Update(true);
+
+ // Only one true, should still read false.
+ EXPECT_EQ(false, bouncer.current_state());
+
+ bouncer.Update(true);
+
+ // Two trues, should now read true.
+ EXPECT_EQ(true, bouncer.current_state());
+
+ bouncer.Update(false);
+
+ // Only one false, should still read true.
+ EXPECT_EQ(true, bouncer.current_state());
+
+ bouncer.Update(true);
+
+ EXPECT_EQ(true, bouncer.current_state());
+}
+} // namespace testing
+} // namespace superstructure
+} // namespace control_loops
+} // namespace y2018
diff --git a/y2018/control_loops/superstructure/superstructure.cc b/y2018/control_loops/superstructure/superstructure.cc
index 757b166..2098cd0 100644
--- a/y2018/control_loops/superstructure/superstructure.cc
+++ b/y2018/control_loops/superstructure/superstructure.cc
@@ -5,8 +5,11 @@
#include "aos/common/controls/control_loops.q.h"
#include "aos/common/logging/logging.h"
#include "frc971/control_loops/control_loops.q.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "y2018/constants.h"
#include "y2018/control_loops/superstructure/intake/intake.h"
+#include "y2018/status_light.q.h"
+#include "y2018/vision/vision.q.h"
namespace y2018 {
namespace control_loops {
@@ -21,6 +24,17 @@
constexpr double kMaxIntakeRollerVoltage = 12.0;
} // namespace
+void SendColors(float red, float green, float blue) {
+ auto new_status_light = status_light.MakeMessage();
+ new_status_light->red = red;
+ new_status_light->green = green;
+ new_status_light->blue = blue;
+
+ if (!new_status_light.Send()) {
+ LOG(ERROR, "Failed to send lights.\n");
+ }
+}
+
Superstructure::Superstructure(
control_loops::SuperstructureQueue *superstructure_queue)
: aos::controls::ControlLoop<control_loops::SuperstructureQueue>(
@@ -237,6 +251,34 @@
stuck_count_ = 0;
}
status->rotation_state = static_cast<uint32_t>(rotation_state_);
+
+ ::frc971::control_loops::drivetrain_queue.output.FetchLatest();
+
+ ::y2018::vision::vision_status.FetchLatest();
+ if (status->estopped) {
+ SendColors(0.5, 0.0, 0.0);
+ } else if (!y2018::vision::vision_status.get() ||
+ y2018::vision::vision_status.Age() > chrono::seconds(1)) {
+ SendColors(0.5, 0.5, 0.0);
+ } else if (rotation_state_ == RotationState::ROTATING_LEFT ||
+ rotation_state_ == RotationState::ROTATING_RIGHT) {
+ SendColors(0.5, 0.20, 0.0);
+ } else if (rotation_state_ == RotationState::STUCK) {
+ SendColors(0.5, 0.0, 0.5);
+ } else if (position->box_back_beambreak_triggered) {
+ SendColors(0.0, 0.0, 0.5);
+ } else if (position->box_distance < 0.2) {
+ SendColors(0.0, 0.5, 0.0);
+ } else if (::frc971::control_loops::drivetrain_queue.output.get() &&
+ ::std::max(::std::abs(::frc971::control_loops::drivetrain_queue
+ .output->left_voltage),
+ ::std::abs(::frc971::control_loops::drivetrain_queue
+ .output->right_voltage)) > 11.5) {
+ SendColors(0.5, 0.0, 0.5);
+ } else {
+ SendColors(0.0, 0.0, 0.0);
+ }
+
last_box_distance_ = clipped_box_distance;
}
diff --git a/y2018/control_loops/superstructure/superstructure_lib_test.cc b/y2018/control_loops/superstructure/superstructure_lib_test.cc
index 639a683..d21d1ce 100644
--- a/y2018/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2018/control_loops/superstructure/superstructure_lib_test.cc
@@ -7,6 +7,7 @@
#include "aos/common/controls/control_loop_test.h"
#include "aos/common/queue.h"
+#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/position_sensor_sim.h"
#include "frc971/control_loops/team_number_test_environment.h"
#include "gtest/gtest.h"
@@ -14,6 +15,8 @@
#include "y2018/control_loops/superstructure/arm/dynamics.h"
#include "y2018/control_loops/superstructure/arm/generated_graph.h"
#include "y2018/control_loops/superstructure/intake/intake_plant.h"
+#include "y2018/status_light.q.h"
+#include "y2018/vision/vision.q.h"
using ::frc971::control_loops::PositionSensorSimulator;
@@ -271,6 +274,9 @@
".y2018.control_loops.superstructure.output",
".y2018.control_loops.superstructure.status"),
superstructure_(&superstructure_queue_) {
+ status_light.Clear();
+ ::y2018::vision::vision_status.Clear();
+ ::frc971::control_loops::drivetrain_queue.output.Clear();
set_team_id(::frc971::control_loops::testing::kTeamNumber);
}
diff --git a/y2018/joystick_reader.cc b/y2018/joystick_reader.cc
index 4f8a410..61e3d9f 100644
--- a/y2018/joystick_reader.cc
+++ b/y2018/joystick_reader.cc
@@ -2,26 +2,31 @@
#include <stdio.h>
#include <string.h>
#include <unistd.h>
+#include <mutex>
#include "aos/common/actions/actions.h"
#include "aos/common/input/driver_station_data.h"
#include "aos/common/logging/logging.h"
+#include "aos/common/stl_mutex.h"
#include "aos/common/time.h"
#include "aos/common/util/log_interval.h"
#include "aos/input/drivetrain_input.h"
#include "aos/input/joystick_input.h"
#include "aos/linux_code/init.h"
+#include "aos/vision/events/udp.h"
#include "frc971/autonomous/auto.q.h"
#include "frc971/autonomous/base_autonomous_actor.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "y2018/control_loops/drivetrain/drivetrain_base.h"
#include "y2018/control_loops/superstructure/arm/generated_graph.h"
#include "y2018/control_loops/superstructure/superstructure.q.h"
-#include "y2018/status_light.q.h"
+#include "y2018/vision.pb.h"
+using ::aos::monotonic_clock;
using ::frc971::control_loops::drivetrain_queue;
using ::y2018::control_loops::superstructure_queue;
-
+using ::aos::events::ProtoTXUdpSocket;
+using ::aos::events::RXUdpSocket;
using ::aos::input::driver_station::ButtonLocation;
using ::aos::input::driver_station::ControlBit;
using ::aos::input::driver_station::JoystickAxis;
@@ -78,7 +83,7 @@
class Reader : public ::aos::input::JoystickInput {
public:
- Reader() {
+ Reader() : video_tx_("10.9.71.179", 5000) {
drivetrain_input_reader_ = DrivetrainInputReader::Make(
DrivetrainInputReader::InputType::kPistol,
::y2018::control_loops::drivetrain::GetDrivetrainConfig());
@@ -110,17 +115,6 @@
drivetrain_input_reader_->HandleDrivetrain(data);
}
- void SendColors(float red, float green, float blue) {
- auto new_status_light = status_light.MakeMessage();
- new_status_light->red = red;
- new_status_light->green = green;
- new_status_light->blue = blue;
-
- if (!new_status_light.Send()) {
- LOG(ERROR, "Failed to send lights.\n");
- }
- }
-
void HandleTeleop(const ::aos::input::driver_station::Data &data) {
if (!data.GetControlBit(ControlBit::kEnabled)) {
action_queue_.CancelAllActions();
@@ -140,6 +134,11 @@
new_superstructure_goal->intake.left_intake_angle = intake_goal_;
new_superstructure_goal->intake.right_intake_angle = intake_goal_;
+ if (data.PosEdge(kIntakeIn) || data.PosEdge(kSmallBox) ||
+ data.PosEdge(kIntakeClosed)) {
+ vision_control_.set_high_video(false);
+ }
+
if (data.IsPressed(kIntakeIn)) {
// Turn on the rollers.
new_superstructure_goal->intake.roller_voltage = 8.0;
@@ -151,14 +150,6 @@
new_superstructure_goal->intake.roller_voltage = 0.0;
}
- if (superstructure_queue.position->box_back_beambreak_triggered) {
- SendColors(0.0, 0.5, 0.0);
- } else if (superstructure_queue.position->box_distance < 0.2) {
- SendColors(0.0, 0.0, 0.5);
- } else {
- SendColors(0.0, 0.0, 0.0);
- }
-
if (data.IsPressed(kSmallBox)) {
// Deploy the intake.
if (superstructure_queue.position->box_back_beambreak_triggered) {
@@ -255,24 +246,33 @@
}
}
} else if (data.PosEdge(kArmPickupBoxFromIntake)) {
+ vision_control_.set_high_video(false);
arm_goal_position_ = arm::NeutralIndex();
} else if (data.IsPressed(kDuck)) {
+ vision_control_.set_high_video(false);
arm_goal_position_ = arm::DuckIndex();
} else if (data.IsPressed(kArmNeutral)) {
+ vision_control_.set_high_video(false);
arm_goal_position_ = arm::NeutralIndex();
} else if (data.IsPressed(kArmUp)) {
+ vision_control_.set_high_video(true);
arm_goal_position_ = arm::UpIndex();
} else if (data.IsPressed(kArmFrontSwitch)) {
arm_goal_position_ = arm::FrontSwitchIndex();
} else if (data.IsPressed(kArmFrontExtraHighBox)) {
+ vision_control_.set_high_video(true);
arm_goal_position_ = arm::FrontHighBoxIndex();
} else if (data.IsPressed(kArmFrontHighBox)) {
+ vision_control_.set_high_video(true);
arm_goal_position_ = arm::FrontMiddle2BoxIndex();
} else if (data.IsPressed(kArmFrontMiddle2Box)) {
+ vision_control_.set_high_video(true);
arm_goal_position_ = arm::FrontMiddle3BoxIndex();
} else if (data.IsPressed(kArmFrontMiddle1Box)) {
+ vision_control_.set_high_video(true);
arm_goal_position_ = arm::FrontMiddle1BoxIndex();
} else if (data.IsPressed(kArmFrontLowBox)) {
+ vision_control_.set_high_video(true);
arm_goal_position_ = arm::FrontLowBoxIndex();
} else if (data.IsPressed(kArmBackExtraHighBox)) {
arm_goal_position_ = arm::BackHighBoxIndex();
@@ -328,6 +328,8 @@
if (!new_superstructure_goal.Send()) {
LOG(ERROR, "Sending superstructure goal failed.\n");
}
+
+ video_tx_.Send(vision_control_);
}
private:
@@ -362,11 +364,14 @@
bool never_disabled_ = true;
uint32_t arm_goal_position_ = 0;
+ VisionControl vision_control_;
decltype(FrontPoints()) front_points_ = FrontPoints();
decltype(BackPoints()) back_points_ = BackPoints();
::aos::common::actions::ActionQueue action_queue_;
+
+ ProtoTXUdpSocket<VisionControl> video_tx_;
};
} // namespace joysticks
diff --git a/y2018/vision.proto b/y2018/vision.proto
new file mode 100644
index 0000000..04b3181
--- /dev/null
+++ b/y2018/vision.proto
@@ -0,0 +1,12 @@
+syntax = "proto2";
+
+package y2018;
+
+message VisionControl {
+ optional bool high_video = 1;
+}
+
+message VisionStatus {
+ optional uint32 high_frame_count = 1;
+ optional uint32 low_frame_count = 2;
+}
diff --git a/y2018/vision/BUILD b/y2018/vision/BUILD
index 11fb7c5..dfe5ebb 100644
--- a/y2018/vision/BUILD
+++ b/y2018/vision/BUILD
@@ -1,13 +1,43 @@
+load("//aos/build:queues.bzl", "queue_library")
+
cc_binary(
- name = "image_streamer",
- srcs = ["image_streamer.cc"],
- deps = [
- "//aos/vision/events:socket_types",
- '//aos/vision/events:epoll_events',
- '//aos/vision/image:reader',
- '//aos/vision/image:image_stream',
- '//aos/vision/blob:codec',
- '//aos/common/logging:logging',
- '//aos/common/logging:implementations',
- ],
+ name = "image_streamer",
+ srcs = ["image_streamer.cc"],
+ deps = [
+ "//aos/common/logging",
+ "//aos/common/logging:implementations",
+ "//aos/vision/blob:codec",
+ "//aos/vision/events:epoll_events",
+ "//aos/vision/events:socket_types",
+ "//aos/vision/events:udp",
+ "//aos/vision/image:image_stream",
+ "//aos/vision/image:reader",
+ "//third_party/gflags",
+ "//y2018:vision_proto",
+ ],
+)
+
+queue_library(
+ name = "vision_queue",
+ srcs = [
+ "vision.q",
+ ],
+ visibility = ["//visibility:public"],
+)
+
+cc_binary(
+ name = "vision_status",
+ srcs = [
+ "vision_status.cc",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":vision_queue",
+ "//aos/common:time",
+ "//aos/common/logging",
+ "//aos/common/logging:queue_logging",
+ "//aos/linux_code:init",
+ "//aos/vision/events:udp",
+ "//y2018:vision_proto",
+ ],
)
diff --git a/y2018/vision/exposure_2018.sh b/y2018/vision/exposure_2018.sh
index c96f5f0..bfcd31c 100755
--- a/y2018/vision/exposure_2018.sh
+++ b/y2018/vision/exposure_2018.sh
@@ -2,18 +2,20 @@
set -e
-A=`ls /dev/video*`
EXPOSURE='100'
echo $SHELL
-echo $A
# Michael added this one to try and have the exposer set correctly sooner.
sleep 1
echo Setting exposure again after 1 seconds
-v4l2-ctl --set-ctrl="exposure_absolute=$EXPOSURE" -d $A
+for CAMERA in /dev/video0 /dev/video1
+do
+ echo "${CAMERA}"
+ v4l2-ctl --set-ctrl="exposure_absolute=$EXPOSURE" -d "${CAMERA}"
+done
echo Done setting exposure again after 1 seconds
@@ -21,8 +23,11 @@
# Michael added this one to try and have the exposer set correctly sooner.
sleep 5
echo Setting exposure again after 5 seconds
-v4l2-ctl --set-ctrl="exposure_absolute=$EXPOSURE" -d $A
-
+for CAMERA in /dev/video0 /dev/video1
+do
+ echo "${CAMERA}"
+ v4l2-ctl --set-ctrl="exposure_absolute=$EXPOSURE" -d "${CAMERA}"
+done
echo Done setting exposure again after 5 seconds
diff --git a/y2018/vision/image_streamer.cc b/y2018/vision/image_streamer.cc
index ce6f5e2..93f8c58 100644
--- a/y2018/vision/image_streamer.cc
+++ b/y2018/vision/image_streamer.cc
@@ -1,28 +1,41 @@
-#include "aos/vision/events/socket_types.h"
-#include "aos/vision/image/reader.h"
#include "aos/vision/image/image_stream.h"
+
+#include <sys/stat.h>
+#include <deque>
+#include <fstream>
+#include <string>
+
#include "aos/common/logging/implementations.h"
#include "aos/common/logging/logging.h"
#include "aos/vision/blob/codec.h"
-#include <fstream>
-#include <sys/stat.h>
-#include <deque>
-#include <string>
+#include "aos/vision/events/socket_types.h"
+#include "aos/vision/events/udp.h"
+#include "aos/vision/image/reader.h"
+#include "third_party/gflags/include/gflags/gflags.h"
+#include "y2018/vision.pb.h"
-using aos::events::TCPServer;
-using aos::events::DataSocket;
-using aos::vision::Int32Codec;
-using aos::vision::DataRef;
+using ::aos::events::DataSocket;
+using ::aos::events::RXUdpSocket;
+using ::aos::events::TCPServer;
+using ::aos::vision::DataRef;
+using ::aos::vision::Int32Codec;
+using ::aos::monotonic_clock;
+using ::y2018::VisionControl;
-aos::vision::DataRef mjpg_header = "HTTP/1.0 200 OK\r\n"\
- "Server: YourServerName\r\n"\
- "Connection: close\r\n"\
- "Max-Age: 0\r\n"\
- "Expires: 0\r\n"\
- "Cache-Control: no-cache, private\r\n"\
- "Pragma: no-cache\r\n"\
- "Content-Type: multipart/x-mixed-replace; "\
- "boundary=--boundary\r\n\r\n";
+DEFINE_bool(single_camera, true, "If true, only use video0");
+DEFINE_int32(camera0_exposure, 300, "Exposure for video0");
+DEFINE_int32(camera1_exposure, 300, "Exposure for video1");
+
+aos::vision::DataRef mjpg_header =
+ "HTTP/1.0 200 OK\r\n"
+ "Server: YourServerName\r\n"
+ "Connection: close\r\n"
+ "Max-Age: 0\r\n"
+ "Expires: 0\r\n"
+ "Cache-Control: no-cache, private\r\n"
+ "Pragma: no-cache\r\n"
+ "Content-Type: multipart/x-mixed-replace; "
+ "boundary=--boundary\r\n\r\n";
struct Frame {
std::string data;
@@ -58,11 +71,49 @@
std::ofstream ofst_;
};
+class UdpClient : public ::aos::events::EpollEvent {
+ public:
+ UdpClient(int port, ::std::function<void(void *, size_t)> callback)
+ : ::aos::events::EpollEvent(RXUdpSocket::SocketBindListenOnPort(port)),
+ callback_(callback) {}
+
+ private:
+ ::std::function<void(void *, size_t)> callback_;
+
+ void ReadEvent() override {
+ char data[1024];
+ size_t received_data_size = Recv(data, sizeof(data));
+ callback_(data, received_data_size);
+ }
+
+ size_t Recv(void *data, int size) {
+ return PCHECK(recv(fd(), static_cast<char *>(data), size, 0));
+ }
+};
+
+template <typename PB>
+class ProtoUdpClient : public UdpClient {
+ public:
+ ProtoUdpClient(int port, ::std::function<void(const PB &)> proto_callback)
+ : UdpClient(port, ::std::bind(&ProtoUdpClient::ReadData, this,
+ ::std::placeholders::_1,
+ ::std::placeholders::_2)),
+ proto_callback_(proto_callback) {}
+
+ private:
+ ::std::function<void(const PB &)> proto_callback_;
+
+ void ReadData(void *data, size_t size) {
+ PB pb;
+ pb.ParseFromArray(data, size);
+ proto_callback_(pb);
+ }
+};
+
class MjpegDataSocket : public aos::events::SocketConnection {
public:
-
- MjpegDataSocket(aos::events::TCPServerBase *serv, int fd)
- : aos::events::SocketConnection(serv, fd) {
+ MjpegDataSocket(aos::events::TCPServerBase *server, int fd)
+ : aos::events::SocketConnection(server, fd) {
SetEvents(EPOLLOUT | EPOLLET);
}
@@ -73,6 +124,12 @@
NewDataToSend();
events &= ~EPOLLOUT;
}
+ // Other end hung up. Ditch the connection.
+ if (events & EPOLLHUP) {
+ CloseConnection();
+ events &= ~EPOLLHUP;
+ return;
+ }
if (events) {
aos::events::EpollEvent::DirectEvent(events);
}
@@ -124,9 +181,10 @@
aos::vision::DataRef data = frame->data;
size_t n_written = snprintf(data_header_tmp_, sizeof(data_header_tmp_),
- "--boundary\r\n"\
- "Content-type: image/jpg\r\n"\
- "Content-Length: %zu\r\n\r\n", data.size());
+ "--boundary\r\n"
+ "Content-type: image/jpg\r\n"
+ "Content-Length: %zu\r\n\r\n",
+ data.size());
// This should never happen because the buffer should be properly sized.
if (n_written == sizeof(data_header_tmp_)) {
fprintf(stderr, "wrong sized buffer\n");
@@ -138,13 +196,11 @@
NewDataToSend();
}
- void NewFrame(std::shared_ptr<Frame> frame) {
- RasterFrame(std::move(frame));
- }
+ void NewFrame(std::shared_ptr<Frame> frame) { RasterFrame(std::move(frame)); }
void NewDataToSend() {
while (!output_buffer_.empty()) {
- auto& data = *output_buffer_.begin();
+ auto &data = *output_buffer_.begin();
while (!data.empty()) {
int len = send(fd(), data.data(), data.size(), MSG_NOSIGNAL);
@@ -178,16 +234,25 @@
size_t match_i_ = 0;
};
-using namespace aos::vision;
-class CameraStream : public ImageStreamEvent {
+class CameraStream : public ::aos::vision::ImageStreamEvent {
public:
- CameraStream(aos::vision::CameraParams params,
- const std::string &fname, TCPServer<MjpegDataSocket>* tcp_serv)
- : ImageStreamEvent(fname, params), tcp_serv_(tcp_serv),
- log_("./logging/blob_record_", ".dat") {}
+ CameraStream(::aos::vision::CameraParams params, const ::std::string &fname,
+ TCPServer<MjpegDataSocket> *tcp_server, bool log,
+ ::std::function<void()> frame_callback)
+ : ImageStreamEvent(fname, params),
+ tcp_server_(tcp_server),
+ frame_callback_(frame_callback) {
+ if (log) {
+ log_.reset(new BlobLog("./logging/blob_record_", ".dat"));
+ }
+ }
- void ProcessImage(DataRef data, aos::monotonic_clock::time_point tp) {
- (void)tp;
+ void set_active(bool active) { active_ = active; }
+
+ bool active() const { return active_; }
+
+ void ProcessImage(DataRef data,
+ monotonic_clock::time_point /*monotonic_now*/) {
++sampling;
// 20 is the sampling rate.
if (sampling == 20) {
@@ -199,36 +264,98 @@
buf = Int32Codec::Write(&log_record[0], tmp_size);
data.copy(buf, data.size());
}
- log_.WriteLogEntry(log_record);
+ if (log_) {
+ log_->WriteLogEntry(log_record);
+ }
sampling = 0;
}
- auto frame = std::make_shared<Frame>(Frame{std::string(data)});
- tcp_serv_->Broadcast([frame](MjpegDataSocket* event) {
- event->NewFrame(frame);
- });
+ if (active_) {
+ auto frame = std::make_shared<Frame>(Frame{std::string(data)});
+ tcp_server_->Broadcast(
+ [frame](MjpegDataSocket *event) { event->NewFrame(frame); });
+ }
+ frame_callback_();
}
+
private:
int sampling = 0;
- TCPServer<MjpegDataSocket>* tcp_serv_;
- BlobLog log_;
+ TCPServer<MjpegDataSocket> *tcp_server_;
+ ::std::unique_ptr<BlobLog> log_;
+ ::std::function<void()> frame_callback_;
+ bool active_ = false;
};
-int main() {
+int main(int argc, char ** argv) {
+ gflags::ParseCommandLineFlags(&argc, &argv, false);
::aos::logging::Init();
::aos::logging::AddImplementation(
new ::aos::logging::StreamLogImplementation(stderr));
- TCPServer<MjpegDataSocket> tcp_serv_(80);
- aos::vision::CameraParams params;
- params.set_exposure(100);
- params.set_width(320);
- params.set_height(240);
- CameraStream camera(params, "/dev/video0", &tcp_serv_);
+ TCPServer<MjpegDataSocket> tcp_server_(80);
+ aos::vision::CameraParams params0;
+ params0.set_exposure(FLAGS_camera0_exposure);
+ params0.set_brightness(-40);
+ params0.set_width(320);
+ //params0.set_fps(10);
+ params0.set_height(240);
+
+ aos::vision::CameraParams params1 = params0;
+ params1.set_exposure(FLAGS_camera1_exposure);
+
+ ::y2018::VisionStatus vision_status;
+ ::aos::events::ProtoTXUdpSocket<::y2018::VisionStatus> status_socket(
+ "10.9.71.2", 5001);
+
+ ::std::unique_ptr<CameraStream> camera1;
+ ::std::unique_ptr<CameraStream> camera0(new CameraStream(
+ params0, "/dev/video0", &tcp_server_, true,
+ [&camera0, &camera1, &status_socket, &vision_status]() {
+ vision_status.set_low_frame_count(vision_status.low_frame_count() + 1);
+ LOG(INFO, "Got a frame cam0\n");
+ if (camera0->active()) {
+ status_socket.Send(vision_status);
+ }
+ }));
+ if (!FLAGS_single_camera) {
+ camera1.reset(new CameraStream(
+ // params,
+ // "/dev/v4l/by-path/platform-tegra-xhci-usb-0:3.1:1.0-video-index0",
+ params1, "/dev/video1", &tcp_server_, false,
+ [&camera0, &camera1, &status_socket, &vision_status]() {
+ vision_status.set_high_frame_count(vision_status.high_frame_count() +
+ 1);
+ LOG(INFO, "Got a frame cam1\n");
+ if (camera1->active()) {
+ status_socket.Send(vision_status);
+ }
+ }));
+ }
+
+ ProtoUdpClient<VisionControl> udp_client(
+ 5000, [&camera0, &camera1](const VisionControl &vision_control) {
+ LOG(INFO, "Got control packet\n");
+ if (camera1) {
+ camera0->set_active(!vision_control.high_video());
+ camera1->set_active(vision_control.high_video());
+ } else {
+ camera0->set_active(true);
+ }
+ });
+
+ // Default to camera0
+ camera0->set_active(true);
+ if (camera1) {
+ camera1->set_active(false);
+ }
aos::events::EpollLoop loop;
- loop.Add(&tcp_serv_);
- loop.Add(&camera);
+ loop.Add(&tcp_server_);
+ loop.Add(camera0.get());
+ if (camera1) {
+ loop.Add(camera1.get());
+ }
+ loop.Add(&udp_client);
printf("Running Camera\n");
loop.Run();
diff --git a/y2018/vision/startup.sh b/y2018/vision/startup.sh
index 6cb211a..7122c01 100755
--- a/y2018/vision/startup.sh
+++ b/y2018/vision/startup.sh
@@ -30,25 +30,25 @@
# echo All done setting exposure
# echo "Starting target sender now."
-A=`ls /dev/video*`
-echo $SHELL
+for CAMERA in /dev/video0 /dev/video1
+do
+ echo $CAMERA
-echo $A
+ v4l2-ctl --set-ctrl="exposure_auto=1" -d $CAMERA
+ sleep 0.5
+ v4l2-ctl --set-ctrl="exposure_absolute=100" -d $CAMERA
+ sleep 0.5
-v4l2-ctl --set-ctrl="exposure_auto=1" -d $A
-sleep 0.5
-v4l2-ctl --set-ctrl="exposure_absolute=100" -d $A
-sleep 0.5
+ PATH="./;$PATH"
-PATH="./;$PATH"
-
-/root/camera_primer $A
+ /root/camera_primer $CAMERA
+done
# Run a script to reset the exposure a few times and exit.
/root/exposure_2018.sh &
-/root/image_streamer
+exec /root/image_streamer --single_camera=false
#exec ./target_sender Practice
#exec ./target_sender Comp
#exec ./target_sender Spare
diff --git a/y2018/vision/vision.q b/y2018/vision/vision.q
new file mode 100644
index 0000000..a3b57ea
--- /dev/null
+++ b/y2018/vision/vision.q
@@ -0,0 +1,7 @@
+package y2018.vision;
+
+message VisionStatus {
+ uint32_t high_frame_count;
+ uint32_t low_frame_count;
+};
+queue VisionStatus vision_status;
diff --git a/y2018/vision/vision_status.cc b/y2018/vision/vision_status.cc
new file mode 100644
index 0000000..e8ac6db
--- /dev/null
+++ b/y2018/vision/vision_status.cc
@@ -0,0 +1,42 @@
+#include <netdb.h>
+
+#include "aos/common/logging/logging.h"
+#include "aos/common/logging/queue_logging.h"
+#include "aos/common/time.h"
+#include "aos/linux_code/init.h"
+#include "aos/vision/events/udp.h"
+#include "y2018/vision.pb.h"
+#include "y2018/vision/vision.q.h"
+
+namespace y2018 {
+namespace vision {
+
+using aos::monotonic_clock;
+
+int Main() {
+ ::aos::events::RXUdpSocket video_rx(5001);
+ char data[65507];
+ ::y2018::VisionStatus status;
+
+ while (true) {
+ const ssize_t rx_size = video_rx.Recv(data, sizeof(data));
+ if (rx_size > 0) {
+ status.ParseFromArray(data, rx_size);
+ auto new_vision_status = vision_status.MakeMessage();
+ new_vision_status->high_frame_count = status.high_frame_count();
+ new_vision_status->low_frame_count = status.low_frame_count();
+ LOG_STRUCT(DEBUG, "vision", *new_vision_status);
+ if (!new_vision_status.Send()) {
+ LOG(ERROR, "Failed to send vision information\n");
+ }
+ }
+ }
+}
+
+} // namespace vision
+} // namespace y2018
+
+int main(int /*argc*/, char ** /*argv*/) {
+ ::aos::InitNRT();
+ ::y2018::vision::Main();
+}
diff --git a/y2018/wpilib_interface.cc b/y2018/wpilib_interface.cc
index f204605..ec54da8 100644
--- a/y2018/wpilib_interface.cc
+++ b/y2018/wpilib_interface.cc
@@ -52,6 +52,7 @@
#include "y2018/constants.h"
#include "y2018/control_loops/superstructure/superstructure.q.h"
#include "y2018/status_light.q.h"
+#include "y2018/vision/vision.q.h"
#ifndef M_PI
#define M_PI 3.14159265358979323846
@@ -660,35 +661,66 @@
LOG_STRUCT(DEBUG, "pneumatics info", to_log);
}
- static double last_red = -1.0;
- static double last_green = -1.0;
- static double last_blue = -1.0;
status_light.FetchLatest();
- if (status_light.get()) {
- LOG_STRUCT(DEBUG, "writing", *status_light);
- // Not sure which of these is red vs green. We're not ready to use
- // either, so just turn them off.
- if (status_light->green != last_green) {
- canifier_.SetLEDOutput(1.0 - status_light->green,
- ::ctre::phoenix::CANifier::LEDChannelA);
- last_green = status_light->green;
+ // If we don't have a light request (or it's an old one), we are borked.
+ // Flash the red light slowly.
+ if (!status_light.get() ||
+ status_light.Age() > chrono::milliseconds(100)) {
+ StatusLight color;
+ color.red = 0.0;
+ color.green = 0.0;
+ color.blue = 0.0;
+
+ ::y2018::vision::vision_status.FetchLatest();
+ ++light_flash_;
+ if (light_flash_ > 10) {
+ color.red = 0.5;
+ } else if (!y2018::vision::vision_status.get() ||
+ y2018::vision::vision_status.Age() > chrono::seconds(1)) {
+ color.red = 0.5;
+ color.green = 0.5;
}
- if (status_light->blue != last_blue) {
- canifier_.SetLEDOutput(1.0 - status_light->blue,
- ::ctre::phoenix::CANifier::LEDChannelB);
- last_blue = status_light->blue;
+ if (light_flash_ > 20) {
+ light_flash_ = 0;
}
- if (status_light->red != last_red) {
- canifier_.SetLEDOutput(1.0 - status_light->red,
- ::ctre::phoenix::CANifier::LEDChannelC);
- last_red = status_light->red;
- }
+ LOG_STRUCT(DEBUG, "color", color);
+ SetColor(color);
+ } else {
+ LOG_STRUCT(DEBUG, "color", *status_light);
+ SetColor(*status_light);
}
}
}
+ void SetColor(const StatusLight &status_light) {
+ // Save CAN bandwidth and CPU at the cost of RT. Only change the light when
+ // it actually changes. This is pretty low priority anyways.
+ static int time_since_last_send = 0;
+ ++time_since_last_send;
+ if (time_since_last_send > 10) {
+ time_since_last_send = 0;
+ }
+ if (status_light.green != last_green_ || time_since_last_send == 0) {
+ canifier_.SetLEDOutput(1.0 - status_light.green,
+ ::ctre::phoenix::CANifier::LEDChannelB);
+ last_green_ = status_light.green;
+ }
+
+ if (status_light.blue != last_blue_ || time_since_last_send == 0) {
+ canifier_.SetLEDOutput(1.0 - status_light.blue,
+ ::ctre::phoenix::CANifier::LEDChannelA);
+ last_blue_ = status_light.blue;
+ }
+
+ if (status_light.red != last_red_ || time_since_last_send == 0) {
+ canifier_.SetLEDOutput(1.0 - status_light.red,
+ ::ctre::phoenix::CANifier::LEDChannelC);
+ last_red_ = status_light.red;
+ }
+ }
+
void Quit() { run_ = false; }
private:
@@ -705,6 +737,12 @@
::ctre::phoenix::CANifier canifier_{0};
::std::atomic<bool> run_{true};
+
+ double last_red_ = -1.0;
+ double last_green_ = -1.0;
+ double last_blue_ = -1.0;
+
+ int light_flash_ = 0;
};
class DrivetrainWriter : public ::frc971::wpilib::LoopOutputHandler {