| Line | Branch | Decision | Exec | Source |
|---|---|---|---|---|
| 1 | /** | |||
| 2 | * @file DcMotor.cpp | |||
| 3 | * @brief DC motor controller | |||
| 4 | * | |||
| 5 | * @date Dec 22, 2018 | |||
| 6 | * @author Matthew Kennedy | |||
| 7 | */ | |||
| 8 | ||||
| 9 | #include "pch.h" | |||
| 10 | ||||
| 11 | #include "dc_motor.h" | |||
| 12 | ||||
| 13 | 12 | TwoPinDcMotor::TwoPinDcMotor(OutputPin& disablePin) | ||
| 14 | 12 | : m_disable(&disablePin) | ||
| 15 | { | |||
| 16 | 12 | disable("init"); | ||
| 17 | 12 | } | ||
| 18 | ||||
| 19 | 6 | void TwoPinDcMotor::configure(IPwm& enable, IPwm& dir1, IPwm& dir2, bool isInverted) { | ||
| 20 | 6 | m_enable = &enable; | ||
| 21 | 6 | m_dir1 = &dir1; | ||
| 22 | 6 | m_dir2 = &dir2; | ||
| 23 | 6 | m_isInverted = isInverted; | ||
| 24 | 6 | } | ||
| 25 | ||||
| 26 | 155 | void TwoPinDcMotor::enable() { | ||
| 27 |
1/2✓ Branch 0 taken 155 times.
✗ Branch 1 not taken.
|
1/2✓ Decision 'true' taken 155 times.
✗ Decision 'false' not taken.
|
155 | if (m_disable) { |
| 28 | 155 | m_disable->setValue(false); | ||
| 29 | } | |||
| 30 | ||||
| 31 | 155 | m_msg = nullptr; | ||
| 32 | 155 | } | ||
| 33 | ||||
| 34 | 65 | void TwoPinDcMotor::disable(const char *msg) { | ||
| 35 | 65 | m_msg = msg; | ||
| 36 |
1/2✓ Branch 0 taken 65 times.
✗ Branch 1 not taken.
|
1/2✓ Decision 'true' taken 65 times.
✗ Decision 'false' not taken.
|
65 | if (m_disable) { |
| 37 | 65 | m_disable->setValue(true); | ||
| 38 | } | |||
| 39 | ||||
| 40 | // Also set the duty to zero | |||
| 41 | 65 | set(0); | ||
| 42 | 65 | } | ||
| 43 | ||||
| 44 | ✗ | bool TwoPinDcMotor::isOpenDirection() const { | ||
| 45 | ✗ | return m_value >= 0; | ||
| 46 | } | |||
| 47 | ||||
| 48 | 522956 | float TwoPinDcMotor::get() const { | ||
| 49 | 522956 | return m_value; | ||
| 50 | } | |||
| 51 | ||||
| 52 | /** | |||
| 53 | * @param duty value between -1.0 and 1.0 | |||
| 54 | */ | |||
| 55 | 224 | bool TwoPinDcMotor::set(float duty) | ||
| 56 | { | |||
| 57 | 224 | m_value = duty; | ||
| 58 | ||||
| 59 | // For low voltage, voltageRatio will be >1 to boost duty so that motor current stays the same | |||
| 60 | // At high voltage, the inverse will be true to keep behavior always the same. | |||
| 61 |
1/1✓ Branch 2 taken 224 times.
|
224 | float voltageRatio = 14 / Sensor::get(SensorType::BatteryVoltage).value_or(14); | |
| 62 | 224 | duty *= voltageRatio; | ||
| 63 | ||||
| 64 | // If not init, don't try to set | |||
| 65 |
4/6✓ Branch 0 taken 210 times.
✓ Branch 1 taken 14 times.
✓ Branch 2 taken 210 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 210 times.
|
2/2✓ Decision 'true' taken 14 times.
✓ Decision 'false' taken 210 times.
|
224 | if (!m_dir1 || !m_dir2 || !m_enable) { |
| 66 |
1/2✓ Branch 0 taken 14 times.
✗ Branch 1 not taken.
|
1/2✓ Decision 'true' taken 14 times.
✗ Decision 'false' not taken.
|
14 | if (m_disable) { |
| 67 | 14 | m_disable->setValue(true); | ||
| 68 | } | |||
| 69 | ||||
| 70 | 14 | return false; | ||
| 71 | } | |||
| 72 | ||||
| 73 | 210 | bool isPositive = duty > 0; | ||
| 74 | ||||
| 75 |
2/2✓ Branch 0 taken 156 times.
✓ Branch 1 taken 54 times.
|
2/2✓ Decision 'true' taken 156 times.
✓ Decision 'false' taken 54 times.
|
210 | if (!isPositive) { |
| 76 | 156 | duty = -duty; | ||
| 77 | } | |||
| 78 | ||||
| 79 | // below here 'duty' is a not negative | |||
| 80 | ||||
| 81 | // Clamp to 100% | |||
| 82 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 210 times.
|
1/2✗ Decision 'true' not taken.
✓ Decision 'false' taken 210 times.
|
210 | if (duty > 1.0f) { |
| 83 | ✗ | duty = 1.0f; | ||
| 84 | } | |||
| 85 | // Disable for very small duty | |||
| 86 |
2/2✓ Branch 0 taken 52 times.
✓ Branch 1 taken 158 times.
|
2/2✓ Decision 'true' taken 52 times.
✓ Decision 'false' taken 158 times.
|
210 | else if (duty < 0.01f) |
| 87 | { | |||
| 88 | 52 | duty = 0.0f; | ||
| 89 | } | |||
| 90 | ||||
| 91 | // If we're in two pin mode, force 100%, else use this pin to PWM | |||
| 92 |
2/2✓ Branch 0 taken 208 times.
✓ Branch 1 taken 2 times.
|
210 | float enableDuty = m_type == ControlType::PwmEnablePin ? duty : 1; | |
| 93 | ||||
| 94 | // Direction pins get 100% duty unless we're in PwmDirectionPins mode | |||
| 95 |
2/2✓ Branch 0 taken 2 times.
✓ Branch 1 taken 208 times.
|
210 | float dirDuty = m_type == ControlType::PwmDirectionPins ? duty : 1; | |
| 96 | ||||
| 97 | 210 | m_enable->setSimplePwmDutyCycle(enableDuty); | ||
| 98 | 210 | float recipDuty = 0; | ||
| 99 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 210 times.
|
1/2✗ Decision 'true' not taken.
✓ Decision 'false' taken 210 times.
|
210 | if (m_isInverted) { |
| 100 | ✗ | dirDuty = 1.0f - dirDuty; | ||
| 101 | ✗ | recipDuty = 1.0f; | ||
| 102 | } | |||
| 103 | ||||
| 104 |
2/2✓ Branch 0 taken 54 times.
✓ Branch 1 taken 156 times.
|
210 | m_dir1->setSimplePwmDutyCycle(isPositive ? dirDuty : recipDuty); | |
| 105 |
2/2✓ Branch 0 taken 54 times.
✓ Branch 1 taken 156 times.
|
210 | m_dir2->setSimplePwmDutyCycle(isPositive ? recipDuty : dirDuty); | |
| 106 | ||||
| 107 | // This motor has no fault detection, so always return false (indicate success). | |||
| 108 | 210 | return false; | ||
| 109 | } | |||
| 110 |