| Brian Silverman | 8d3816a | 2017-07-03 18:52:15 -0700 | [diff] [blame] | 1 | #include "motors/algorithms.h" |
| 2 | |
| Stephan Pleines | f63bde8 | 2024-01-13 15:59:33 -0800 | [diff] [blame^] | 3 | namespace frc971::motors { |
| Brian Silverman | 8d3816a | 2017-07-03 18:52:15 -0700 | [diff] [blame] | 4 | |
| 5 | BalancedReadings BalanceReadings(const ReadingsToBalance to_balance) { |
| 6 | // TODO(Brian): Get rid of the floating point divides. |
| 7 | BalancedReadings result; |
| 8 | if (to_balance.weights[0] == 0) { |
| 9 | const float average1 = static_cast<float>(to_balance.sums[1]) / |
| 10 | static_cast<float>(to_balance.weights[1]); |
| 11 | const float average2 = static_cast<float>(to_balance.sums[2]) / |
| 12 | static_cast<float>(to_balance.weights[2]); |
| 13 | result.readings[0] = -(average1 + average2); |
| 14 | result.readings[1] = average1; |
| 15 | result.readings[2] = average2; |
| 16 | } else if (to_balance.weights[1] == 0) { |
| 17 | const float average0 = static_cast<float>(to_balance.sums[0]) / |
| 18 | static_cast<float>(to_balance.weights[0]); |
| 19 | const float average2 = static_cast<float>(to_balance.sums[2]) / |
| 20 | static_cast<float>(to_balance.weights[2]); |
| 21 | result.readings[0] = average0; |
| 22 | result.readings[1] = -(average0 + average2); |
| 23 | result.readings[2] = average2; |
| 24 | } else if (to_balance.weights[2] == 0) { |
| 25 | const float average0 = static_cast<float>(to_balance.sums[0]) / |
| 26 | static_cast<float>(to_balance.weights[0]); |
| 27 | const float average1 = static_cast<float>(to_balance.sums[1]) / |
| 28 | static_cast<float>(to_balance.weights[1]); |
| 29 | result.readings[0] = average0; |
| 30 | result.readings[1] = average1; |
| 31 | result.readings[2] = -(average0 + average1); |
| 32 | } else { |
| 33 | const float average0 = static_cast<float>(to_balance.sums[0]) / |
| 34 | static_cast<float>(to_balance.weights[0]); |
| 35 | const float average1 = static_cast<float>(to_balance.sums[1]) / |
| 36 | static_cast<float>(to_balance.weights[1]); |
| 37 | const float average2 = static_cast<float>(to_balance.sums[2]) / |
| 38 | static_cast<float>(to_balance.weights[2]); |
| 39 | const float offset = (average0 + average1 + average2) / -3; |
| 40 | result.readings[0] = average0 + offset; |
| 41 | result.readings[1] = average1 + offset; |
| 42 | result.readings[2] = average2 + offset; |
| 43 | } |
| 44 | |
| 45 | return result; |
| 46 | } |
| 47 | |
| Stephan Pleines | f63bde8 | 2024-01-13 15:59:33 -0800 | [diff] [blame^] | 48 | } // namespace frc971::motors |