File content as of revision 8:7433f35f0338:

#include "RWR_Utils.hxx"

// TODO: remove this. added for debugging purposes only
extern Serial serial;

Motor::Motor(const PinName &_in1Pin, const PinName &_in2Pin, const bool _brake):
  in1PWM(_in1Pin),
  in2PWM(_in2Pin),
  brake(_brake)
{
  in1PWM.period(PWM_PERIOD);
  in1PWM.write(brake * 1.0f);
  in2PWM.period(PWM_PERIOD);
  in2PWM.write(brake * 1.0f);
}

void Motor::setSpeed(const float speed)
{
  if (speed >= 0.0f)
  {
    in2PWM.write(brake * 1.0f);
    in1PWM.write(speed);
  } else
  if (speed < 0.0f)
  {
    in1PWM.write(brake * 1.0f);
    in2PWM.write(-speed);
  }
}

void Motor::setBrake(const bool _brake)
{
  in1PWM.write(_brake * 1.0f);
  in2PWM.write(_brake * 1.0f);
}

QTR::QTR(DigitalOut &_ctl, AnalogIn (&_sensors)[SENSORS_BAR_SIZE], const uint32_t dimPulses):
  ctl(_ctl),
  sensors(_sensors),
  lastSensorState(0)
{
  dim(dimPulses);
  ctl = 1;
}

void QTR::dim(const uint32_t pulses)
{
  // reset QTR dim
  ctl = 0;
  wait_ms(1.5f);
  ctl = 1;
  wait_us(1);

  // apply pulses on CTL to dim sensors
  for (uint32_t i = 0; i < pulses; i++)
  {
    ctl = 0;
    wait_us(1);
    ctl = 1;
    wait_us(1);
  }
}

uint32_t QTR::readSensorsAsDigital()
{
  uint32_t currentSensorState = lastSensorState;

  // for each sensor
  for (uint32_t i = 0; i < SENSORS_BAR_SIZE; i++)
  {
    uint32_t mask = (1U << i);

    // if last sensor state was 'true'
    if (lastSensorState & mask)
    {
      // if current analog value is below low threshold
      if ((uint32_t)sensors[i].read_u16() < QTR_THRESHOLD_LOW)
      {
        // reset bit
        currentSensorState &= ~mask;
      }
    } else // if last sensor state was 'false'
    {
      // if current analog value is above high threshold
      if ((uint32_t)sensors[i].read_u16() > QTR_THRESHOLD_HIGH)
      {
        // set bit
        currentSensorState |= mask;
      }
    }
  }

  lastSensorState = currentSensorState;
  return currentSensorState;
}

void QTR::readSensorsAsAnalog(uint32_t *buffer)
{
  for (uint32_t i = 0; i < SENSORS_BAR_SIZE; i++)
  {
    buffer[i] = (uint32_t)sensors[i].read_u16();
  }
}

float QTR::readSensorsAsFloat()
{
  uint32_t sensorValue = readSensorsAsDigital();
  float returnValue = digitizedSensorsToFloat(sensorValue);

  // return line position as a float between -1.0 (left) and 1.0 (right)
  return returnValue;
}

float QTR::digitizedSensorsToFloat(uint32_t digitizedSensors)
{
  float returnValue = 0.0f;

  // count leading zeros
  uint32_t clz = __CLZ(digitizedSensors);
  // reverse bits
  uint32_t ctz = __RBIT(digitizedSensors);
  // count trailing zeros
  ctz = __CLZ(ctz);

  // convert CLZ and CTZ to intermediate line position
  int intermediateResult = (32 - (int32_t)clz - SENSORS_BAR_SIZE + (int32_t)ctz);
  // normalize line position and multiply by 1000 to allow integer comparison
  intermediateResult = intermediateResult * 1000 / (SENSORS_BAR_SIZE - 1);

  // replace out of range readings with 0
  if ((intermediateResult < -1000) || (intermediateResult > 1000))
  {
    returnValue = 0.0f;
  } else
  {
    returnValue = intermediateResult / 1000.0f;
  }

  // return line position as a float between -1.0 (left) and 1.0 (right)
  return returnValue;
}