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@1:755da47160cb, 2021-07-14 (annotated)

Committer:
steliansaracut
Date:
Wed Jul 14 08:49:04 2021 +0300
Revision:
1:755da47160cb
Parent:
0:3960679bf525
Child:
5:6cd52beadb7c
fixed bugs in web server state machine. work in progress.

Who changed what in which revision?

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