got the sensors working on the real robot
diff --git a/bbb_cape/src/cape/robot_comp.c b/bbb_cape/src/cape/robot_comp.c
index 2f94ade..ddd3f2d 100644
--- a/bbb_cape/src/cape/robot_comp.c
+++ b/bbb_cape/src/cape/robot_comp.c
@@ -15,8 +15,8 @@
} EdgeCounts;
#define COPY_EDGE_COUNTS(edge_counts, hall_effect_edges) \
- hall_effect_edges.posedges = edge_counts.posedges; \
- hall_effect_edges.negedges = edge_counts.negedges;
+ hall_effect_edges.posedges = edge_counts.negedges; \
+ hall_effect_edges.negedges = edge_counts.posedges;
#define HALL_CAPTURE(num, edges, encoder, capture) \
void digital_capture_##num##P(void) { \
@@ -35,28 +35,28 @@
int32_t posedge, negedge;
} pusher_distal_captures, pusher_proximal_captures;
-#define SHOOTER(plunger_num, pusher_distal_num, pusher_proximal_num, \
- latch_num, encoder) \
- HALL_CAPTURE_DECL(pusher_distal_num, pusher_distal, encoder, \
- pusher_distal_captures); \
- HALL_CAPTURE_DECL(pusher_proximal_num, pusher_proximal, encoder, \
- pusher_proximal_captures); \
- static inline void fill_shooter_values(struct DataStruct *packet) { \
- digital_capture_disable(pusher_distal_num); \
- digital_capture_disable(pusher_proximal_num); \
- packet->main.shooter_position = encoder_read(encoder); \
- packet->main.pusher_distal_posedge_position = \
- pusher_distal_captures.posedge; \
- packet->main.pusher_proximal_posedge_position = \
- pusher_proximal_captures.negedge; \
- packet->main.bools.pusher_distal = digital_read(pusher_distal_num); \
- packet->main.bools.pusher_proximal = digital_read(pusher_proximal_num); \
- COPY_EDGE_COUNTS(pusher_distal, packet->main.pusher_distal); \
- COPY_EDGE_COUNTS(pusher_proximal, packet->main.pusher_proximal); \
- digital_capture_enable(pusher_distal_num); \
- digital_capture_enable(pusher_proximal_num); \
- packet->main.bools.plunger = digital_read(plunger_num); \
- packet->main.bools.latch = digital_read(latch_num); \
+#define SHOOTER(plunger_num, pusher_distal_num, pusher_proximal_num, \
+ latch_num, encoder) \
+ HALL_CAPTURE_DECL(pusher_distal_num, pusher_distal, encoder, \
+ pusher_distal_captures); \
+ HALL_CAPTURE_DECL(pusher_proximal_num, pusher_proximal, encoder, \
+ pusher_proximal_captures); \
+ static inline void fill_shooter_values(struct DataStruct *packet) { \
+ digital_capture_disable(pusher_distal_num); \
+ digital_capture_disable(pusher_proximal_num); \
+ packet->main.shooter_position = encoder_read(encoder); \
+ packet->main.pusher_distal_posedge_position = \
+ pusher_distal_captures.negedge; \
+ packet->main.pusher_proximal_posedge_position = \
+ pusher_proximal_captures.negedge; \
+ packet->main.bools.pusher_distal = !digital_read(pusher_distal_num); \
+ packet->main.bools.pusher_proximal = !digital_read(pusher_proximal_num); \
+ COPY_EDGE_COUNTS(pusher_distal, packet->main.pusher_distal); \
+ COPY_EDGE_COUNTS(pusher_proximal, packet->main.pusher_proximal); \
+ digital_capture_enable(pusher_distal_num); \
+ digital_capture_enable(pusher_proximal_num); \
+ packet->main.bools.plunger = !digital_read(plunger_num); \
+ packet->main.bools.latch = !digital_read(latch_num); \
}
typedef struct {
@@ -74,11 +74,11 @@
digital_capture_disable(calibration_num); \
digital_capture_disable(back_num); \
packet->main.name.position = encoder_read(encoder); \
- packet->main.name.posedge_position = name.posedge; \
- packet->main.name.negedge_position = name.negedge; \
- packet->main.name.bools.front = digital_read(front_num); \
- packet->main.name.bools.calibration = digital_read(calibration_num); \
- packet->main.name.bools.back = digital_read(back_num); \
+ packet->main.name.negedge_position = name.posedge; \
+ packet->main.name.posedge_position = name.negedge; \
+ packet->main.name.bools.front = !digital_read(front_num); \
+ packet->main.name.bools.calibration = !digital_read(calibration_num); \
+ packet->main.name.bools.back = !digital_read(back_num); \
COPY_EDGE_COUNTS(name.front, packet->main.name.front); \
COPY_EDGE_COUNTS(name.calibration, packet->main.name.calibration); \
COPY_EDGE_COUNTS(name.back, packet->main.name.back); \
@@ -87,24 +87,27 @@
digital_capture_enable(back_num); \
}
-CLAW(0, 2, 1, top_claw, 2);
-CLAW(9, 11, 10, bottom_claw, 7);
+CLAW(1, 2, 0, top_claw, 2);
+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;
+ TIM11->CR1 = 0;
ultrasonic_pulse_length = TIM11->CCR1;
+ ultrasonic_pulse_length = 1;
+ TIM11->CNT = 0;
}
void robot_init(void) {
gpio_setup_alt(GPIOB, 9, 3);
RCC->APB2ENR |= RCC_APB2ENR_TIM11EN;
- TIM11->CR1 = TIM_CR1_URS;
+ TIM11->CR1 = 0;
TIM11->SMCR = 5 << 4 /* TI1 */ |
- 4 << 0 /* reset and start on edge */;
+ 6 << 0 /* start on rising edge */;
TIM11->DIER = TIM_DIER_CC1IE;
TIM11->CCMR1 = TIM_CCMR1_CC1S_0 /* input pin 1 -> timer input 1 */;
- TIM11->CCER = TIM_CCER_CC1P /* go on falling edge */;
+ TIM11->CCER = TIM_CCER_CC1P /* go on falling edge */ | TIM_CCER_CC1E;
TIM11->EGR = TIM_EGR_UG;
TIM11->PSC = 1200 - 1; // 100kHZ timer
TIM11->CR1 |= TIM_CR1_CEN;
@@ -115,11 +118,12 @@
void robot_fill_packet(struct DataStruct *packet) {
packet->main.left_drive = encoder_read(6);
packet->main.right_drive = encoder_read(5);
- packet->main.left_drive_hall = analog_get(0);
- packet->main.right_drive_hall = analog_get(1);
+ packet->main.left_drive_hall = analog_get(7);
+ packet->main.right_drive_hall = analog_get(0);
packet->main.ultrasonic_pulse_length = ultrasonic_pulse_length;
fill_top_claw_values(packet);
fill_bottom_claw_values(packet);
+ fill_shooter_values(packet);
}
diff --git a/frc971/control_loops/shooter/shooter.cc b/frc971/control_loops/shooter/shooter.cc
index 08138ff..b898c58 100755
--- a/frc971/control_loops/shooter/shooter.cc
+++ b/frc971/control_loops/shooter/shooter.cc
@@ -73,12 +73,14 @@
return;
}
- if (initial_loop_) {
- // TODO(austin): If 'reset()', we are lost, start over.
- initial_loop_ = false;
- shooter_.SetPositionDirectly(position->position);
- } else {
- shooter_.SetPositionValues(position->position);
+ if (position) {
+ if (initial_loop_) {
+ // TODO(austin): If 'reset()', we are lost, start over.
+ initial_loop_ = false;
+ shooter_.SetPositionDirectly(position->position);
+ } else {
+ shooter_.SetPositionValues(position->position);
+ }
}
// Disable the motors now so that all early returns will return with the
diff --git a/frc971/control_loops/shooter/shooter.h b/frc971/control_loops/shooter/shooter.h
index 89109dd..8c8ab81 100755
--- a/frc971/control_loops/shooter/shooter.h
+++ b/frc971/control_loops/shooter/shooter.h
@@ -95,12 +95,12 @@
Correct(Y);
}
- void SetGoalPosition(double desired_position, double desired_velocity) {
+ void SetGoalPosition(double, double) {
// austin said something about which matrix to set, but I didn't under
// very much of it
//some_matrix = {desired_position, desired_velocity};
- LOG(DEBUG, "ZSFL> dp: %.2f dz: %.2f\n", desired_position, desired_velocity);
- R << desired_position, desired_velocity, 0;
+ //LOG(DEBUG, "ZSFL> dp: %.2f dz: %.2f\n", desired_position, desired_velocity);
+ //R << desired_position, desired_velocity, 0;
}
double position() const { return X_hat(0, 0); }
diff --git a/frc971/input/sensor_receiver.cc b/frc971/input/sensor_receiver.cc
index 369dab5..84ccc70 100644
--- a/frc971/input/sensor_receiver.cc
+++ b/frc971/input/sensor_receiver.cc
@@ -72,15 +72,15 @@
return (voltage - k.low) / (k.high - k.low);
}
-double shooter_translate(int32_t in) {
+double claw_translate(int32_t in) {
return static_cast<double>(in)
/ (256.0 /*cpr*/ * 4.0 /*quad*/)
- * (18.0 / 48.0 /*encoder gears*/)
- * (12.0 / 60.0 /*chain reduction*/)
+ / (18.0 / 48.0 /*encoder gears*/)
+ / (12.0 / 60.0 /*chain reduction*/)
* (M_PI / 180.0);
}
-double claw_translate(int32_t in) {
+double shooter_translate(int32_t in) {
return static_cast<double>(in)
/ (256.0 /*cpr*/ * 4.0 /*quad*/)
* 16 /*sprocket teeth*/ * 0.375 /*chain pitch*/
@@ -97,22 +97,29 @@
void CopyClawPosition(control_loops::HalfClawPosition *output,
const ::bbb::SingleClawPosition &input,
- State::SingleClawState *state) {
+ State::SingleClawState *state,
+ bool reversed) {
CopyHallEffectEdges(&output->front, input.front, &state->front);
+ output->front.current = input.bools.front;
CopyHallEffectEdges(&output->calibration, input.calibration,
&state->calibration);
+ output->calibration.current = input.bools.calibration;
CopyHallEffectEdges(&output->back, input.back, &state->back);
+ output->back.current = input.bools.back;
- output->position = claw_translate(input.position);
- output->posedge_value = claw_translate(input.posedge_position);
- output->negedge_value = claw_translate(input.negedge_position);
+ const double multiplier = reversed ? -1.0 : 1.0;
+
+ output->position = multiplier * claw_translate(input.position);
+ output->posedge_value = multiplier * claw_translate(input.posedge_position);
+ output->negedge_value = multiplier * claw_translate(input.negedge_position);
}
void PacketReceived(const ::bbb::DataStruct *data,
const ::aos::time::Time &cape_timestamp,
State *state) {
- ::frc971::logging_structs::CapeReading reading_to_log(cape_timestamp.sec(),
- cape_timestamp.nsec());
+ ::frc971::logging_structs::CapeReading reading_to_log(
+ cape_timestamp.sec(), cape_timestamp.nsec(),
+ data->main.ultrasonic_pulse_length);
LOG_STRUCT(DEBUG, "cape reading", reading_to_log);
bool bad_gyro;
if (data->uninitialized_gyro) {
@@ -151,9 +158,9 @@
{
auto claw_position = control_loops::claw_queue_group.position.MakeMessage();
CopyClawPosition(&claw_position->top, data->main.top_claw,
- &state->top_claw);
+ &state->top_claw, false);
CopyClawPosition(&claw_position->bottom, data->main.bottom_claw,
- &state->bottom_claw);
+ &state->bottom_claw, true);
claw_position.Send();
}
diff --git a/frc971/queues/to_log.q b/frc971/queues/to_log.q
index c3cc076..24e2402 100644
--- a/frc971/queues/to_log.q
+++ b/frc971/queues/to_log.q
@@ -3,4 +3,5 @@
struct CapeReading {
uint32_t sec;
uint32_t nsec;
+ uint32_t sonar;
};