got the output check stuff actually compiling and closer to working
diff --git a/aos/prime/output/motor_output.cc b/aos/prime/output/motor_output.cc
index af9632b..249ccfd 100644
--- a/aos/prime/output/motor_output.cc
+++ b/aos/prime/output/motor_output.cc
@@ -89,6 +89,10 @@
values_.pwm_outputs[channel - 1] = bounds.Map(value);
}
+void MotorOutput::SetRawPWMOutput(uint8_t channel, uint8_t value) {
+ values_.pwm_outputs[channel - 1] = value;
+}
+
void MotorOutput::DisablePWMOutput(uint8_t channel) {
values_.pwm_outputs[channel - 1] = 0;
}
diff --git a/aos/prime/output/motor_output.h b/aos/prime/output/motor_output.h
index 53bdc83..01e271a 100644
--- a/aos/prime/output/motor_output.h
+++ b/aos/prime/output/motor_output.h
@@ -51,11 +51,13 @@
void SetSolenoid(uint8_t channel, bool set);
void SetPWMOutput(uint8_t channel, double value,
const MotorControllerBounds &bounds);
+ void SetRawPWMOutput(uint8_t cahnnel, uint8_t value);
void DisablePWMOutput(uint8_t channel);
void SetDigitalOutput(uint8_t channel, bool value);
// The data that's going to get sent over.
- // Gets reset (everything set so that it won't do anything) each time through.
+ // Gets reset (everything set so that it won't do anything) each time through
+ // except for solenoids.
NetworkRobotMotors values_;
private:
diff --git a/bbb_cape/src/cape/data_struct.h b/bbb_cape/src/cape/data_struct.h
index d60a0e9..6d747bb 100644
--- a/bbb_cape/src/cape/data_struct.h
+++ b/bbb_cape/src/cape/data_struct.h
@@ -82,9 +82,12 @@
int32_t left_drive;
int32_t right_drive;
- // The length of the pulse from the ultrasonic sensor in 100kHZ ticks.
+ // The length of the pulse from the ultrasonic sensor in 100KHz ticks.
uint32_t ultrasonic_pulse_length;
+ // The length of the pulse from the sidecar PWM output in 10MHz ticks.
+ uint32_t output_check_pulse_length;
+
int32_t shooter_position, pusher_distal_posedge_position,
pusher_proximal_posedge_position;
diff --git a/bbb_cape/src/cape/digital.h b/bbb_cape/src/cape/digital.h
index 2f227a5..6071ca8 100644
--- a/bbb_cape/src/cape/digital.h
+++ b/bbb_cape/src/cape/digital.h
@@ -8,7 +8,7 @@
// For all of the digital functions, a high voltage level on the input reads as
// 1 (and a low to high transition is a positive edge).
// Encoder inputs 0-7 A and B are mapped to "digital inputs" 12-27 (12 is 0A,
-13 is B, 14 is 1A, etc).
+// 13 is B, 14 is 1A, etc).
static inline int digital_read(int num) {
switch (num) {
diff --git a/bbb_cape/src/cape/peripherial_usage.notes b/bbb_cape/src/cape/peripherial_usage.notes
index 757f14a..c8005c3 100644
--- a/bbb_cape/src/cape/peripherial_usage.notes
+++ b/bbb_cape/src/cape/peripherial_usage.notes
@@ -57,4 +57,4 @@
[robots]
robot_comp
TIM11
- TIM11_IRQ:3 (aka TIM1_TRG_COM)
+ TIM11_IRQ:1 (aka TIM1_TRG_COM)
diff --git a/bbb_cape/src/cape/robot_comp.c b/bbb_cape/src/cape/robot_comp.c
index cab3e28..03e529a 100644
--- a/bbb_cape/src/cape/robot_comp.c
+++ b/bbb_cape/src/cape/robot_comp.c
@@ -6,9 +6,7 @@
#include "cape/analog.h"
#include "cape/digital.h"
#include "cape/util.h"
-
-// TIM11.1 on PB9, aka digital input 6.
-static volatile uint32_t ultrasonic_pulse_length = 0;
+#include "cape/timer.h"
typedef struct {
uint32_t posedges, negedges;
@@ -91,27 +89,15 @@
CLAW(10, 11, 9, bottom_claw, 7);
SHOOTER(7, 5, 4, 8, 0)
-void TIM1_TRG_COM_TIM11_IRQHandler(void) {
- TIM11->SR = ~TIM_SR_CC1IF;
- if (digital_read(6)) {
- TIM11->EGR = TIM_EGR_UG;
- } else {
- ultrasonic_pulse_length = TIM11->CCR1;
- }
-}
+// TIM11.1 on PB9, aka digital input 6.
+timer_declare(TIM11, TIM1_TR_GCOM_TIM11_IRQHandler, 1, 1, ultrasonic)
void robot_init(void) {
gpio_setup_alt(GPIOB, 9, 3);
RCC->APB2ENR |= RCC_APB2ENR_TIM11EN;
- TIM11->CR1 = TIM_CR1_URS;
- TIM11->DIER = TIM_DIER_CC1IE;
TIM11->CCMR1 = TIM_CCMR1_CC1S_0 /* input pin 1 -> timer input 1 */;
- TIM11->CCER = TIM_CCER_CC1P | TIM_CCER_CC1NP | TIM_CCER_CC1E;
- TIM11->EGR = TIM_EGR_UG;
- TIM11->PSC = 1200 - 1; // 100kHZ timer
- TIM11->CR1 |= TIM_CR1_CEN;
- NVIC_SetPriority(TIM1_TRG_COM_TIM11_IRQn, 3);
- NVIC_EnableIRQ(TIM1_TRG_COM_TIM11_IRQn);
+ TIM11->PSC = 1200 - 1; // 100KHz timer
+ timer_setup(TIM11, TIM1_TRG_COM_TIM11_IRQn, 1);
}
void robot_fill_packet(struct DataStruct *packet) {
@@ -125,7 +111,7 @@
packet->main.battery_voltage_high = analog_get(5);
packet->main.battery_voltage_low = analog_get(3);
- packet->main.ultrasonic_pulse_length = ultrasonic_pulse_length;
+ packet->main.ultrasonic_pulse_length = ultrasonic_length;
fill_top_claw_values(packet);
fill_bottom_claw_values(packet);
diff --git a/bbb_cape/src/cape/timer.h b/bbb_cape/src/cape/timer.h
new file mode 100644
index 0000000..a356de1
--- /dev/null
+++ b/bbb_cape/src/cape/timer.h
@@ -0,0 +1,34 @@
+#ifndef CAPE_TIMER_H_
+#define CAPE_TIMER_H_
+
+#include <STM32F2XX.h>
+
+// inverted is 1 for timing low periods, 0 for high ones.
+#define timer_declare(timer, irq, input, inverted, name) \
+ static volatile uint32_t name##_length; \
+ void irq(void) { \
+ timer->SR = ~TIM_SR_CC##input##IF; \
+ const uint32_t ccer = timer->CCER; \
+ timer->CCER = ccer ^ (TIM_CCER_CC##input##P | TIM_CCER_CC##input##NP); \
+ const uint32_t rising = ccer & TIM_CCER_CC##input##P; \
+ if (inverted ? rising : !rising) { \
+ name##_length = timer->CCR##input; \
+ } else { \
+ timer->EGR = TIM_EGR_UG; \
+ } \
+ }
+
+// You need to enable the clock, set up the alt function for the input pin,
+// set the prescaler, and set up the timer input before calling this.
+#define timer_setup(timer, irq, input) \
+ do { \
+ timer->CR1 = TIM_CR1_URS; \
+ timer->DIER = TIM_DIER_CC##input##IE; \
+ timer->CCER = TIM_CCER_CC##input##P | TIM_CCER_CC##input##E; \
+ timer->EGR = TIM_EGR_UG; \
+ timer->CR1 |= TIM_CR1_CEN; \
+ NVIC_SetPriority(irq, 1); \
+ NVIC_EnableIRQ(irq); \
+ } while (0)
+
+#endif // CAPE_TIMER_H_
diff --git a/frc971/input/sensor_receiver.cc b/frc971/input/sensor_receiver.cc
index 3def0d7..199df99 100644
--- a/frc971/input/sensor_receiver.cc
+++ b/frc971/input/sensor_receiver.cc
@@ -71,7 +71,8 @@
}
double sonar_translate(uint32_t in) {
- return static_cast<double>(in) / 1000.0 * 2.0;
+ return static_cast<double>(in) * 10.0 /*us/tick*/ / 147.0 /*in/us*/ *
+ 0.0254 /*m/in*/;
}
double hall_translate(const constants::ShifterHallEffect &k, uint16_t in_low,
diff --git a/frc971/output/motor_writer.cc b/frc971/output/motor_writer.cc
index d42cc05..eefd7d6 100644
--- a/frc971/output/motor_writer.cc
+++ b/frc971/output/motor_writer.cc
@@ -12,11 +12,10 @@
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/claw/claw.q.h"
#include "frc971/control_loops/shooter/shooter.q.h"
+#include "frc971/queues/output_check.q.h"
using ::aos::util::SimpleLogInterval;
-using ::frc971::control_loops::drivetrain;
-
namespace frc971 {
namespace output {
@@ -39,13 +38,14 @@
values_.solenoid_module = 0;
if (true) {
- drivetrain.output.FetchLatest();
- if (drivetrain.output.IsNewerThanMS(kOutputMaxAgeMS)) {
- LOG_STRUCT(DEBUG, "will output", *drivetrain.output.get());
- SetPWMOutput(3, drivetrain.output->right_voltage / 12.0, kTalonBounds);
- SetPWMOutput(6, -drivetrain.output->left_voltage / 12.0, kTalonBounds);
- SetSolenoid(7, drivetrain.output->left_high);
- SetSolenoid(8, drivetrain.output->right_high);
+ static auto &drivetrain = ::frc971::control_loops::drivetrain.output;
+ drivetrain.FetchLatest();
+ if (drivetrain.IsNewerThanMS(kOutputMaxAgeMS)) {
+ LOG_STRUCT(DEBUG, "will output", *drivetrain);
+ SetPWMOutput(3, drivetrain->right_voltage / 12.0, kTalonBounds);
+ SetPWMOutput(6, -drivetrain->left_voltage / 12.0, kTalonBounds);
+ SetSolenoid(7, drivetrain->left_high);
+ SetSolenoid(8, drivetrain->right_high);
} else {
DisablePWMOutput(3);
DisablePWMOutput(8);
@@ -59,7 +59,7 @@
::frc971::control_loops::shooter_queue_group.output;
shooter.FetchLatest();
if (shooter.IsNewerThanMS(kOutputMaxAgeMS)) {
- LOG_STRUCT(DEBUG, "will output", *shooter.get());
+ LOG_STRUCT(DEBUG, "will output", *shooter);
SetPWMOutput(7, shooter->voltage / 12.0, kTalonBounds);
SetSolenoid(6, !shooter->latch_piston);
SetSolenoid(5, !shooter->brake_piston);
@@ -75,7 +75,7 @@
static auto &claw = ::frc971::control_loops::claw_queue_group.output;
claw.FetchLatest();
if (claw.IsNewerThanMS(kOutputMaxAgeMS)) {
- LOG_STRUCT(DEBUG, "will output", *claw.get());
+ LOG_STRUCT(DEBUG, "will output", *claw);
SetPWMOutput(9, claw->intake_voltage / 12.0, kTalonBounds);
SetPWMOutput(8, claw->intake_voltage / 12.0, kTalonBounds);
SetPWMOutput(1, -claw->bottom_claw_voltage / 12.0, kTalonBounds);
@@ -94,11 +94,15 @@
claw_old_.Print();
}
- ++output_check_;
- if (output_check_ == 0) output_check_ = 1;
- ::frc971::output_check_queue.MakeWithBuilder()
- .pwm_value(output_check_).Send();
- SetPWMOutput(10, output_check_);
+ {
+ auto message = ::frc971::output_check_queue.MakeMessage();
+ ++output_check_;
+ if (output_check_ == 0) output_check_ = 1;
+ SetRawPWMOutput(10, output_check_);
+ message->sent_value = output_check_;
+ LOG_STRUCT(DEBUG, "sending", *message);
+ message.Send();
+ }
}
SimpleLogInterval drivetrain_old_ =
diff --git a/frc971/output/output.gyp b/frc971/output/output.gyp
index bc2d604..de41943 100644
--- a/frc971/output/output.gyp
+++ b/frc971/output/output.gyp
@@ -53,6 +53,7 @@
'<(AOS)/common/logging/logging.gyp:queue_logging',
'<(DEPTH)/frc971/control_loops/claw/claw.gyp:claw_loop',
'<(DEPTH)/frc971/control_loops/shooter/shooter.gyp:shooter_loop',
+ '<(DEPTH)/frc971/queues/queues.gyp:output_check',
],
},
],
diff --git a/frc971/queues/output_check.q b/frc971/queues/output_check.q
new file mode 100644
index 0000000..0573426
--- /dev/null
+++ b/frc971/queues/output_check.q
@@ -0,0 +1,8 @@
+package frc971;
+
+message OutputCheck {
+ uint8_t sent_value;
+};
+// Each message here represents a value that was sent to the cRIO.
+// The sent timestamp of the message is when the value was sent.
+queue OutputCheck output_check_queue;
diff --git a/frc971/queues/queues.gyp b/frc971/queues/queues.gyp
index 8434437..d71be25 100644
--- a/frc971/queues/queues.gyp
+++ b/frc971/queues/queues.gyp
@@ -22,19 +22,14 @@
'includes': ['../../aos/build/queues.gypi'],
},
{
- 'target_name': 'frc971_queues_so',
- 'type': 'loadable_module',
- 'sources': ['<@(queue_files)'],
+ 'target_name': 'output_check',
+ 'type': 'static_library',
+ 'sources': [
+ 'output_check.q',
+ ],
'variables': {
'header_path': 'frc971/queues',
},
- 'dependencies': [
- ],
- 'direct_dependent_settings': {
- 'variables': {
- 'jni_libs': ['frc971_queues_so'],
- },
- },
'includes': ['../../aos/build/queues.gypi'],
},
],