merging the recent gyro board code updates in
I'm merging the wrong way in the middle because the merge is complicated
because there are changes to things that got split out into different
files.
diff --git a/frc971/input/gyro_board_data.h b/frc971/input/gyro_board_data.h
index cc0a4db..6f07a2a 100644
--- a/frc971/input/gyro_board_data.h
+++ b/frc971/input/gyro_board_data.h
@@ -5,72 +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;
- uint8_t loader_top : 1;
- uint8_t loader_bottom : 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 0305bf0..438fc30 100644
--- a/frc971/input/gyro_board_reader.cc
+++ b/frc971/input/gyro_board_reader.cc
@@ -170,8 +170,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)) {
@@ -182,58 +188,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_)
.loader_top(data->loader_top)
.loader_bottom(data->loader_bottom)
diff --git a/frc971/input/gyro_sensor_receiver.cc b/frc971/input/gyro_sensor_receiver.cc
index 574f5f5..a0f5693 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_)
.loader_top(data->loader_top)
.loader_bottom(data->loader_bottom)
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 1499710..424df84 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 efb1e83..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,552 +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) {
- SC->EXTPOLAR ^= 0x2;
- SC->EXTINT = 0x2;
- int fiopin = GPIO2->FIOPIN;
- if (((fiopin >> 1) ^ fiopin) & 0x800) {
- ++encoder1_val;
- } else {
- --encoder1_val;
- }
-}
-// ENC1B 2.12
-void EINT2_IRQHandler(void) {
- SC->EXTPOLAR ^= 0x4;
- SC->EXTINT = 0x4;
- int fiopin = GPIO2->FIOPIN;
- if (((fiopin >> 1) ^ fiopin) & 0x800) {
- --encoder1_val;
- } else {
- ++encoder1_val;
- }
-}
-
-// TODO(brians): Have this indicate some kind of error instead of just looping
-// infinitely in the ISR because it never clears it.
-static void NoGPIO(void) {}
-static void Encoder2ARise(void) {
- GPIOINT->IO0IntClr = (1 << 22);
- if (GPIO0->FIOPIN & (1 << 21)) {
- ++encoder2_val;
- } else {
- --encoder2_val;
- }
-}
-static void Encoder2AFall(void) {
- GPIOINT->IO0IntClr = (1 << 22);
- if (GPIO0->FIOPIN & (1 << 21)) {
- --encoder2_val;
- } else {
- ++encoder2_val;
- }
-}
-static void Encoder2BRise(void) {
- GPIOINT->IO0IntClr = (1 << 21);
- if (GPIO0->FIOPIN & (1 << 22)) {
- --encoder2_val;
- } else {
- ++encoder2_val;
- }
-}
-static void Encoder2BFall(void) {
- GPIOINT->IO0IntClr = (1 << 21);
- if (GPIO0->FIOPIN & (1 << 22)) {
- ++encoder2_val;
- } else {
- --encoder2_val;
- }
-}
-
-static void Encoder3ARise(void) {
- GPIOINT->IO0IntClr = (1 << 20);
- if (GPIO0->FIOPIN & (1 << 19)) {
- ++encoder3_val;
- } else {
- --encoder3_val;
- }
-}
-static void Encoder3AFall(void) {
- GPIOINT->IO0IntClr = (1 << 20);
- if (GPIO0->FIOPIN & (1 << 19)) {
- --encoder3_val;
- } else {
- ++encoder3_val;
- }
-}
-static void Encoder3BRise(void) {
- GPIOINT->IO0IntClr = (1 << 19);
- if (GPIO0->FIOPIN & (1 << 20)) {
- --encoder3_val;
- } else {
- ++encoder3_val;
- }
-}
-static void Encoder3BFall(void) {
- GPIOINT->IO0IntClr = (1 << 19);
- if (GPIO0->FIOPIN & (1 << 20)) {
- ++encoder3_val;
- } else {
- --encoder3_val;
- }
-}
-
-static void Encoder4ARise(void) {
- GPIOINT->IO2IntClr = (1 << 0);
- if (GPIO2->FIOPIN & (1 << 1)) {
- ++encoder4_val;
- } else {
- --encoder4_val;
- }
-}
-static void Encoder4AFall(void) {
- GPIOINT->IO2IntClr = (1 << 0);
- if (GPIO2->FIOPIN & (1 << 1)) {
- --encoder4_val;
- } else {
- ++encoder4_val;
- }
-}
-static void Encoder4BRise(void) {
- GPIOINT->IO2IntClr = (1 << 1);
- if (GPIO2->FIOPIN & (1 << 0)) {
- --encoder4_val;
- } else {
- ++encoder4_val;
- }
-}
-static void Encoder4BFall(void) {
- GPIOINT->IO2IntClr = (1 << 1);
- if (GPIO2->FIOPIN & (1 << 0)) {
- ++encoder4_val;
- } else {
- --encoder4_val;
- }
-}
-
-static void Encoder5ARise(void) {
- GPIOINT->IO2IntClr = (1 << 2);
- if (GPIO2->FIOPIN & (1 << 3)) {
- ++encoder5_val;
- } else {
- --encoder5_val;
- }
-}
-static void Encoder5AFall(void) {
- GPIOINT->IO2IntClr = (1 << 2);
- if (GPIO2->FIOPIN & (1 << 3)) {
- --encoder5_val;
- } else {
- ++encoder5_val;
- }
-}
-static void Encoder5BRise(void) {
- GPIOINT->IO2IntClr = (1 << 3);
- if (GPIO2->FIOPIN & (1 << 2)) {
- --encoder5_val;
- } else {
- ++encoder5_val;
- }
-}
-static void Encoder5BFall(void) {
- 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(void) {
- 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(void) {
- 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(void) {
- 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(USB_IRQn);
- capture_bottom_fall_delay = encoder3_val;
- ++bottom_fall_delay_count;
- NVIC_EnableIRQ(USB_IRQn);
- } else {
- NVIC_EnableIRQ(EINT3_IRQn);
- vTaskDelayUntil(&xSleepFrom, 10 / portTICK_RATE_MS);
- }
- }
-}
-
-volatile int8_t bottom_fall_count;
-static void IndexerBottomFall(void) {
- 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(void) {
- 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(void) {
- 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) {
- IRQ_Dispatch();
-}
-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 = encoder5_val;
- packet->right_drive = encoder4_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->loader_top = !digital(5);
- packet->loader_bottom = !digital(6);
-
- 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 8763181..435d194 100644
--- a/gyro_board/src/usb/analog.h
+++ b/gyro_board/src/usb/analog.h
@@ -1,65 +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;
- uint8_t loader_top : 1;
- uint8_t loader_bottom : 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..edccea3
--- /dev/null
+++ b/gyro_board/src/usb/data_struct.h
@@ -0,0 +1,92 @@
+// 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;
+ uint8_t loader_top : 1;
+ uint8_t loader_bottom : 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)
+
+#ifdef __cplusplus
+// TODO(brians): Consider using C1X's _Static_assert once we have a compiler
+// (GCC 4.6) + flags that support it.
+static_assert(sizeof(DATA_STRUCT_NAME) <= 64,
+ "We only have room for 64 bytes in the USB packet.");
+#endif // defined(__cplusplus)
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..136cefd
--- /dev/null
+++ b/gyro_board/src/usb/encoder.c
@@ -0,0 +1,512 @@
+#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) {
+ SC->EXTPOLAR ^= 0x2;
+ SC->EXTINT = 0x2;
+ int fiopin = GPIO2->FIOPIN;
+ if (((fiopin >> 1) ^ fiopin) & 0x800) {
+ ++encoder1_val;
+ } else {
+ --encoder1_val;
+ }
+}
+// ENC1B 2.12
+void EINT2_IRQHandler(void) {
+ SC->EXTPOLAR ^= 0x4;
+ SC->EXTINT = 0x4;
+ int fiopin = GPIO2->FIOPIN;
+ if (((fiopin >> 1) ^ fiopin) & 0x800) {
+ --encoder1_val;
+ } else {
+ ++encoder1_val;
+ }
+}
+
+// TODO(brians): Have this indicate some kind of error instead of just looping
+// infinitely in the ISR because it never clears it.
+static void NoGPIO(void) {}
+static void Encoder2ARise(void) {
+ GPIOINT->IO0IntClr = (1 << 22);
+ if (GPIO0->FIOPIN & (1 << 21)) {
+ ++encoder2_val;
+ } else {
+ --encoder2_val;
+ }
+}
+static void Encoder2AFall(void) {
+ GPIOINT->IO0IntClr = (1 << 22);
+ if (GPIO0->FIOPIN & (1 << 21)) {
+ --encoder2_val;
+ } else {
+ ++encoder2_val;
+ }
+}
+static void Encoder2BRise(void) {
+ GPIOINT->IO0IntClr = (1 << 21);
+ if (GPIO0->FIOPIN & (1 << 22)) {
+ --encoder2_val;
+ } else {
+ ++encoder2_val;
+ }
+}
+static void Encoder2BFall(void) {
+ GPIOINT->IO0IntClr = (1 << 21);
+ if (GPIO0->FIOPIN & (1 << 22)) {
+ ++encoder2_val;
+ } else {
+ --encoder2_val;
+ }
+}
+
+static void Encoder3ARise(void) {
+ GPIOINT->IO0IntClr = (1 << 20);
+ if (GPIO0->FIOPIN & (1 << 19)) {
+ ++encoder3_val;
+ } else {
+ --encoder3_val;
+ }
+}
+static void Encoder3AFall(void) {
+ GPIOINT->IO0IntClr = (1 << 20);
+ if (GPIO0->FIOPIN & (1 << 19)) {
+ --encoder3_val;
+ } else {
+ ++encoder3_val;
+ }
+}
+static void Encoder3BRise(void) {
+ GPIOINT->IO0IntClr = (1 << 19);
+ if (GPIO0->FIOPIN & (1 << 20)) {
+ --encoder3_val;
+ } else {
+ ++encoder3_val;
+ }
+}
+static void Encoder3BFall(void) {
+ GPIOINT->IO0IntClr = (1 << 19);
+ if (GPIO0->FIOPIN & (1 << 20)) {
+ ++encoder3_val;
+ } else {
+ --encoder3_val;
+ }
+}
+
+static void Encoder4ARise(void) {
+ GPIOINT->IO2IntClr = (1 << 0);
+ if (GPIO2->FIOPIN & (1 << 1)) {
+ ++encoder4_val;
+ } else {
+ --encoder4_val;
+ }
+}
+static void Encoder4AFall(void) {
+ GPIOINT->IO2IntClr = (1 << 0);
+ if (GPIO2->FIOPIN & (1 << 1)) {
+ --encoder4_val;
+ } else {
+ ++encoder4_val;
+ }
+}
+static void Encoder4BRise(void) {
+ GPIOINT->IO2IntClr = (1 << 1);
+ if (GPIO2->FIOPIN & (1 << 0)) {
+ --encoder4_val;
+ } else {
+ ++encoder4_val;
+ }
+}
+static void Encoder4BFall(void) {
+ GPIOINT->IO2IntClr = (1 << 1);
+ if (GPIO2->FIOPIN & (1 << 0)) {
+ ++encoder4_val;
+ } else {
+ --encoder4_val;
+ }
+}
+
+static void Encoder5ARise(void) {
+ GPIOINT->IO2IntClr = (1 << 2);
+ if (GPIO2->FIOPIN & (1 << 3)) {
+ ++encoder5_val;
+ } else {
+ --encoder5_val;
+ }
+}
+static void Encoder5AFall(void) {
+ GPIOINT->IO2IntClr = (1 << 2);
+ if (GPIO2->FIOPIN & (1 << 3)) {
+ --encoder5_val;
+ } else {
+ ++encoder5_val;
+ }
+}
+static void Encoder5BRise(void) {
+ GPIOINT->IO2IntClr = (1 << 3);
+ if (GPIO2->FIOPIN & (1 << 2)) {
+ --encoder5_val;
+ } else {
+ ++encoder5_val;
+ }
+}
+static void Encoder5BFall(void) {
+ 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(void) {
+ 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(void) {
+ 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(void) {
+ 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(USB_IRQn);
+ capture_bottom_fall_delay = encoder3_val;
+ ++bottom_fall_delay_count;
+ NVIC_EnableIRQ(USB_IRQn);
+ } else {
+ NVIC_EnableIRQ(EINT3_IRQn);
+ vTaskDelayUntil(&xSleepFrom, 10 / portTICK_RATE_MS);
+ }
+ }
+}
+
+volatile int8_t bottom_fall_count;
+static void IndexerBottomFall(void) {
+ 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(void) {
+ 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(void) {
+ 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) {
+ IRQ_Dispatch();
+}
+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 = encoder5_val;
+ packet->main.right_drive = encoder4_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.loader_top = !digital(5);
+ packet->main.loader_bottom = !digital(6);
+
+ 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();