Tetrapod Project
step_response_test.cpp
Go to the documentation of this file.
1 #include "ros/ros.h"
2 #include <ros/console.h>
3 
5 
7 
8 int main(int argc, char **argv)
9 {
10  enum ControlType {position, velocity, torque};
11 
12  // Configuration Start
13 
14  // Set Gains
15  double K_P_POS = 10.0;
16  double K_I_POS = 10.0;
17 
18  double K_P_VEL = 50.0;
19  double K_I_VEL = 50.0;
20 
21  double K_P_TOR = 100.0;
22  double K_I_TOR = 100.0;
23 
24  // Choose test mode
25  ControlType TEST_MODE = ControlType::position;
26  //ControlType TEST_MODE = ControlType::velocity;
27  //ControlType TEST_MODE = ControlType::torque;
28 
29  // Only used if the control type is position control
30  double SETPOINT_POSITION = math_utils::degToRad(180); // [rad]
31 
32  // Only used if the control type is velocity control
33  double SETPOINT_VELOCITY = math_utils::degToRad(360); // [rad/s]
34 
35  // Only used if the control type is torque control
36  double SETPOINT_TORQUE = 10; // [Nm]
37 
38  // Rate of which control messages are being sent
39  double PUBLISH_FREQUENCY = 200.0; // [1/s]
40 
41  // Time of the step reference signal
42  double STEP_START_TIME = 0.2; // [s]
43 
44  // Duration the step reference signal is high
45  double STEP_DURATION = 1.0; // [s]
46 
47 
48  // Configuration End
49 
50  // Create single motor controller instance
51  SingleMotorController controller(PUBLISH_FREQUENCY);
52 
53  // Setup the ROS communication
54  controller.initROS();
55 
56  // Set the PID Parameters of the motors
58 
59  // Set the rate for which we check for incomming messages
60  ros::Rate check_for_messages_rate(1);
61 
62  // Set the command rate
63  ros::Rate send_command_rate(PUBLISH_FREQUENCY);
64 
65  // Make sure we have received initial states from the motor
66  while(!controller.initialStateReceived())
67  {
68  controller.checkForNewMessages();
69  ROS_INFO("Waiting for state");
70  controller.printAll();
71  check_for_messages_rate.sleep();
72  }
73 
74  // Wait for the start signal
75  while(!controller.readyToProceed())
76  {
77  controller.checkForNewMessages();
78  ROS_INFO("Waiting for ready to proceed");
79  controller.printAll();
80  check_for_messages_rate.sleep();
81  }
82 
83  // If we are using position control we want the position to start at zero
84  while(abs(controller.getPosition()) > math_utils::degToRad(0.5))
85  {
86  controller.checkForNewMessages();
87  ROS_INFO("Trying to move position to zero");
88  controller.setPositionDirectly(0.0);
89  controller.printAll();
90  send_command_rate.sleep();
91  }
92 
93  // Let some time pass before making the step response
94  for(int i = 0; i < STEP_START_TIME*PUBLISH_FREQUENCY; i++)
95  {
96  controller.checkForNewMessages();
97  controller.writeToLog();
98  controller.printAll();
99  send_command_rate.sleep();
100  }
101 
102  // Apply the desired step torque for a fixed duration and log the states
103  for(int i = 0; i < STEP_DURATION*PUBLISH_FREQUENCY; i++)
104  {
105  controller.checkForNewMessages();
106 
107  switch (TEST_MODE)
108  {
110  {
111  controller.setPositionDirectly(SETPOINT_POSITION);
112  break;
113  }
115  {
116  controller.setVelocityDirectly(SETPOINT_VELOCITY);
117  break;
118  }
119  case ControlType::torque:
120  {
121  controller.setTorqueDirectly(SETPOINT_TORQUE);
122  break;
123  }
124  default:
125  {
126  break;
127  }
128  }
129 
130  controller.writeToLog();
131  controller.printAll();
132  send_command_rate.sleep();
133  }
134 
135  // Apply zero torque until the program shuts down
136  /*
137  for (int i = 0; i < 100; i++)
138  {
139  controller.checkForNewMessages();
140  controller.setVelocityDirectly(0.0);
141  controller.printAll();
142  send_command_rate.sleep();
143  }
144  */
145 
146  return 0;
147 }
148 
void setPositionDirectly(double _joint_pos)
void setTorqueDirectly(double _torque)
void setVelocityDirectly(double _joint_vel)
void initializeMotor(double _k_p_pos, double _k_d_pos, double _k_p_vel, double _k_i_vel, double _k_p_torque, double _k_i_torque)
static int K_I_VEL[6]
static int K_I_POS[6]
static int K_P_POS[6]
static int K_P_TOR[6]
static int K_I_TOR[6]
static int K_P_VEL[6]
double degToRad(const double &_deg)
The degToRad function converts degrees to radians.
Definition: angle_utils.cpp:33
int main(int argc, char **argv)