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