allwpilib/wpilibc/src/main/native/cpp/hardware/expansionhub/ExpansionHubVelocityConstants.cpp at cfd9d49629b732f6d152fad5a5ba15785033fc5e · wpilibsuite/allwpilib · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

#include "wpi/hardware/expansionhub/ExpansionHubVelocityConstants.hpp"

#include "fmt/core.h"
#include "wpi/system/SystemServer.hpp"

using namespace wpi;

ExpansionHubVelocityConstants::ExpansionHubVelocityConstants(int hubNumber,
                                                             int motorNumber) {
  auto systemServer = SystemServer::GetSystemServer();

  wpi::nt::PubSubOptions options;
  options.sendAll = true;
  options.keepDuplicates = true;
  options.periodic = 0.005;

  m_pPublisher =
      systemServer
          .GetDoubleTopic(fmt::format("/rhsp/{}/motor{}/constants/velocity/kp",
                                      hubNumber, motorNumber))
          .Publish(options);

  m_iPublisher =
      systemServer
          .GetDoubleTopic(fmt::format("/rhsp/{}/motor{}/constants/velocity/ki",
                                      hubNumber, motorNumber))
          .Publish(options);

  m_dPublisher =
      systemServer
          .GetDoubleTopic(fmt::format("/rhsp/{}/motor{}/constants/velocity/kd",
                                      hubNumber, motorNumber))
          .Publish(options);
    m_pPublisher.SetDefault(1.0);
  m_iPublisher.SetDefault(0.0);
  m_dPublisher.SetDefault(0.01);

  m_sPublisher =
      systemServer
          .GetDoubleTopic(fmt::format("/rhsp/{}/motor{}/constants/velocity/ks",
                                      hubNumber, motorNumber))
          .Publish(options);

  m_vPublisher =
      systemServer
          .GetDoubleTopic(fmt::format("/rhsp/{}/motor{}/constants/velocity/kv",
                                      hubNumber, motorNumber))
          .Publish(options);

  m_aPublisher =
      systemServer
          .GetDoubleTopic(fmt::format("/rhsp/{}/motor{}/constants/velocity/ka",
                                      hubNumber, motorNumber))
          .Publish(options);
}

ExpansionHubVelocityConstants& ExpansionHubVelocityConstants::SetPID(double p,
                                                                     double i,
                                                                     double d) {
  m_pPublisher.Set(p);
  m_iPublisher.Set(i);
  m_dPublisher.Set(d);
  return *this;
}

ExpansionHubVelocityConstants& ExpansionHubVelocityConstants::SetFF(double s,
                                                                    double v,
                                                                    double a) {
  m_sPublisher.Set(s);
  m_vPublisher.Set(v);
  m_aPublisher.Set(a);
  return *this;
}