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