| Brian Silverman | 890a32a | 2018-03-11 15:41:56 -0700 | [diff] [blame] | 1 | #ifndef CTR_EXCLUDE_WPILIB_CLASSES |
| 2 | |
| 3 | #include "ctre/phoenix/RCRadio3Ch.h" |
| 4 | #include <vector> |
| 5 | |
| 6 | namespace ctre { |
| 7 | namespace phoenix { |
| 8 | |
| 9 | RCRadio3Ch::RCRadio3Ch(ctre::phoenix::CANifier *canifier) { |
| 10 | _canifier = canifier; |
| 11 | } |
| 12 | |
| 13 | float RCRadio3Ch::GetDutyCycleUs(Channel channel) { |
| 14 | return _dutyCycleAndPeriods[(int) channel][0]; |
| 15 | } |
| 16 | |
| 17 | float RCRadio3Ch::GetDutyCyclePerc(Channel channel) { |
| 18 | float retval = RCRadio3Ch::GetDutyCycleUs(channel); |
| 19 | |
| 20 | std::vector<double> xData = { 1000, 2000 }; |
| 21 | std::vector<double> yData = { -1, 1 }; |
| 22 | |
| 23 | retval = RCRadio3Ch::Interpolate(xData, yData, retval, true); |
| 24 | |
| 25 | if (retval < -1) { |
| 26 | retval = -1; |
| 27 | } else if (retval > +1) { |
| 28 | retval = +1; |
| 29 | } |
| 30 | |
| 31 | return retval; |
| 32 | } |
| 33 | |
| 34 | bool RCRadio3Ch::GetSwitchValue(Channel channel) { |
| 35 | float retval = RCRadio3Ch::GetDutyCyclePerc(channel); |
| 36 | |
| 37 | return retval > 0.5f; |
| 38 | } |
| 39 | |
| 40 | float RCRadio3Ch::GetPeriodUs(Channel channel) { |
| 41 | return _dutyCycleAndPeriods[(int) channel][1]; |
| 42 | } |
| 43 | |
| 44 | void RCRadio3Ch::Process() { |
| 45 | //Does some error code stuff, which we don't have... |
| 46 | _errorCodes[0] = _canifier->GetPWMInput( |
| 47 | ctre::phoenix::CANifier::PWMChannel::PWMChannel0, _dutyCycleAndPeriods[0]); |
| 48 | _errorCodes[1] = _canifier->GetPWMInput( |
| 49 | ctre::phoenix::CANifier::PWMChannel::PWMChannel1, _dutyCycleAndPeriods[1]); |
| 50 | _errorCodes[2] = _canifier->GetPWMInput( |
| 51 | ctre::phoenix::CANifier::PWMChannel::PWMChannel2, _dutyCycleAndPeriods[2]); |
| 52 | _errorCodes[3] = _canifier->GetPWMInput( |
| 53 | ctre::phoenix::CANifier::PWMChannel::PWMChannel3, _dutyCycleAndPeriods[3]); |
| 54 | |
| 55 | Status health = Status::Okay; |
| 56 | if (health == Status::Okay) { |
| 57 | if (_errorCodes[0] < 0) { |
| 58 | health = Status::LossOfCAN; |
| 59 | } |
| 60 | if (_errorCodes[1] < 0) { |
| 61 | health = Status::LossOfCAN; |
| 62 | } |
| 63 | if (_errorCodes[2] < 0) { |
| 64 | health = Status::LossOfCAN; |
| 65 | } |
| 66 | if (_errorCodes[3] < 0) { |
| 67 | health = Status::LossOfCAN; |
| 68 | } |
| 69 | } |
| 70 | |
| 71 | if (health == Status::Okay) { |
| 72 | if (RCRadio3Ch::GetPeriodUs(RCRadio3Ch::Channel1) == 0) { |
| 73 | health = Status::LossOfPwm; |
| 74 | } |
| 75 | if (RCRadio3Ch::GetPeriodUs(RCRadio3Ch::Channel2) == 0) { |
| 76 | health = Status::LossOfPwm; |
| 77 | } |
| 78 | if (RCRadio3Ch::GetPeriodUs(RCRadio3Ch::Channel3) == 0) { |
| 79 | health = Status::LossOfPwm; |
| 80 | } |
| 81 | } |
| 82 | CurrentStatus = health; //Will have to change this to a getter and a setter |
| 83 | } |
| 84 | |
| 85 | double RCRadio3Ch::Interpolate(std::vector<double> &xData, |
| 86 | std::vector<double> &yData, double x, bool extrapolate) { |
| 87 | int size = xData.size(); |
| 88 | |
| 89 | int i = 0; // find left end of interval for interpolation |
| 90 | if (x >= xData[size - 2]) // special case: beyond right end |
| 91 | { |
| 92 | i = size - 2; |
| 93 | } else { |
| 94 | while (x > xData[i + 1]) |
| 95 | i++; |
| 96 | } |
| 97 | double xL = xData[i], yL = yData[i], xR = xData[i + 1], yR = yData[i + 1]; // points on either side (unless beyond ends) |
| 98 | if (!extrapolate) // if beyond ends of array and not extrapolating |
| 99 | { |
| 100 | if (x < xL) |
| 101 | yR = yL; |
| 102 | if (x > xR) |
| 103 | yL = yR; |
| 104 | } |
| 105 | |
| 106 | double dydx = (yR - yL) / (xR - xL); // gradient |
| 107 | |
| 108 | return yL + dydx * (x - xL); |
| 109 | } |
| 110 | |
| 111 | } |
| 112 | } |
| 113 | #endif |