split analog.c out intelligently and added an id to the sensor packet
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;
+ }
+}