gyro_board/src/usb/encoder.c

#include "fill_packet.h"
#include "encoder.h"

#include "FreeRTOS.h"
#include "task.h"

#include "digital.h"
#include "analog.h"
#include "gyro.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;

// The timer to use for timestamping sensor readings.
// This is a constant to avoid hard-coding it in a lot of places, but there ARE
// things (PCONP bits, IRQ numbers, etc) that have this value in them
// implicitly.
#define SENSOR_TIMING_TIMER TIM1
// How many counts per second SENSOR_TIMING_TIMER should be.
// This will wrap the counter about every 1/3 of a second.
static const int kSensorTimingRate = 100000;

#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;

// It is important to clear the various interrupt flags first thing in the ISRs.
// It doesn't seem to work otherwise, possibly because of the reason that Brian
// found poking around online: caches on the bus make it so that the clearing of
// the interrupt gets to the NVIC after the ISR returns, so it runs the ISR a
// second time. Also, by clearing them early, if a second interrupt arrives from
// the same source it will still get handled instead of getting lost.

// ENC1A 2.11
void EINT1_IRQHandler(void) {
  // Make sure to change this BEFORE clearing the interrupt like the datasheet
  // says you have to.
  SC->EXTPOLAR ^= 0x2;
  SC->EXTINT = 0x2;
  int fiopin = GPIO2->FIOPIN;
  // This looks like a weird way to XOR the 2 inputs, but it compiles down to
  // just 2 instructions, which is hard to beat.
  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;
portTickType xDelayTimeFrom;
static portTASK_FUNCTION(vDelayCapture, pvParameters)
{
  portTickType xSleepFrom = xTaskGetTickCount();

  for (;;) {
    // Atomically (wrt the ISR) switch xDelayTimeFrom to 0 and store its old
    // value to use later.
    NVIC_DisableIRQ(EINT3_IRQn);
    portTickType new_time = xDelayTimeFrom;
    xDelayTimeFrom = 0;
    NVIC_EnableIRQ(EINT3_IRQn);

    if (new_time != 0) {
      xSleepFrom = new_time;

      vTaskDelayUntil(&xSleepFrom, kBottomFallDelayTime / portTICK_RATE_MS);

      // Make sure that the USB ISR doesn't look at inconsistent values.
      NVIC_DisableIRQ(USB_IRQn);
      capture_bottom_fall_delay = encoder3_val;
      ++bottom_fall_delay_count;
      NVIC_EnableIRQ(USB_IRQn);
    } else {
      // Wait 10ms and then check again.
      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();
}
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) {
  // There is no need to add a loop here to handle multiple interrupts at the
  // same time because the processor has tail chaining of interrupts which we
  // can't really beat with our own loop.
  // It would actually be bad because a loop here would block EINT1/2 for longer
  // lengths of time.

  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;
  }
}

static volatile uint32_t sensor_timing_wraps = 0;

void TIMER1_IRQHandler(void) {
  SENSOR_TIMING_TIMER->IR = 1 << 0;  // clear channel 0 match
  ++sensor_timing_wraps;
}

void encoder_init(void) {
  // Set up the timer for timestamping sensor readings.
  SC->PCONP |= 1 << 2;
  SENSOR_TIMING_TIMER->PR = (configCPU_CLOCK_HZ / kSensorTimingRate) - 1UL;
  SENSOR_TIMING_TIMER->TC = 1;  // don't match the first time around
  SENSOR_TIMING_TIMER->MR0 = 0;  // match every time it wraps
  SENSOR_TIMING_TIMER->MCR = 1 << 0;  // interrupt on match channel 0
  // Priority 4 is higher than any FreeRTOS-managed stuff (ie USB), but lower
  // than encoders etc.
  NVIC_SetPriority(TIMER1_IRQn, 4);
  NVIC_EnableIRQ(TIMER1_IRQn);
  SENSOR_TIMING_TIMER->TCR = 1;  // enable it

  // 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) {
  if (gyro_output.initialized) {
    packet->gyro_angle = gyro_output.angle;
    packet->old_gyro_reading = gyro_output.last_reading_bad;
    packet->bad_gyro = gyro_output.gyro_bad;
  } else {
    packet->gyro_angle = 0;
    packet->old_gyro_reading = 1;
    packet->bad_gyro = 0;
  }

  NVIC_DisableIRQ(TIMER1_IRQn);
  packet->timestamp = ((uint64_t)sensor_timing_wraps << 32) | TIM1->TC;
  NVIC_EnableIRQ(TIMER1_IRQn);

  packet->checksum = DATA_STRUCT_CHECKSUM;

  packet->dip_switch0 = dip_switch(0);
  packet->dip_switch1 = dip_switch(1);
  packet->dip_switch2 = dip_switch(2);
  packet->dip_switch3 = dip_switch(3);

  // We disable EINT3 to avoid sending back inconsistent values. All of the
  // aligned reads from the variables are atomic, so disabling it isn't
  // necessary for just reading encoder values. We re-enable it periodically
  // because disabling and enabling is cheap (2 instructions) and we really rely
  // on low interrupt latencies.

  if (is_bot3) {
    packet->robot_id = 1;
  } else {  // is main robot
    packet->robot_id = 2;

    packet->main.shooter = encoder1_val;
    packet->main.left_drive = encoder5_val;
    packet->main.right_drive = encoder4_val;
    packet->main.indexer = encoder3_val;
    packet->main.battery_voltage = analog(3);
    packet->main.left_drive_hall = analog(1);
    packet->main.right_drive_hall = analog(2);

    NVIC_DisableIRQ(EINT3_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(EINT3_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);

    NVIC_EnableIRQ(EINT3_IRQn);
    NVIC_DisableIRQ(EINT3_IRQn);

    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);

    NVIC_EnableIRQ(EINT3_IRQn);
    NVIC_DisableIRQ(EINT3_IRQn);

    packet->main.loader_top = !digital(5);
    packet->main.loader_bottom = !digital(6);

    NVIC_EnableIRQ(EINT3_IRQn);
    NVIC_DisableIRQ(EINT3_IRQn);

    packet->main.shooter_angle = encoder2_val;
    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;
  }
}