Wirtek Robotics Workshop / RWR_Utils

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

RWR_Utils.cpp@0:3960679bf525, 2021-06-27 (annotated)

Committer:
steliansaracut
Date:
Sun Jun 27 12:20:30 2021 +0000
Revision:
0:3960679bf525
Child:
1:755da47160cb
Initial commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
steliansaracut 0:3960679bf525 1 #include "RWR_Utils.hxx"
steliansaracut 0:3960679bf525 2
steliansaracut 0:3960679bf525 3 // TODO: remove this. added for debugging purposes only
steliansaracut 0:3960679bf525 4 extern Serial serial;
steliansaracut 0:3960679bf525 5
steliansaracut 0:3960679bf525 6 Motor::Motor(const PinName &_in1Pin, const PinName &_in2Pin, const bool _brake):
steliansaracut 0:3960679bf525 7 in1PWM(_in1Pin),
steliansaracut 0:3960679bf525 8 in2PWM(_in2Pin),
steliansaracut 0:3960679bf525 9 brake(_brake)
steliansaracut 0:3960679bf525 10 {
steliansaracut 0:3960679bf525 11 in1PWM.period(PWM_PERIOD);
steliansaracut 0:3960679bf525 12 in1PWM.write(brake * 1.0f);
steliansaracut 0:3960679bf525 13 in2PWM.period(PWM_PERIOD);
steliansaracut 0:3960679bf525 14 in2PWM.write(brake * 1.0f);
steliansaracut 0:3960679bf525 15 }
steliansaracut 0:3960679bf525 16
steliansaracut 0:3960679bf525 17 void Motor::setSpeed(const float speed)
steliansaracut 0:3960679bf525 18 {
steliansaracut 0:3960679bf525 19 if (speed >= 0.0f)
steliansaracut 0:3960679bf525 20 {
steliansaracut 0:3960679bf525 21 in2PWM.write(brake * 1.0f);
steliansaracut 0:3960679bf525 22 in1PWM.write(speed);
steliansaracut 0:3960679bf525 23 } else
steliansaracut 0:3960679bf525 24 if (speed < 0.0f)
steliansaracut 0:3960679bf525 25 {
steliansaracut 0:3960679bf525 26 in1PWM.write(brake * 1.0f);
steliansaracut 0:3960679bf525 27 in2PWM.write(-speed);
steliansaracut 0:3960679bf525 28 }
steliansaracut 0:3960679bf525 29 }
steliansaracut 0:3960679bf525 30
steliansaracut 0:3960679bf525 31 void Motor::setBrake(const bool _brake)
steliansaracut 0:3960679bf525 32 {
steliansaracut 0:3960679bf525 33 in1PWM.write(_brake * 1.0f);
steliansaracut 0:3960679bf525 34 in2PWM.write(_brake * 1.0f);
steliansaracut 0:3960679bf525 35 }
steliansaracut 0:3960679bf525 36
steliansaracut 0:3960679bf525 37 QTR::QTR(DigitalOut &_ctl, AnalogIn (&_sensors)[SENSORS_BAR_SIZE], const uint32_t dimPulses):
steliansaracut 0:3960679bf525 38 ctl(_ctl),
steliansaracut 0:3960679bf525 39 sensors(_sensors),
steliansaracut 0:3960679bf525 40 lastSensorState(0)
steliansaracut 0:3960679bf525 41 {
steliansaracut 0:3960679bf525 42 dim(dimPulses);
steliansaracut 0:3960679bf525 43 ctl = 1;
steliansaracut 0:3960679bf525 44 }
steliansaracut 0:3960679bf525 45
steliansaracut 0:3960679bf525 46 void QTR::dim(const uint32_t pulses)
steliansaracut 0:3960679bf525 47 {
steliansaracut 0:3960679bf525 48 // reset QTR dim
steliansaracut 0:3960679bf525 49 ctl = 0;
steliansaracut 0:3960679bf525 50 wait_ms(1.5f);
steliansaracut 0:3960679bf525 51 ctl = 1;
steliansaracut 0:3960679bf525 52 wait_us(1);
steliansaracut 0:3960679bf525 53
steliansaracut 0:3960679bf525 54 // apply pulses on CTL to dim sensors
steliansaracut 0:3960679bf525 55 for (uint32_t i = 0; i < pulses; i++)
steliansaracut 0:3960679bf525 56 {
steliansaracut 0:3960679bf525 57 ctl = 0;
steliansaracut 0:3960679bf525 58 wait_us(1);
steliansaracut 0:3960679bf525 59 ctl = 1;
steliansaracut 0:3960679bf525 60 wait_us(1);
steliansaracut 0:3960679bf525 61 }
steliansaracut 0:3960679bf525 62 }
steliansaracut 0:3960679bf525 63
steliansaracut 0:3960679bf525 64 uint32_t QTR::readSensorsAsDigital()
steliansaracut 0:3960679bf525 65 {
steliansaracut 0:3960679bf525 66 uint32_t currentSensorState = lastSensorState;
steliansaracut 0:3960679bf525 67
steliansaracut 0:3960679bf525 68 // for each sensor
steliansaracut 0:3960679bf525 69 for (uint32_t i = 0; i < SENSORS_BAR_SIZE; i++)
steliansaracut 0:3960679bf525 70 {
steliansaracut 0:3960679bf525 71 uint32_t mask = (1U << i);
steliansaracut 0:3960679bf525 72
steliansaracut 0:3960679bf525 73 // if last sensor state was 'true'
steliansaracut 0:3960679bf525 74 if (lastSensorState & mask)
steliansaracut 0:3960679bf525 75 {
steliansaracut 0:3960679bf525 76 // if current analog value is below low threshold
steliansaracut 0:3960679bf525 77 if ((uint32_t)sensors[i].read_u16() < QTR_THRESHOLD_LOW)
steliansaracut 0:3960679bf525 78 {
steliansaracut 0:3960679bf525 79 // reset bit
steliansaracut 0:3960679bf525 80 currentSensorState &= ~mask;
steliansaracut 0:3960679bf525 81 }
steliansaracut 0:3960679bf525 82 } else // if last sensor state was 'false'
steliansaracut 0:3960679bf525 83 {
steliansaracut 0:3960679bf525 84 // if current analog value is above high threshold
steliansaracut 0:3960679bf525 85 if ((uint32_t)sensors[i].read_u16() > QTR_THRESHOLD_HIGH)
steliansaracut 0:3960679bf525 86 {
steliansaracut 0:3960679bf525 87 // set bit
steliansaracut 0:3960679bf525 88 currentSensorState |= mask;
steliansaracut 0:3960679bf525 89 }
steliansaracut 0:3960679bf525 90 }
steliansaracut 0:3960679bf525 91 }
steliansaracut 0:3960679bf525 92
steliansaracut 0:3960679bf525 93 lastSensorState = currentSensorState;
steliansaracut 0:3960679bf525 94 return currentSensorState;
steliansaracut 0:3960679bf525 95 }
steliansaracut 0:3960679bf525 96
steliansaracut 0:3960679bf525 97 float QTR::readSensorsAsFloat()
steliansaracut 0:3960679bf525 98 {
steliansaracut 0:3960679bf525 99 float returnValue = 0.0f;
steliansaracut 0:3960679bf525 100 uint32_t sensorValue = readSensorsAsDigital();
steliansaracut 0:3960679bf525 101
steliansaracut 0:3960679bf525 102 // count leading zeros
steliansaracut 0:3960679bf525 103 uint32_t clz = __CLZ(sensorValue);
steliansaracut 0:3960679bf525 104 // reverse bits
steliansaracut 0:3960679bf525 105 uint32_t ctz = __RBIT(sensorValue);
steliansaracut 0:3960679bf525 106 // count trailing zeros
steliansaracut 0:3960679bf525 107 ctz = __CLZ(ctz);
steliansaracut 0:3960679bf525 108
steliansaracut 0:3960679bf525 109 // convert CLZ and CTZ to intermediate line position
steliansaracut 0:3960679bf525 110 int intermediateResult = (32 - (int32_t)clz - SENSORS_BAR_SIZE + (int32_t)ctz);
steliansaracut 0:3960679bf525 111 // normalize line position and multiply by 1000 to allow integer comparison
steliansaracut 0:3960679bf525 112 intermediateResult = intermediateResult * 1000 / (SENSORS_BAR_SIZE - 1);
steliansaracut 0:3960679bf525 113
steliansaracut 0:3960679bf525 114 // replace out of range readings with 0
steliansaracut 0:3960679bf525 115 if ((intermediateResult < -1000) || (intermediateResult > 1000))
steliansaracut 0:3960679bf525 116 {
steliansaracut 0:3960679bf525 117 returnValue = 0.0f;
steliansaracut 0:3960679bf525 118 } else
steliansaracut 0:3960679bf525 119 {
steliansaracut 0:3960679bf525 120 returnValue = intermediateResult / 1000.0f;
steliansaracut 0:3960679bf525 121 }
steliansaracut 0:3960679bf525 122
steliansaracut 0:3960679bf525 123 // return line position as a float between -1.0 (left) and 1.0 (right)
steliansaracut 0:3960679bf525 124 return returnValue;
steliansaracut 0:3960679bf525 125 }