Diff: RWR_Utils.cpp

Revision:

0:3960679bf525

Child:

1:755da47160cb

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/RWR_Utils.cpp	Sun Jun 27 12:20:30 2021 +0000
@@ -0,0 +1,125 @@
+#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;
+}
+
+float QTR::readSensorsAsFloat()
+{
+  float returnValue = 0.0f;
+  uint32_t sensorValue = readSensorsAsDigital();
+
+  // count leading zeros
+  uint32_t clz = __CLZ(sensorValue);
+  // reverse bits
+  uint32_t ctz = __RBIT(sensorValue);
+  // 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;
+}