Diff: RWR_Utils.cpp

Revision:

1:755da47160cb

Parent:

0:3960679bf525

Child:

5:6cd52beadb7c

--- a/RWR_Utils.cpp	Sun Jun 27 12:20:30 2021 +0000
+++ b/RWR_Utils.cpp	Wed Jul 14 08:49:04 2021 +0300
@@ -1,125 +1,133 @@
-#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;
-}
+#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()
+{
+  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;
+}