split analog.c out intelligently and added an id to the sensor packet
diff --git a/frc971/input/gyro_board_data.h b/frc971/input/gyro_board_data.h
index 10135e3..6f07a2a 100644
--- a/frc971/input/gyro_board_data.h
+++ b/frc971/input/gyro_board_data.h
@@ -5,70 +5,9 @@
namespace frc971 {
-// The struct that the gyro board sends out with all of the data in it.
-struct GyroBoardData {
- int64_t gyro_angle;
-
- int32_t left_drive;
- int32_t right_drive;
- int32_t shooter_angle;
- int32_t shooter;
- int32_t indexer;
- int32_t wrist;
-
- int32_t capture_top_rise;
- int32_t capture_top_fall;
- int32_t capture_bottom_fall_delay;
- int32_t capture_wrist_rise;
- int32_t capture_shooter_angle_rise;
-
- uint8_t top_rise_count;
-
- uint8_t top_fall_count;
-
- uint8_t bottom_rise_count;
-
- uint8_t bottom_fall_delay_count;
- uint8_t bottom_fall_count;
-
- uint8_t wrist_rise_count;
-
- uint8_t shooter_angle_rise_count;
-
- union {
- struct {
- uint8_t wrist_hall_effect : 1;
- uint8_t angle_adjust_bottom_hall_effect : 1;
- uint8_t top_disc : 1;
- uint8_t bottom_disc : 1;
- };
- uint32_t digitals;
- };
-
- void NetworkToHost() {
- // Apparently it sends the information out in little endian.
-#if 0
- using ::aos::ntoh;
-
- gyro_angle = ntoh(gyro_angle);
-
- right_drive = ntoh(right_drive);
- left_drive = ntoh(left_drive);
- shooter_angle = ntoh(shooter_angle);
- shooter = ntoh(shooter);
- indexer = ntoh(indexer);
- wrist = ntoh(wrist);
-
- capture_top_rise = ntoh(capture_top_rise);
- capture_top_fall = ntoh(capture_top_fall);
- capture_bottom_fall_delay = ntoh(capture_bottom_fall_delay);
- capture_wrist_rise = ntoh(capture_wrist_rise);
- capture_shooter_angle_rise = ntoh(capture_shooter_angle_rise);
-
- digitals = ntoh(digitals);
-#endif
- }
-} __attribute__((__packed__));
+#define DATA_STRUCT_NAME GyroBoardData
+#include "../../gyro_board/src/usb/data_struct.h"
+#undef DATA_STRUCT_NAME
} // namespace frc971
diff --git a/frc971/input/gyro_board_reader.cc b/frc971/input/gyro_board_reader.cc
index 06e760c..e1e08d7 100644
--- a/frc971/input/gyro_board_reader.cc
+++ b/frc971/input/gyro_board_reader.cc
@@ -159,8 +159,14 @@
}
void ProcessData(GyroBoardData *data) {
- data->NetworkToHost();
- LOG(DEBUG, "processing a packet\n");
+ if (data->robot_id != 0) {
+ LOG(ERROR, "gyro board sent data for robot id %hhd!"
+ " dip switches are %x\n", data->robot_id, data->dip_switches);
+ return;
+ } else {
+ LOG(DEBUG, "processing a packet dip switches %x\n", data->dip_switches);
+ }
+
static ::aos::time::Time last_time = ::aos::time::Time::Now();
if ((last_time - ::aos::time::Time::Now()) >
::aos::time::Time::InMS(0.00205)) {
@@ -171,58 +177,58 @@
.angle(data->gyro_angle / 16.0 / 1000.0 / 180.0 * M_PI)
.Send();
- UpdateWrappingCounter(data->top_rise_count,
+ UpdateWrappingCounter(data->main.top_rise_count,
&last_top_rise_count_, &top_rise_count_);
- UpdateWrappingCounter(data->top_fall_count,
+ UpdateWrappingCounter(data->main.top_fall_count,
&last_top_fall_count_, &top_fall_count_);
- UpdateWrappingCounter(data->bottom_rise_count,
+ UpdateWrappingCounter(data->main.bottom_rise_count,
&last_bottom_rise_count_, &bottom_rise_count_);
- UpdateWrappingCounter(data->bottom_fall_delay_count,
+ UpdateWrappingCounter(data->main.bottom_fall_delay_count,
&last_bottom_fall_delay_count_, &bottom_fall_delay_count_);
- UpdateWrappingCounter(data->bottom_fall_count,
+ UpdateWrappingCounter(data->main.bottom_fall_count,
&last_bottom_fall_count_, &bottom_fall_count_);
- UpdateWrappingCounter(data->wrist_rise_count,
+ UpdateWrappingCounter(data->main.wrist_rise_count,
&last_wrist_rise_count_, &wrist_rise_count_);
- UpdateWrappingCounter(data->shooter_angle_rise_count,
+ UpdateWrappingCounter(data->main.shooter_angle_rise_count,
&last_shooter_angle_rise_count_, &shooter_angle_rise_count_);
drivetrain.position.MakeWithBuilder()
- .right_encoder(drivetrain_translate(data->right_drive))
- .left_encoder(-drivetrain_translate(data->left_drive))
+ .right_encoder(drivetrain_translate(data->main.right_drive))
+ .left_encoder(-drivetrain_translate(data->main.left_drive))
.Send();
wrist.position.MakeWithBuilder()
- .pos(wrist_translate(data->wrist))
- .hall_effect(data->wrist_hall_effect)
- .calibration(wrist_translate(data->capture_wrist_rise))
+ .pos(wrist_translate(data->main.wrist))
+ .hall_effect(data->main.wrist_hall_effect)
+ .calibration(wrist_translate(data->main.capture_wrist_rise))
.Send();
angle_adjust.position.MakeWithBuilder()
- .angle(angle_adjust_translate(data->shooter_angle))
- .bottom_hall_effect(data->angle_adjust_bottom_hall_effect)
+ .angle(angle_adjust_translate(data->main.shooter_angle))
+ .bottom_hall_effect(data->main.angle_adjust_bottom_hall_effect)
.middle_hall_effect(false)
.bottom_calibration(angle_adjust_translate(
- data->capture_shooter_angle_rise))
+ data->main.capture_shooter_angle_rise))
.middle_calibration(angle_adjust_translate(
0))
.Send();
shooter.position.MakeWithBuilder()
- .position(shooter_translate(data->shooter))
+ .position(shooter_translate(data->main.shooter))
.Send();
index_loop.position.MakeWithBuilder()
- .index_position(index_translate(data->indexer))
- .top_disc_detect(data->top_disc)
+ .index_position(index_translate(data->main.indexer))
+ .top_disc_detect(data->main.top_disc)
.top_disc_posedge_count(top_rise_count_)
- .top_disc_posedge_position(index_translate(data->capture_top_rise))
+ .top_disc_posedge_position(index_translate(data->main.capture_top_rise))
.top_disc_negedge_count(top_fall_count_)
- .top_disc_negedge_position(index_translate(data->capture_top_fall))
- .bottom_disc_detect(data->bottom_disc)
+ .top_disc_negedge_position(index_translate(data->main.capture_top_fall))
+ .bottom_disc_detect(data->main.bottom_disc)
.bottom_disc_posedge_count(bottom_rise_count_)
.bottom_disc_negedge_count(bottom_fall_count_)
.bottom_disc_negedge_wait_position(index_translate(
- data->capture_bottom_fall_delay))
+ data->main.capture_bottom_fall_delay))
.bottom_disc_negedge_wait_count(bottom_fall_delay_count_)
.Send();
}
diff --git a/frc971/input/gyro_sensor_receiver.cc b/frc971/input/gyro_sensor_receiver.cc
index 4dec23e..1e5343e 100644
--- a/frc971/input/gyro_sensor_receiver.cc
+++ b/frc971/input/gyro_sensor_receiver.cc
@@ -82,8 +82,13 @@
}
void UnpackFrom(GyroBoardData *data) {
- data->NetworkToHost();
- LOG(DEBUG, "processing a packet\n");
+ if (data->robot_id != 0) {
+ LOG(ERROR, "gyro board sent data for robot id %hhd!"
+ " dip switches are %x\n", data->robot_id, data->dip_switches);
+ return;
+ } else {
+ LOG(DEBUG, "processing a packet dip switches %x\n", data->dip_switches);
+ }
static ::aos::time::Time last_time = ::aos::time::Time::Now();
if ((last_time - ::aos::time::Time::Now()) >
@@ -95,58 +100,58 @@
.angle(data->gyro_angle / 16.0 / 1000.0 / 180.0 * M_PI)
.Send();
- UpdateWrappingCounter(data->top_rise_count,
+ UpdateWrappingCounter(data->main.top_rise_count,
&last_top_rise_count_, &top_rise_count_);
- UpdateWrappingCounter(data->top_fall_count,
+ UpdateWrappingCounter(data->main.top_fall_count,
&last_top_fall_count_, &top_fall_count_);
- UpdateWrappingCounter(data->bottom_rise_count,
+ UpdateWrappingCounter(data->main.bottom_rise_count,
&last_bottom_rise_count_, &bottom_rise_count_);
- UpdateWrappingCounter(data->bottom_fall_delay_count,
+ UpdateWrappingCounter(data->main.bottom_fall_delay_count,
&last_bottom_fall_delay_count_, &bottom_fall_delay_count_);
- UpdateWrappingCounter(data->bottom_fall_count,
+ UpdateWrappingCounter(data->main.bottom_fall_count,
&last_bottom_fall_count_, &bottom_fall_count_);
- UpdateWrappingCounter(data->wrist_rise_count,
+ UpdateWrappingCounter(data->main.wrist_rise_count,
&last_wrist_rise_count_, &wrist_rise_count_);
- UpdateWrappingCounter(data->shooter_angle_rise_count,
+ UpdateWrappingCounter(data->main.shooter_angle_rise_count,
&last_shooter_angle_rise_count_, &shooter_angle_rise_count_);
drivetrain.position.MakeWithBuilder()
- .right_encoder(drivetrain_translate(data->right_drive))
- .left_encoder(-drivetrain_translate(data->left_drive))
+ .right_encoder(drivetrain_translate(data->main.right_drive))
+ .left_encoder(-drivetrain_translate(data->main.left_drive))
.Send();
wrist.position.MakeWithBuilder()
- .pos(wrist_translate(data->wrist))
- .hall_effect(!data->wrist_hall_effect)
- .calibration(wrist_translate(data->capture_wrist_rise))
+ .pos(wrist_translate(data->main.wrist))
+ .hall_effect(!data->main.wrist_hall_effect)
+ .calibration(wrist_translate(data->main.capture_wrist_rise))
.Send();
angle_adjust.position.MakeWithBuilder()
- .angle(angle_adjust_translate(data->shooter_angle))
- .bottom_hall_effect(!data->angle_adjust_bottom_hall_effect)
+ .angle(angle_adjust_translate(data->main.shooter_angle))
+ .bottom_hall_effect(!data->main.angle_adjust_bottom_hall_effect)
.middle_hall_effect(false)
.bottom_calibration(angle_adjust_translate(
- data->capture_shooter_angle_rise))
+ data->main.capture_shooter_angle_rise))
.middle_calibration(angle_adjust_translate(
0))
.Send();
shooter.position.MakeWithBuilder()
- .position(shooter_translate(data->shooter))
+ .position(shooter_translate(data->main.shooter))
.Send();
index_loop.position.MakeWithBuilder()
- .index_position(index_translate(data->indexer))
- .top_disc_detect(!data->top_disc)
+ .index_position(index_translate(data->main.indexer))
+ .top_disc_detect(!data->main.top_disc)
.top_disc_posedge_count(top_rise_count_)
- .top_disc_posedge_position(index_translate(data->capture_top_rise))
+ .top_disc_posedge_position(index_translate(data->main.capture_top_rise))
.top_disc_negedge_count(top_fall_count_)
- .top_disc_negedge_position(index_translate(data->capture_top_fall))
- .bottom_disc_detect(!data->bottom_disc)
+ .top_disc_negedge_position(index_translate(data->main.capture_top_fall))
+ .bottom_disc_detect(!data->main.bottom_disc)
.bottom_disc_posedge_count(bottom_rise_count_)
.bottom_disc_negedge_count(bottom_fall_count_)
.bottom_disc_negedge_wait_position(index_translate(
- data->capture_bottom_fall_delay))
+ data->main.capture_bottom_fall_delay))
.bottom_disc_negedge_wait_count(bottom_fall_delay_count_)
.Send();
}
diff --git a/gyro_board/src/usb/LPCUSB/USB_SENSOR_STREAM.c b/gyro_board/src/usb/LPCUSB/USB_SENSOR_STREAM.c
index 4609c96..bf360f6 100644
--- a/gyro_board/src/usb/LPCUSB/USB_SENSOR_STREAM.c
+++ b/gyro_board/src/usb/LPCUSB/USB_SENSOR_STREAM.c
@@ -38,7 +38,7 @@
#include "LPC17xx.h"
-#include "analog.h"
+#include "fill_packet.h"
#define usbMAX_SEND_BLOCK ( 20 / portTICK_RATE_MS )
#define usbRXBUFFER_LEN ( 80 )
diff --git a/gyro_board/src/usb/Makefile b/gyro_board/src/usb/Makefile
index 8e3cd3c..c6050d6 100644
--- a/gyro_board/src/usb/Makefile
+++ b/gyro_board/src/usb/Makefile
@@ -35,6 +35,8 @@
FreeRTOS/portable/MemMang/heap_2.c \
alloc.c \
analog.c \
+ digital.c \
+ encoder.c \
FreeRTOS/portable/GCC/ARM_CM3/port.c \
FreeRTOS/tasks.c \
FreeRTOS/list.c \
diff --git a/gyro_board/src/usb/analog.c b/gyro_board/src/usb/analog.c
index 6e26e84..87d67ad 100644
--- a/gyro_board/src/usb/analog.c
+++ b/gyro_board/src/usb/analog.c
@@ -2,20 +2,12 @@
// CopyLeft qwerk Robotics unINC. 2010 All Rights Reserved.
// ****************************************************************************
-// ****************************************************************************
-// **************** IO Pin Setup
-// ****************************************************************************
+#include "analog.h"
#include "FreeRTOS.h"
#include "queue.h"
#include "task.h"
-#include "analog.h"
-
-// How long (in ms) to wait after a falling edge on the bottom indexer sensor
-// before reading the indexer encoder.
-static const int kBottomFallDelayTime = 32;
-
void analog_init(void) {
// b[1:0] CAN RD1 p0.0
// b[3:2] CAN TD1 p0.1
@@ -47,15 +39,7 @@
ADC->ADCR = 0x00200500;
}
-// ****************************************************************************
-// **************** ADC Functions
-// ****************************************************************************
-
-// **************** macros
-// starts conversion [26:24] = 001
-
-// **************** functions
int analog(int channel) {
ADC->ADCR = ((ADC->ADCR & 0xF8FFFF00) | (0x01000000 | (1 << channel)));
@@ -64,557 +48,3 @@
return ((ADC->ADGDR & 0x0000FFF0) >> 4);
}
-// GPIO1 P0.4
-// GPIO2 P0.5
-// GPIO3 P0.6
-// GPIO4 P0.7
-// GPIO5 P0.8
-// GPIO6 P0.9
-// GPIO7 P2.0
-// GPIO8 P2.1
-// GPIO9 P2.2
-// GPIO10 P2.3
-// GPIO11 P2.4
-// GPIO12 P2.5
-
-// DIP0 P1.29
-// DIP1 P2.13
-// DIP2 P0.11
-// DIP3 P0.10
-#define readGPIO(gpio, chan) ((((gpio)->FIOPIN) >> (chan)) & 1)
-inline int readGPIO_inline(int major, int minor) {
- switch (major) {
- case 0:
- return readGPIO(GPIO0, minor);
- case 1:
- return readGPIO(GPIO1, minor);
- case 2:
- return readGPIO(GPIO2, minor);
- default:
- return -1;
- }
-}
-int digital(int channel) {
- if (channel < 1) {
- return -1;
- } else if (channel < 7) {
- int chan = channel + 3;
- return readGPIO(GPIO0, chan);
- } else if (channel < 13) {
- int chan = channel - 7;
- return readGPIO(GPIO2, chan);
- }
- return -1;
-}
-int dip(int channel) {
- switch (channel) {
- case 0:
- return readGPIO(GPIO1, 29);
- case 1:
- return readGPIO(GPIO2, 13);
- case 2:
- return readGPIO(GPIO0, 11);
- case 3:
- return readGPIO(GPIO0, 10);
- default:
- return -1;
- }
-}
-// ENC0A 1.20
-// ENC0B 1.23
-// ENC1A 2.11
-// ENC1B 2.12
-// ENC2A 0.21
-// ENC2B 0.22
-// ENC3A 0.19
-// ENC3B 0.20
-
-#define ENC(gpio, a, b) readGPIO(gpio, a) * 2 + readGPIO(gpio, b)
-int encoder_bits(int channel) {
- switch (channel) {
- case 0:
- return ENC(GPIO1, 20, 23);
- case 1:
- return ENC(GPIO2, 11, 12);
- case 2:
- return ENC(GPIO0, 21, 22);
- case 3:
- return ENC(GPIO0, 19, 20);
- default:
- return -1;
- }
- return -1;
-}
-#undef ENC
-
-// Uses EINT1 and EINT2 on 2.11 and 2.12.
-volatile int32_t encoder1_val;
-// On GPIO pins 0.22 and 0.21.
-volatile int32_t encoder2_val;
-// On GPIO pins 0.20 and 0.19.
-volatile int32_t encoder3_val;
-// On GPIO pins 2.0 and 2.1.
-volatile int32_t encoder4_val;
-// On GPIO pins 2.2 and 2.3.
-volatile int32_t encoder5_val;
-
-// ENC1A 2.11
-void EINT1_IRQHandler(void) {
- // TODO(brians): figure out why this has to be up here too
- SC->EXTINT = 0x2;
- int fiopin = GPIO2->FIOPIN;
- if (((fiopin >> 1) ^ fiopin) & 0x800) {
- ++encoder1_val;
- } else {
- --encoder1_val;
- }
- SC->EXTPOLAR ^= 0x2;
- SC->EXTINT = 0x2;
-}
-// ENC1B 2.12
-void EINT2_IRQHandler(void) {
- SC->EXTINT = 0x4;
- int fiopin = GPIO2->FIOPIN;
- if (((fiopin >> 1) ^ fiopin) & 0x800) {
- --encoder1_val;
- } else {
- ++encoder1_val;
- }
- SC->EXTPOLAR ^= 0x4;
- SC->EXTINT = 0x4;
-}
-
-// GPIO Interrupt handlers
-static void NoGPIO() {}
-static void Encoder2ARise() {
- GPIOINT->IO0IntClr |= (1 << 22);
- if (GPIO0->FIOPIN & (1 << 21)) {
- ++encoder2_val;
- } else {
- --encoder2_val;
- }
-}
-static void Encoder2AFall() {
- GPIOINT->IO0IntClr |= (1 << 22);
- if (GPIO0->FIOPIN & (1 << 21)) {
- --encoder2_val;
- } else {
- ++encoder2_val;
- }
-}
-static void Encoder2BRise() {
- GPIOINT->IO0IntClr |= (1 << 21);
- if (GPIO0->FIOPIN & (1 << 22)) {
- --encoder2_val;
- } else {
- ++encoder2_val;
- }
-}
-static void Encoder2BFall() {
- GPIOINT->IO0IntClr |= (1 << 21);
- if (GPIO0->FIOPIN & (1 << 22)) {
- ++encoder2_val;
- } else {
- --encoder2_val;
- }
-}
-
-static void Encoder3ARise() {
- GPIOINT->IO0IntClr |= (1 << 20);
- if (GPIO0->FIOPIN & (1 << 19)) {
- ++encoder3_val;
- } else {
- --encoder3_val;
- }
-}
-static void Encoder3AFall() {
- GPIOINT->IO0IntClr |= (1 << 20);
- if (GPIO0->FIOPIN & (1 << 19)) {
- --encoder3_val;
- } else {
- ++encoder3_val;
- }
-}
-static void Encoder3BRise() {
- GPIOINT->IO0IntClr |= (1 << 19);
- if (GPIO0->FIOPIN & (1 << 20)) {
- --encoder3_val;
- } else {
- ++encoder3_val;
- }
-}
-static void Encoder3BFall() {
- GPIOINT->IO0IntClr |= (1 << 19);
- if (GPIO0->FIOPIN & (1 << 20)) {
- ++encoder3_val;
- } else {
- --encoder3_val;
- }
-}
-
-static void Encoder4ARise() {
- GPIOINT->IO2IntClr |= (1 << 0);
- if (GPIO2->FIOPIN & (1 << 1)) {
- ++encoder4_val;
- } else {
- --encoder4_val;
- }
-}
-static void Encoder4AFall() {
- GPIOINT->IO2IntClr |= (1 << 0);
- if (GPIO2->FIOPIN & (1 << 1)) {
- --encoder4_val;
- } else {
- ++encoder4_val;
- }
-}
-static void Encoder4BRise() {
- GPIOINT->IO2IntClr |= (1 << 1);
- if (GPIO2->FIOPIN & (1 << 0)) {
- --encoder4_val;
- } else {
- ++encoder4_val;
- }
-}
-static void Encoder4BFall() {
- GPIOINT->IO2IntClr |= (1 << 1);
- if (GPIO2->FIOPIN & (1 << 0)) {
- ++encoder4_val;
- } else {
- --encoder4_val;
- }
-}
-
-static void Encoder5ARise() {
- GPIOINT->IO2IntClr |= (1 << 2);
- if (GPIO2->FIOPIN & (1 << 3)) {
- ++encoder5_val;
- } else {
- --encoder5_val;
- }
-}
-static void Encoder5AFall() {
- GPIOINT->IO2IntClr |= (1 << 2);
- if (GPIO2->FIOPIN & (1 << 3)) {
- --encoder5_val;
- } else {
- ++encoder5_val;
- }
-}
-static void Encoder5BRise() {
- GPIOINT->IO2IntClr |= (1 << 3);
- if (GPIO2->FIOPIN & (1 << 2)) {
- --encoder5_val;
- } else {
- ++encoder5_val;
- }
-}
-static void Encoder5BFall() {
- GPIOINT->IO2IntClr |= (1 << 3);
- if (GPIO2->FIOPIN & (1 << 2)) {
- ++encoder5_val;
- } else {
- --encoder5_val;
- }
-}
-
-volatile int32_t capture_top_rise;
-volatile int8_t top_rise_count;
-static void IndexerTopRise() {
- GPIOINT->IO0IntClr |= (1 << 5);
- // edge counting encoder capture
- ++top_rise_count;
- capture_top_rise = encoder3_val;
-}
-volatile int32_t capture_top_fall;
-volatile int8_t top_fall_count;
-static void IndexerTopFall() {
- GPIOINT->IO0IntClr |= (1 << 5);
- // edge counting encoder capture
- ++top_fall_count;
- capture_top_fall = encoder3_val;
-}
-volatile int8_t bottom_rise_count;
-static void IndexerBottomRise() {
- GPIOINT->IO0IntClr |= (1 << 4);
- // edge counting
- ++bottom_rise_count;
-}
-volatile int32_t capture_bottom_fall_delay;
-volatile int8_t bottom_fall_delay_count;
-volatile int32_t dirty_delay;
-portTickType xDelayTimeFrom;
-static portTASK_FUNCTION(vDelayCapture, pvParameters)
-{
- portTickType xSleepFrom = xTaskGetTickCount();
-
- for (;;) {
- NVIC_DisableIRQ(EINT3_IRQn);
- if (dirty_delay != 0) {
- xSleepFrom = xDelayTimeFrom;
- dirty_delay = 0;
- NVIC_EnableIRQ(EINT3_IRQn);
-
- vTaskDelayUntil(&xSleepFrom, kBottomFallDelayTime / portTICK_RATE_MS);
-
- NVIC_DisableIRQ(EINT3_IRQn);
- ++bottom_fall_delay_count;
- capture_bottom_fall_delay = encoder3_val;
- NVIC_EnableIRQ(EINT3_IRQn);
- } else {
- NVIC_EnableIRQ(EINT3_IRQn);
- vTaskDelayUntil(&xSleepFrom, 10 / portTICK_RATE_MS);
- }
- }
-}
-
-volatile int8_t bottom_fall_count;
-static void IndexerBottomFall() {
- GPIOINT->IO0IntClr |= (1 << 4);
- ++bottom_fall_count;
- // edge counting start delayed capture
- xDelayTimeFrom = xTaskGetTickCount();
- dirty_delay = 1;
-}
-volatile int32_t capture_wrist_rise;
-volatile int8_t wrist_rise_count;
-static void WristHallRise() {
- GPIOINT->IO0IntClr |= (1 << 6);
- // edge counting encoder capture
- ++wrist_rise_count;
- capture_wrist_rise = (int32_t)QEI->QEIPOS;
-}
-volatile int32_t capture_shooter_angle_rise;
-volatile int8_t shooter_angle_rise_count;
-static void ShooterHallRise() {
- GPIOINT->IO0IntClr |= (1 << 7);
- // edge counting encoder capture
- ++shooter_angle_rise_count;
- capture_shooter_angle_rise = encoder2_val;
-}
-
-// Count leading zeros.
-// Returns 0 if bit 31 is set etc.
-__attribute__((always_inline)) static __INLINE uint32_t __clz(uint32_t value) {
- uint32_t result;
- __asm__("clz %0, %1" : "=r" (result) : "r" (value));
- return result;
-}
-inline static void IRQ_Dispatch(void) {
- // TODO(brians): think about adding a loop here so that we can handle multiple
- // interrupts right on top of each other faster
- uint32_t index = __clz(GPIOINT->IO2IntStatR | GPIOINT->IO0IntStatR |
- (GPIOINT->IO2IntStatF << 28) | (GPIOINT->IO0IntStatF << 4));
-
- typedef void (*Handler)(void);
- const static Handler table[] = {
- Encoder5BFall, // index 0: P2.3 Fall #bit 31 //Encoder 5 B //Dio 10
- Encoder5AFall, // index 1: P2.2 Fall #bit 30 //Encoder 5 A //Dio 9
- Encoder4BFall, // index 2: P2.1 Fall #bit 29 //Encoder 4 B //Dio 8
- Encoder4AFall, // index 3: P2.0 Fall #bit 28 //Encoder 4 A //Dio 7
- NoGPIO, // index 4: NO GPIO #bit 27
- Encoder2AFall, // index 5: P0.22 Fall #bit 26 //Encoder 2 A
- Encoder2BFall, // index 6: P0.21 Fall #bit 25 //Encoder 2 B
- Encoder3AFall, // index 7: P0.20 Fall #bit 24 //Encoder 3 A
- Encoder3BFall, // index 8: P0.19 Fall #bit 23 //Encoder 3 B
- Encoder2ARise, // index 9: P0.22 Rise #bit 22 //Encoder 2 A
- Encoder2BRise, // index 10: P0.21 Rise #bit 21 //Encoder 2 B
- Encoder3ARise, // index 11: P0.20 Rise #bit 20 //Encoder 3 A
- Encoder3BRise, // index 12: P0.19 Rise #bit 19 //Encoder 3 B
- NoGPIO, // index 13: NO GPIO #bit 18
- NoGPIO, // index 14: NO GPIO #bit 17
- NoGPIO, // index 15: NO GPIO #bit 16
- NoGPIO, // index 16: NO GPIO #bit 15
- NoGPIO, // index 17: NO GPIO #bit 14
- NoGPIO, // index 18: NO GPIO #bit 13
- NoGPIO, // index 19: NO GPIO #bit 12
- ShooterHallRise, // index 20: P0.7 Fall #bit 11 //Shooter Hall //Dio 4
- WristHallRise, // index 21: P0.6 Fall #bit 10 //Wrist Hall //Dio 3
- IndexerTopRise, // index 22: P0.5 Fall #bit 9 //Indexer Top //Dio 2
- IndexerBottomRise, // index 23: P0.4 Fall #bit 8 //Indexer Bottom //Dio 1
- NoGPIO, // index 24: NO GPIO #bit 7
- NoGPIO, // index 25: NO GPIO #bit 6
- IndexerTopFall, // index 26: P0.5 Rise #bit 5 //Indexer Top //Dio 2
- IndexerBottomFall, // index 27: P0.4 Rise #bit 4 //Indexer Bottom //Dio 1
- Encoder5BRise, // index 28: P2.3 Rise #bit 3 //Encoder 5 B //Dio 10
- Encoder5ARise, // index 29: P2.2 Rise #bit 2 //Encoder 5 A //Dio 9
- Encoder4BRise, // index 30: P2.1 Rise #bit 1 //Encoder 4 B //Dio 8
- Encoder4ARise, // index 31: P2.0 Rise #bit 0 //Encoder 4 A //Dio 7
- NoGPIO // index 32: NO BITS SET #False Alarm
- };
- table[index]();
-}
-void EINT3_IRQHandler(void) {
- // Have to disable it here or else it re-fires the interrupt while the code
- // reads to figure out which pin the interrupt is for.
- // TODO(brians): figure out details + look for an alternative
- NVIC_DisableIRQ(EINT3_IRQn);
- IRQ_Dispatch();
- NVIC_EnableIRQ(EINT3_IRQn);
-}
-int32_t encoder_val(int chan) {
- int32_t val;
- switch (chan) {
- case 0: // Wrist
- return (int32_t)QEI->QEIPOS;
- case 1: // Shooter Wheel
- NVIC_DisableIRQ(EINT1_IRQn);
- NVIC_DisableIRQ(EINT2_IRQn);
- val = encoder1_val;
- NVIC_EnableIRQ(EINT2_IRQn);
- NVIC_EnableIRQ(EINT1_IRQn);
- return val;
- case 2: // Shooter Angle
- NVIC_DisableIRQ(EINT3_IRQn);
- val = encoder2_val;
- NVIC_EnableIRQ(EINT3_IRQn);
- return val;
- case 3: // Indexer
- NVIC_DisableIRQ(EINT3_IRQn);
- val = encoder3_val;
- NVIC_EnableIRQ(EINT3_IRQn);
- return val;
- case 4: // Drive R
- NVIC_DisableIRQ(EINT3_IRQn);
- val = encoder4_val;
- NVIC_EnableIRQ(EINT3_IRQn);
- return val;
- case 5: // Drive L
- NVIC_DisableIRQ(EINT3_IRQn);
- val = encoder5_val;
- NVIC_EnableIRQ(EINT3_IRQn);
- return val;
- default:
- return -1;
- }
-}
-void fillSensorPacket(struct DataStruct *packet) {
- packet->gyro_angle = gyro_angle;
-
- packet->shooter = encoder1_val;
- packet->left_drive = encoder4_val;
- packet->right_drive = encoder5_val;
- packet->shooter_angle = encoder2_val;
- packet->indexer = encoder3_val;
-
- NVIC_DisableIRQ(EINT1_IRQn);
- NVIC_DisableIRQ(EINT2_IRQn);
-
- packet->wrist = (int32_t)QEI->QEIPOS;
- packet->wrist_hall_effect = !digital(3);
- packet->capture_wrist_rise = capture_wrist_rise;
- packet->wrist_rise_count = wrist_rise_count;
-
- NVIC_EnableIRQ(EINT1_IRQn);
- NVIC_EnableIRQ(EINT2_IRQn);
-
- NVIC_DisableIRQ(EINT3_IRQn);
-
- packet->capture_top_rise = capture_top_rise;
- packet->top_rise_count = top_rise_count;
-
- packet->capture_top_fall = capture_top_fall;
- packet->top_fall_count = top_fall_count;
- packet->top_disc = !digital(2);
-
- packet->capture_bottom_fall_delay = capture_bottom_fall_delay;
- packet->bottom_fall_delay_count = bottom_fall_delay_count;
- packet->bottom_fall_count = bottom_fall_count;
- packet->bottom_disc = !digital(1);
-
- packet->capture_shooter_angle_rise = capture_shooter_angle_rise;
- packet->shooter_angle_rise_count = shooter_angle_rise_count;
- packet->angle_adjust_bottom_hall_effect = !digital(4);
-
- NVIC_EnableIRQ(EINT3_IRQn);
-
- packet->bottom_rise_count = bottom_rise_count;
-}
-
-void encoder_init(void) {
- // Setup the encoder interface.
- SC->PCONP |= PCONP_PCQEI;
- PINCON->PINSEL3 = ((PINCON->PINSEL3 & 0xffff3dff) | 0x00004100);
- // Reset the count and velocity.
- QEI->QEICON = 0x00000005;
- QEI->QEICONF = 0x00000004;
- // Wrap back to 0 when we wrap the int and vice versa.
- QEI->QEIMAXPOS = 0xFFFFFFFF;
-
- // Set up encoder 1.
- // Make GPIOs 2.11 and 2.12 trigger EINT1 and EINT2 (respectively).
- // PINSEL4[23:22] = {0 1}
- // PINSEL4[25:24] = {0 1}
- PINCON->PINSEL4 = (PINCON->PINSEL4 & ~(0x3 << 22)) | (0x1 << 22);
- PINCON->PINSEL4 = (PINCON->PINSEL4 & ~(0x3 << 24)) | (0x1 << 24);
- // Clear the interrupt flags for EINT1 and EINT2 (0x6 = 0b0110).
- SC->EXTMODE = 0x6;
- SC->EXTINT = 0x6;
- NVIC_EnableIRQ(EINT1_IRQn);
- NVIC_EnableIRQ(EINT2_IRQn);
- encoder1_val = 0;
-
- // Set up encoder 2.
- GPIOINT->IO0IntEnF |= (1 << 22); // Set GPIO falling interrupt.
- GPIOINT->IO0IntEnR |= (1 << 22); // Set GPIO rising interrupt.
- GPIOINT->IO0IntEnF |= (1 << 21); // Set GPIO falling interrupt.
- GPIOINT->IO0IntEnR |= (1 << 21); // Set GPIO rising interrupt.
- // Make sure they're in mode 00 (the default, aka nothing special).
- PINCON->PINSEL1 &= ~(0x3 << 12);
- PINCON->PINSEL1 &= ~(0x3 << 10);
- encoder2_val = 0;
-
- // Set up encoder 3.
- GPIOINT->IO0IntEnF |= (1 << 20); // Set GPIO falling interrupt.
- GPIOINT->IO0IntEnR |= (1 << 20); // Set GPIO rising interrupt.
- GPIOINT->IO0IntEnF |= (1 << 19); // Set GPIO falling interrupt.
- GPIOINT->IO0IntEnR |= (1 << 19); // Set GPIO rising interrupt.
- // Make sure they're in mode 00 (the default, aka nothing special).
- PINCON->PINSEL1 &= ~(0x3 << 8);
- PINCON->PINSEL1 &= ~(0x3 << 6);
- encoder3_val = 0;
-
- // Set up encoder 4.
- GPIOINT->IO2IntEnF |= (1 << 0); // Set GPIO falling interrupt.
- GPIOINT->IO2IntEnR |= (1 << 0); // Set GPIO rising interrupt.
- GPIOINT->IO2IntEnF |= (1 << 1); // Set GPIO falling interrupt.
- GPIOINT->IO2IntEnR |= (1 << 1); // Set GPIO rising interrupt.
- // Make sure they're in mode 00 (the default, aka nothing special).
- PINCON->PINSEL4 &= ~(0x3 << 0);
- PINCON->PINSEL4 &= ~(0x3 << 2);
- encoder4_val = 0;
-
- // Set up encoder 5.
- GPIOINT->IO2IntEnF |= (1 << 2); // Set GPIO falling interrupt.
- GPIOINT->IO2IntEnR |= (1 << 2); // Set GPIO rising interrupt.
- GPIOINT->IO2IntEnF |= (1 << 3); // Set GPIO falling interrupt.
- GPIOINT->IO2IntEnR |= (1 << 3); // Set GPIO rising interrupt.
- // Make sure they're in mode 00 (the default, aka nothing special).
- PINCON->PINSEL4 &= ~(0x3 << 4);
- PINCON->PINSEL4 &= ~(0x3 << 6);
- encoder5_val = 0;
-
- // Enable interrupts from the GPIO pins.
- NVIC_EnableIRQ(EINT3_IRQn);
-
- xTaskCreate(vDelayCapture,
- (signed char *) "SENSORs",
- configMINIMAL_STACK_SIZE + 100,
- NULL /*parameters*/,
- tskIDLE_PRIORITY + 5,
- NULL /*return task handle*/);
-
- GPIOINT->IO0IntEnF |= (1 << 4); // Set GPIO falling interrupt
- GPIOINT->IO0IntEnR |= (1 << 4); // Set GPIO rising interrupt
- PINCON->PINSEL0 &= ~(0x3 << 8);
-
- GPIOINT->IO0IntEnF |= (1 << 5); // Set GPIO falling interrupt
- GPIOINT->IO0IntEnR |= (1 << 5); // Set GPIO rising interrupt
- PINCON->PINSEL0 &= ~(0x3 << 10);
-
- GPIOINT->IO0IntEnF |= (1 << 6);
- PINCON->PINSEL0 &= ~(0x3 << 12);
-
- GPIOINT->IO0IntEnF |= (1 << 7);
- PINCON->PINSEL0 &= ~(0x3 << 14);
-}
diff --git a/gyro_board/src/usb/analog.h b/gyro_board/src/usb/analog.h
index b72218b..435d194 100644
--- a/gyro_board/src/usb/analog.h
+++ b/gyro_board/src/usb/analog.h
@@ -1,63 +1,11 @@
#ifndef __ANALOG_H__
#define __ANALOG_H__
+#include <stdint.h>
+
extern int64_t gyro_angle;
-struct DataStruct {
- int64_t gyro_angle;
-
- int32_t left_drive;
- int32_t right_drive;
- int32_t shooter_angle;
- int32_t shooter;
- int32_t indexer;
- int32_t wrist;
-
- int32_t capture_top_rise;
- int32_t capture_top_fall;
- int32_t capture_bottom_fall_delay;
- int32_t capture_wrist_rise;
- int32_t capture_shooter_angle_rise;
-
- int8_t top_rise_count;
-
- int8_t top_fall_count;
-
- int8_t bottom_rise_count;
-
- int8_t bottom_fall_delay_count;
- int8_t bottom_fall_count;
-
- int8_t wrist_rise_count;
-
- int8_t shooter_angle_rise_count;
-
- union {
- struct {
- uint8_t wrist_hall_effect : 1;
- uint8_t angle_adjust_bottom_hall_effect : 1;
- uint8_t top_disc : 1;
- uint8_t bottom_disc : 1;
- };
- uint32_t digitals;
- };
-} __attribute__((__packed__));
-// Gets called in the USB data output ISR. Assumes that it will not be preempted
-// except by very high priority things.
-void fillSensorPacket(struct DataStruct *packet);
-
void analog_init(void);
int analog(int channel);
-int digital(int channel);
-
-void encoder_init(void);
-// For debugging only.
-// Returns the current values of the inputs for the given encoder (as the low 2
-// bits).
-int encoder_bits(int channel);
-// Returns the current position of the given encoder.
-int32_t encoder_val(int channel);
-
-int dip(int channel);
#endif // __ANALOG_H__
diff --git a/gyro_board/src/usb/data_struct.h b/gyro_board/src/usb/data_struct.h
new file mode 100644
index 0000000..d0a740b
--- /dev/null
+++ b/gyro_board/src/usb/data_struct.h
@@ -0,0 +1,83 @@
+// This isn't really a header file. It's designed to be #included directly into
+// other code (possibly in a namespace or whatever), so it doesn't have include
+// guards.
+// In the gyro board code, fill_packet.h #includes this file.
+// In the fitpc code, frc971/input/gyro_board_data.h #includes this file.
+
+#pragma pack(push, 1)
+struct DATA_STRUCT_NAME {
+ int64_t gyro_angle;
+
+ union {
+ struct {
+ // Which robot (+version) the gyro board is sending out data for.
+ // We should keep this in the same place for all gyro board software
+ // versions so that the fitpc can detect when it's reading from a gyro
+ // board set up for a different robot than it is.
+ // 0 = 2013 competition/practice robot
+ // 1 = 2013 3rd robot
+ uint8_t robot_id;
+ // This information should also be kept in the same place from year to
+ // year so that the fitpc code can record the dip switch values when it
+ // detects the wrong robot id to make debugging easier.
+ union {
+ struct {
+ uint8_t dip_switch0 : 1;
+ uint8_t dip_switch1 : 1;
+ uint8_t dip_switch2 : 1;
+ uint8_t dip_switch3 : 1;
+ };
+ uint8_t dip_switches;
+ };
+ };
+ uint16_t header;
+ };
+
+ union {
+ struct {
+ union {
+ struct {
+ uint8_t wrist_hall_effect : 1;
+ uint8_t angle_adjust_bottom_hall_effect : 1;
+ uint8_t top_disc : 1;
+ uint8_t bottom_disc : 1;
+ };
+ uint16_t booleans;
+ };
+ int32_t left_drive;
+ int32_t right_drive;
+ int32_t shooter_angle;
+ int32_t shooter;
+ int32_t indexer;
+ int32_t wrist;
+
+ int32_t capture_top_rise;
+ int32_t capture_top_fall;
+ int32_t capture_bottom_fall_delay;
+ int32_t capture_wrist_rise;
+ int32_t capture_shooter_angle_rise;
+
+ int8_t top_rise_count;
+
+ int8_t top_fall_count;
+
+ int8_t bottom_rise_count;
+
+ int8_t bottom_fall_delay_count;
+ int8_t bottom_fall_count;
+
+ int8_t wrist_rise_count;
+
+ int8_t shooter_angle_rise_count;
+ } main;
+
+ struct {
+ union {
+ struct {
+ };
+ uint16_t booleans;
+ };
+ } bot3;
+ };
+};
+#pragma pack(pop)
diff --git a/gyro_board/src/usb/digital.c b/gyro_board/src/usb/digital.c
new file mode 100644
index 0000000..aed2d71
--- /dev/null
+++ b/gyro_board/src/usb/digital.c
@@ -0,0 +1,49 @@
+#include "digital.h"
+
+#include "FreeRTOS.h"
+
+inline int readGPIO_inline(int major, int minor) {
+ switch (major) {
+ case 0:
+ return readGPIO(GPIO0, minor);
+ case 1:
+ return readGPIO(GPIO1, minor);
+ case 2:
+ return readGPIO(GPIO2, minor);
+ default:
+ return -1;
+ }
+}
+
+int digital(int channel) {
+ if (channel < 1) {
+ return -1;
+ } else if (channel < 7) {
+ int chan = channel + 3;
+ return readGPIO(GPIO0, chan);
+ } else if (channel < 13) {
+ int chan = channel - 7;
+ return readGPIO(GPIO2, chan);
+ }
+ return -1;
+}
+
+int dip_switch(int channel) {
+ switch (channel) {
+ case 0:
+ return readGPIO(GPIO1, 29);
+ case 1:
+ return readGPIO(GPIO2, 13);
+ case 2:
+ return readGPIO(GPIO0, 11);
+ case 3:
+ return readGPIO(GPIO0, 10);
+ default:
+ return -1;
+ }
+}
+
+int is_bot3;
+void digital_init(void) {
+ is_bot3 = 0;
+}
diff --git a/gyro_board/src/usb/digital.h b/gyro_board/src/usb/digital.h
new file mode 100644
index 0000000..6d6e7ac
--- /dev/null
+++ b/gyro_board/src/usb/digital.h
@@ -0,0 +1,46 @@
+#ifndef GYRO_BOARD_USB_DIGITAL_H_
+#define GYRO_BOARD_USB_DIGITAL_H_
+
+#define readGPIO(gpio, chan) ((((gpio)->FIOPIN) >> (chan)) & 1)
+
+// These are the actual pin numbers for all of the digital I(/0) pins on the
+// board.
+//
+// GPIO1 P0.4
+// GPIO2 P0.5
+// GPIO3 P0.6
+// GPIO4 P0.7
+// GPIO5 P0.8
+// GPIO6 P0.9
+// GPIO7 P2.0
+// GPIO8 P2.1
+// GPIO9 P2.2
+// GPIO10 P2.3
+// GPIO11 P2.4
+// GPIO12 P2.5
+//
+// DIP0 P1.29
+// DIP1 P2.13
+// DIP2 P0.11
+// DIP3 P0.10
+//
+// ENC0A 1.20
+// ENC0B 1.23
+// ENC1A 2.11
+// ENC1B 2.12
+// ENC2A 0.21
+// ENC2B 0.22
+// ENC3A 0.19
+// ENC3B 0.20
+
+void digital_init(void);
+
+int digital(int channel);
+
+int dip_switch(int channel);
+
+// Boolean set by digital_init() which says whether or not this is the 3rd
+// robot.
+extern int is_bot3;
+
+#endif // GYRO_BOARD_USB_DIGITAL_H_
diff --git a/gyro_board/src/usb/encoder.c b/gyro_board/src/usb/encoder.c
new file mode 100644
index 0000000..ff0a282
--- /dev/null
+++ b/gyro_board/src/usb/encoder.c
@@ -0,0 +1,517 @@
+#include "fill_packet.h"
+#include "encoder.h"
+
+#include "FreeRTOS.h"
+#include "task.h"
+
+#include "digital.h"
+#include "analog.h"
+
+// How long (in ms) to wait after a falling edge on the bottom indexer sensor
+// before reading the indexer encoder.
+static const int kBottomFallDelayTime = 32;
+
+#define ENC(gpio, a, b) readGPIO(gpio, a) * 2 + readGPIO(gpio, b)
+int encoder_bits(int channel) {
+ switch (channel) {
+ case 0:
+ return ENC(GPIO1, 20, 23);
+ case 1:
+ return ENC(GPIO2, 11, 12);
+ case 2:
+ return ENC(GPIO0, 21, 22);
+ case 3:
+ return ENC(GPIO0, 19, 20);
+ default:
+ return -1;
+ }
+ return -1;
+}
+#undef ENC
+
+// Uses EINT1 and EINT2 on 2.11 and 2.12.
+volatile int32_t encoder1_val;
+// On GPIO pins 0.22 and 0.21.
+volatile int32_t encoder2_val;
+// On GPIO pins 0.20 and 0.19.
+volatile int32_t encoder3_val;
+// On GPIO pins 2.0 and 2.1.
+volatile int32_t encoder4_val;
+// On GPIO pins 2.2 and 2.3.
+volatile int32_t encoder5_val;
+
+// ENC1A 2.11
+void EINT1_IRQHandler(void) {
+ // TODO(brians): figure out why this has to be up here too
+ SC->EXTINT = 0x2;
+ int fiopin = GPIO2->FIOPIN;
+ if (((fiopin >> 1) ^ fiopin) & 0x800) {
+ ++encoder1_val;
+ } else {
+ --encoder1_val;
+ }
+ SC->EXTPOLAR ^= 0x2;
+ SC->EXTINT = 0x2;
+}
+// ENC1B 2.12
+void EINT2_IRQHandler(void) {
+ SC->EXTINT = 0x4;
+ int fiopin = GPIO2->FIOPIN;
+ if (((fiopin >> 1) ^ fiopin) & 0x800) {
+ --encoder1_val;
+ } else {
+ ++encoder1_val;
+ }
+ SC->EXTPOLAR ^= 0x4;
+ SC->EXTINT = 0x4;
+}
+
+// GPIO Interrupt handlers
+static void NoGPIO() {}
+static void Encoder2ARise() {
+ GPIOINT->IO0IntClr |= (1 << 22);
+ if (GPIO0->FIOPIN & (1 << 21)) {
+ ++encoder2_val;
+ } else {
+ --encoder2_val;
+ }
+}
+static void Encoder2AFall() {
+ GPIOINT->IO0IntClr |= (1 << 22);
+ if (GPIO0->FIOPIN & (1 << 21)) {
+ --encoder2_val;
+ } else {
+ ++encoder2_val;
+ }
+}
+static void Encoder2BRise() {
+ GPIOINT->IO0IntClr |= (1 << 21);
+ if (GPIO0->FIOPIN & (1 << 22)) {
+ --encoder2_val;
+ } else {
+ ++encoder2_val;
+ }
+}
+static void Encoder2BFall() {
+ GPIOINT->IO0IntClr |= (1 << 21);
+ if (GPIO0->FIOPIN & (1 << 22)) {
+ ++encoder2_val;
+ } else {
+ --encoder2_val;
+ }
+}
+
+static void Encoder3ARise() {
+ GPIOINT->IO0IntClr |= (1 << 20);
+ if (GPIO0->FIOPIN & (1 << 19)) {
+ ++encoder3_val;
+ } else {
+ --encoder3_val;
+ }
+}
+static void Encoder3AFall() {
+ GPIOINT->IO0IntClr |= (1 << 20);
+ if (GPIO0->FIOPIN & (1 << 19)) {
+ --encoder3_val;
+ } else {
+ ++encoder3_val;
+ }
+}
+static void Encoder3BRise() {
+ GPIOINT->IO0IntClr |= (1 << 19);
+ if (GPIO0->FIOPIN & (1 << 20)) {
+ --encoder3_val;
+ } else {
+ ++encoder3_val;
+ }
+}
+static void Encoder3BFall() {
+ GPIOINT->IO0IntClr |= (1 << 19);
+ if (GPIO0->FIOPIN & (1 << 20)) {
+ ++encoder3_val;
+ } else {
+ --encoder3_val;
+ }
+}
+
+static void Encoder4ARise() {
+ GPIOINT->IO2IntClr |= (1 << 0);
+ if (GPIO2->FIOPIN & (1 << 1)) {
+ ++encoder4_val;
+ } else {
+ --encoder4_val;
+ }
+}
+static void Encoder4AFall() {
+ GPIOINT->IO2IntClr |= (1 << 0);
+ if (GPIO2->FIOPIN & (1 << 1)) {
+ --encoder4_val;
+ } else {
+ ++encoder4_val;
+ }
+}
+static void Encoder4BRise() {
+ GPIOINT->IO2IntClr |= (1 << 1);
+ if (GPIO2->FIOPIN & (1 << 0)) {
+ --encoder4_val;
+ } else {
+ ++encoder4_val;
+ }
+}
+static void Encoder4BFall() {
+ GPIOINT->IO2IntClr |= (1 << 1);
+ if (GPIO2->FIOPIN & (1 << 0)) {
+ ++encoder4_val;
+ } else {
+ --encoder4_val;
+ }
+}
+
+static void Encoder5ARise() {
+ GPIOINT->IO2IntClr |= (1 << 2);
+ if (GPIO2->FIOPIN & (1 << 3)) {
+ ++encoder5_val;
+ } else {
+ --encoder5_val;
+ }
+}
+static void Encoder5AFall() {
+ GPIOINT->IO2IntClr |= (1 << 2);
+ if (GPIO2->FIOPIN & (1 << 3)) {
+ --encoder5_val;
+ } else {
+ ++encoder5_val;
+ }
+}
+static void Encoder5BRise() {
+ GPIOINT->IO2IntClr |= (1 << 3);
+ if (GPIO2->FIOPIN & (1 << 2)) {
+ --encoder5_val;
+ } else {
+ ++encoder5_val;
+ }
+}
+static void Encoder5BFall() {
+ GPIOINT->IO2IntClr |= (1 << 3);
+ if (GPIO2->FIOPIN & (1 << 2)) {
+ ++encoder5_val;
+ } else {
+ --encoder5_val;
+ }
+}
+
+volatile int32_t capture_top_rise;
+volatile int8_t top_rise_count;
+static void IndexerTopRise() {
+ GPIOINT->IO0IntClr |= (1 << 5);
+ // edge counting encoder capture
+ ++top_rise_count;
+ capture_top_rise = encoder3_val;
+}
+volatile int32_t capture_top_fall;
+volatile int8_t top_fall_count;
+static void IndexerTopFall() {
+ GPIOINT->IO0IntClr |= (1 << 5);
+ // edge counting encoder capture
+ ++top_fall_count;
+ capture_top_fall = encoder3_val;
+}
+volatile int8_t bottom_rise_count;
+static void IndexerBottomRise() {
+ GPIOINT->IO0IntClr |= (1 << 4);
+ // edge counting
+ ++bottom_rise_count;
+}
+volatile int32_t capture_bottom_fall_delay;
+volatile int8_t bottom_fall_delay_count;
+volatile int32_t dirty_delay;
+portTickType xDelayTimeFrom;
+static portTASK_FUNCTION(vDelayCapture, pvParameters)
+{
+ portTickType xSleepFrom = xTaskGetTickCount();
+
+ for (;;) {
+ NVIC_DisableIRQ(EINT3_IRQn);
+ if (dirty_delay != 0) {
+ xSleepFrom = xDelayTimeFrom;
+ dirty_delay = 0;
+ NVIC_EnableIRQ(EINT3_IRQn);
+
+ vTaskDelayUntil(&xSleepFrom, kBottomFallDelayTime / portTICK_RATE_MS);
+
+ NVIC_DisableIRQ(EINT3_IRQn);
+ ++bottom_fall_delay_count;
+ capture_bottom_fall_delay = encoder3_val;
+ NVIC_EnableIRQ(EINT3_IRQn);
+ } else {
+ NVIC_EnableIRQ(EINT3_IRQn);
+ vTaskDelayUntil(&xSleepFrom, 10 / portTICK_RATE_MS);
+ }
+ }
+}
+
+volatile int8_t bottom_fall_count;
+static void IndexerBottomFall() {
+ GPIOINT->IO0IntClr |= (1 << 4);
+ ++bottom_fall_count;
+ // edge counting start delayed capture
+ xDelayTimeFrom = xTaskGetTickCount();
+ dirty_delay = 1;
+}
+volatile int32_t capture_wrist_rise;
+volatile int8_t wrist_rise_count;
+static void WristHallRise() {
+ GPIOINT->IO0IntClr |= (1 << 6);
+ // edge counting encoder capture
+ ++wrist_rise_count;
+ capture_wrist_rise = (int32_t)QEI->QEIPOS;
+}
+volatile int32_t capture_shooter_angle_rise;
+volatile int8_t shooter_angle_rise_count;
+static void ShooterHallRise() {
+ GPIOINT->IO0IntClr |= (1 << 7);
+ // edge counting encoder capture
+ ++shooter_angle_rise_count;
+ capture_shooter_angle_rise = encoder2_val;
+}
+
+// Count leading zeros.
+// Returns 0 if bit 31 is set etc.
+__attribute__((always_inline)) static __INLINE uint32_t __clz(uint32_t value) {
+ uint32_t result;
+ __asm__("clz %0, %1" : "=r" (result) : "r" (value));
+ return result;
+}
+inline static void IRQ_Dispatch(void) {
+ // TODO(brians): think about adding a loop here so that we can handle multiple
+ // interrupts right on top of each other faster
+ uint32_t index = __clz(GPIOINT->IO2IntStatR | GPIOINT->IO0IntStatR |
+ (GPIOINT->IO2IntStatF << 28) | (GPIOINT->IO0IntStatF << 4));
+
+ typedef void (*Handler)(void);
+ const static Handler table[] = {
+ Encoder5BFall, // index 0: P2.3 Fall #bit 31 //Encoder 5 B //Dio 10
+ Encoder5AFall, // index 1: P2.2 Fall #bit 30 //Encoder 5 A //Dio 9
+ Encoder4BFall, // index 2: P2.1 Fall #bit 29 //Encoder 4 B //Dio 8
+ Encoder4AFall, // index 3: P2.0 Fall #bit 28 //Encoder 4 A //Dio 7
+ NoGPIO, // index 4: NO GPIO #bit 27
+ Encoder2AFall, // index 5: P0.22 Fall #bit 26 //Encoder 2 A
+ Encoder2BFall, // index 6: P0.21 Fall #bit 25 //Encoder 2 B
+ Encoder3AFall, // index 7: P0.20 Fall #bit 24 //Encoder 3 A
+ Encoder3BFall, // index 8: P0.19 Fall #bit 23 //Encoder 3 B
+ Encoder2ARise, // index 9: P0.22 Rise #bit 22 //Encoder 2 A
+ Encoder2BRise, // index 10: P0.21 Rise #bit 21 //Encoder 2 B
+ Encoder3ARise, // index 11: P0.20 Rise #bit 20 //Encoder 3 A
+ Encoder3BRise, // index 12: P0.19 Rise #bit 19 //Encoder 3 B
+ NoGPIO, // index 13: NO GPIO #bit 18
+ NoGPIO, // index 14: NO GPIO #bit 17
+ NoGPIO, // index 15: NO GPIO #bit 16
+ NoGPIO, // index 16: NO GPIO #bit 15
+ NoGPIO, // index 17: NO GPIO #bit 14
+ NoGPIO, // index 18: NO GPIO #bit 13
+ NoGPIO, // index 19: NO GPIO #bit 12
+ ShooterHallRise, // index 20: P0.7 Fall #bit 11 //Shooter Hall //Dio 4
+ WristHallRise, // index 21: P0.6 Fall #bit 10 //Wrist Hall //Dio 3
+ IndexerTopRise, // index 22: P0.5 Fall #bit 9 //Indexer Top //Dio 2
+ IndexerBottomRise, // index 23: P0.4 Fall #bit 8 //Indexer Bottom //Dio 1
+ NoGPIO, // index 24: NO GPIO #bit 7
+ NoGPIO, // index 25: NO GPIO #bit 6
+ IndexerTopFall, // index 26: P0.5 Rise #bit 5 //Indexer Top //Dio 2
+ IndexerBottomFall, // index 27: P0.4 Rise #bit 4 //Indexer Bottom //Dio 1
+ Encoder5BRise, // index 28: P2.3 Rise #bit 3 //Encoder 5 B //Dio 10
+ Encoder5ARise, // index 29: P2.2 Rise #bit 2 //Encoder 5 A //Dio 9
+ Encoder4BRise, // index 30: P2.1 Rise #bit 1 //Encoder 4 B //Dio 8
+ Encoder4ARise, // index 31: P2.0 Rise #bit 0 //Encoder 4 A //Dio 7
+ NoGPIO // index 32: NO BITS SET #False Alarm
+ };
+ table[index]();
+}
+void EINT3_IRQHandler(void) {
+ // Have to disable it here or else it re-fires the interrupt while the code
+ // reads to figure out which pin the interrupt is for.
+ // TODO(brians): figure out details + look for an alternative
+ NVIC_DisableIRQ(EINT3_IRQn);
+ IRQ_Dispatch();
+ NVIC_EnableIRQ(EINT3_IRQn);
+}
+int32_t encoder_val(int chan) {
+ int32_t val;
+ switch (chan) {
+ case 0: // Wrist
+ return (int32_t)QEI->QEIPOS;
+ case 1: // Shooter Wheel
+ NVIC_DisableIRQ(EINT1_IRQn);
+ NVIC_DisableIRQ(EINT2_IRQn);
+ val = encoder1_val;
+ NVIC_EnableIRQ(EINT2_IRQn);
+ NVIC_EnableIRQ(EINT1_IRQn);
+ return val;
+ case 2: // Shooter Angle
+ NVIC_DisableIRQ(EINT3_IRQn);
+ val = encoder2_val;
+ NVIC_EnableIRQ(EINT3_IRQn);
+ return val;
+ case 3: // Indexer
+ NVIC_DisableIRQ(EINT3_IRQn);
+ val = encoder3_val;
+ NVIC_EnableIRQ(EINT3_IRQn);
+ return val;
+ case 4: // Drive R
+ NVIC_DisableIRQ(EINT3_IRQn);
+ val = encoder4_val;
+ NVIC_EnableIRQ(EINT3_IRQn);
+ return val;
+ case 5: // Drive L
+ NVIC_DisableIRQ(EINT3_IRQn);
+ val = encoder5_val;
+ NVIC_EnableIRQ(EINT3_IRQn);
+ return val;
+ default:
+ return -1;
+ }
+}
+
+void encoder_init(void) {
+ // Setup the encoder interface.
+ SC->PCONP |= PCONP_PCQEI;
+ PINCON->PINSEL3 = ((PINCON->PINSEL3 & 0xffff3dff) | 0x00004100);
+ // Reset the count and velocity.
+ QEI->QEICON = 0x00000005;
+ QEI->QEICONF = 0x00000004;
+ // Wrap back to 0 when we wrap the int and vice versa.
+ QEI->QEIMAXPOS = 0xFFFFFFFF;
+
+ // Set up encoder 1.
+ // Make GPIOs 2.11 and 2.12 trigger EINT1 and EINT2 (respectively).
+ // PINSEL4[23:22] = {0 1}
+ // PINSEL4[25:24] = {0 1}
+ PINCON->PINSEL4 = (PINCON->PINSEL4 & ~(0x3 << 22)) | (0x1 << 22);
+ PINCON->PINSEL4 = (PINCON->PINSEL4 & ~(0x3 << 24)) | (0x1 << 24);
+ // Clear the interrupt flags for EINT1 and EINT2 (0x6 = 0b0110).
+ SC->EXTMODE = 0x6;
+ SC->EXTINT = 0x6;
+ NVIC_EnableIRQ(EINT1_IRQn);
+ NVIC_EnableIRQ(EINT2_IRQn);
+ encoder1_val = 0;
+
+ // Set up encoder 2.
+ GPIOINT->IO0IntEnF |= (1 << 22); // Set GPIO falling interrupt.
+ GPIOINT->IO0IntEnR |= (1 << 22); // Set GPIO rising interrupt.
+ GPIOINT->IO0IntEnF |= (1 << 21); // Set GPIO falling interrupt.
+ GPIOINT->IO0IntEnR |= (1 << 21); // Set GPIO rising interrupt.
+ // Make sure they're in mode 00 (the default, aka nothing special).
+ PINCON->PINSEL1 &= ~(0x3 << 12);
+ PINCON->PINSEL1 &= ~(0x3 << 10);
+ encoder2_val = 0;
+
+ // Set up encoder 3.
+ GPIOINT->IO0IntEnF |= (1 << 20); // Set GPIO falling interrupt.
+ GPIOINT->IO0IntEnR |= (1 << 20); // Set GPIO rising interrupt.
+ GPIOINT->IO0IntEnF |= (1 << 19); // Set GPIO falling interrupt.
+ GPIOINT->IO0IntEnR |= (1 << 19); // Set GPIO rising interrupt.
+ // Make sure they're in mode 00 (the default, aka nothing special).
+ PINCON->PINSEL1 &= ~(0x3 << 8);
+ PINCON->PINSEL1 &= ~(0x3 << 6);
+ encoder3_val = 0;
+
+ // Set up encoder 4.
+ GPIOINT->IO2IntEnF |= (1 << 0); // Set GPIO falling interrupt.
+ GPIOINT->IO2IntEnR |= (1 << 0); // Set GPIO rising interrupt.
+ GPIOINT->IO2IntEnF |= (1 << 1); // Set GPIO falling interrupt.
+ GPIOINT->IO2IntEnR |= (1 << 1); // Set GPIO rising interrupt.
+ // Make sure they're in mode 00 (the default, aka nothing special).
+ PINCON->PINSEL4 &= ~(0x3 << 0);
+ PINCON->PINSEL4 &= ~(0x3 << 2);
+ encoder4_val = 0;
+
+ // Set up encoder 5.
+ GPIOINT->IO2IntEnF |= (1 << 2); // Set GPIO falling interrupt.
+ GPIOINT->IO2IntEnR |= (1 << 2); // Set GPIO rising interrupt.
+ GPIOINT->IO2IntEnF |= (1 << 3); // Set GPIO falling interrupt.
+ GPIOINT->IO2IntEnR |= (1 << 3); // Set GPIO rising interrupt.
+ // Make sure they're in mode 00 (the default, aka nothing special).
+ PINCON->PINSEL4 &= ~(0x3 << 4);
+ PINCON->PINSEL4 &= ~(0x3 << 6);
+ encoder5_val = 0;
+
+ // Enable interrupts from the GPIO pins.
+ NVIC_EnableIRQ(EINT3_IRQn);
+
+ if (is_bot3) {
+ } else { // is main robot
+ xTaskCreate(vDelayCapture,
+ (signed char *) "SENSORs",
+ configMINIMAL_STACK_SIZE + 100,
+ NULL /*parameters*/,
+ tskIDLE_PRIORITY + 5,
+ NULL /*return task handle*/);
+
+ GPIOINT->IO0IntEnF |= (1 << 4); // Set GPIO falling interrupt
+ GPIOINT->IO0IntEnR |= (1 << 4); // Set GPIO rising interrupt
+ PINCON->PINSEL0 &= ~(0x3 << 8);
+
+ GPIOINT->IO0IntEnF |= (1 << 5); // Set GPIO falling interrupt
+ GPIOINT->IO0IntEnR |= (1 << 5); // Set GPIO rising interrupt
+ PINCON->PINSEL0 &= ~(0x3 << 10);
+
+ GPIOINT->IO0IntEnF |= (1 << 6);
+ PINCON->PINSEL0 &= ~(0x3 << 12);
+
+ GPIOINT->IO0IntEnF |= (1 << 7);
+ PINCON->PINSEL0 &= ~(0x3 << 14);
+ }
+}
+
+void fillSensorPacket(struct DataStruct *packet) {
+ packet->gyro_angle = gyro_angle;
+
+ packet->dip_switch0 = dip_switch(0);
+ packet->dip_switch1 = dip_switch(1);
+ packet->dip_switch2 = dip_switch(2);
+ packet->dip_switch3 = dip_switch(3);
+
+ if (is_bot3) {
+ packet->robot_id = 1;
+ } else { // is main robot
+ packet->robot_id = 0;
+
+ packet->main.shooter = encoder1_val;
+ packet->main.left_drive = encoder4_val;
+ packet->main.right_drive = encoder5_val;
+ packet->main.shooter_angle = encoder2_val;
+ packet->main.indexer = encoder3_val;
+
+ NVIC_DisableIRQ(EINT1_IRQn);
+ NVIC_DisableIRQ(EINT2_IRQn);
+
+ packet->main.wrist = (int32_t)QEI->QEIPOS;
+ packet->main.wrist_hall_effect = !digital(3);
+ packet->main.capture_wrist_rise = capture_wrist_rise;
+ packet->main.wrist_rise_count = wrist_rise_count;
+
+ NVIC_EnableIRQ(EINT1_IRQn);
+ NVIC_EnableIRQ(EINT2_IRQn);
+
+ NVIC_DisableIRQ(EINT3_IRQn);
+
+ packet->main.capture_top_rise = capture_top_rise;
+ packet->main.top_rise_count = top_rise_count;
+
+ packet->main.capture_top_fall = capture_top_fall;
+ packet->main.top_fall_count = top_fall_count;
+ packet->main.top_disc = !digital(2);
+
+ packet->main.capture_bottom_fall_delay = capture_bottom_fall_delay;
+ packet->main.bottom_fall_delay_count = bottom_fall_delay_count;
+ packet->main.bottom_fall_count = bottom_fall_count;
+ packet->main.bottom_disc = !digital(1);
+
+ packet->main.capture_shooter_angle_rise = capture_shooter_angle_rise;
+ packet->main.shooter_angle_rise_count = shooter_angle_rise_count;
+ packet->main.angle_adjust_bottom_hall_effect = !digital(4);
+
+ NVIC_EnableIRQ(EINT3_IRQn);
+
+ packet->main.bottom_rise_count = bottom_rise_count;
+ }
+}
diff --git a/gyro_board/src/usb/encoder.h b/gyro_board/src/usb/encoder.h
new file mode 100644
index 0000000..5b1f3ab
--- /dev/null
+++ b/gyro_board/src/usb/encoder.h
@@ -0,0 +1,16 @@
+#ifndef GYRO_BOARD_USB_ENCODER_H_
+#define GYRO_BOARD_USB_ENCODER_H_
+
+#include <stdint.h>
+
+void encoder_init(void);
+
+// For debugging only.
+// Returns the current values of the inputs for the given encoder (as the low 2
+// bits).
+int encoder_bits(int channel);
+
+// Returns the current position of the given encoder.
+int32_t encoder_val(int channel);
+
+#endif // GYRO_BOARD_USB_ENCODER_H_
diff --git a/gyro_board/src/usb/fill_packet.h b/gyro_board/src/usb/fill_packet.h
new file mode 100644
index 0000000..9acc094
--- /dev/null
+++ b/gyro_board/src/usb/fill_packet.h
@@ -0,0 +1,16 @@
+#ifndef GYRO_BOARD_FILL_PACKET_H_
+#define GYRO_BOARD_FILL_PACKET_H_
+
+#include <stdint.h>
+
+#define DATA_STRUCT_NAME DataStruct
+#include "data_struct.h"
+#undef DATA_STRUCT_NAME
+
+// Gets called in the USB data output ISR. Assumes that it will not be preempted
+// except by very high priority things.
+//
+// Implemented in encoder.c because it depends on so many things in there.
+void fillSensorPacket(struct DataStruct *packet);
+
+#endif // GYRO_BOARD_FILL_PACKET_H_
diff --git a/gyro_board/src/usb/main.c b/gyro_board/src/usb/main.c
index 6e69970..f0f76bf 100644
--- a/gyro_board/src/usb/main.c
+++ b/gyro_board/src/usb/main.c
@@ -31,10 +31,13 @@
/* Demo app includes. */
#include "flash.h"
#include "partest.h"
-#include "analog.h"
#include "spi.h"
#include "LPCUSB/usbapi.h"
+#include "analog.h"
+#include "digital.h"
+#include "encoder.h"
+
/*-----------------------------------------------------------*/
/* The time between cycles of the 'check' functionality (defined within the
@@ -86,6 +89,8 @@
vTaskDelayUntil(). */
xLastFlashTime = xTaskGetTickCount();
+ digital_init();
+
analog_init();
encoder_init();