Skip to content
This repository was archived by the owner on Dec 1, 2020. It is now read-only.

Commit 22da3b4

Browse files
committed
Clang
1 parent 6417a8a commit 22da3b4

File tree

4 files changed

+41
-42
lines changed

4 files changed

+41
-42
lines changed

march_hardware/include/march_hardware/motor_controller/odrive/odrive_states.h

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -23,7 +23,7 @@ struct OdriveStates : public MotorControllerStates
2323
virtual std::string getErrorStatus()
2424
{
2525
std::ostringstream error_stream;
26-
// std::string state = IMCStateOfOperation(this->statusWord).getString().c_str();
26+
// std::string state = IMCStateOfOperation(this->statusWord).getString().c_str();
2727

2828
error_stream << "State: " << state;
2929
return error_stream.str();

march_hardware/src/imotioncube/imotioncube.cpp

Lines changed: 7 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -186,11 +186,11 @@ void IMotionCube::actuateIU(int32_t target_iu)
186186

187187
void IMotionCube::actuateTorque(double target_torque_ampere)
188188
{
189-
if (target_torque_ampere >= IPEAK)
190-
{
191-
throw error::HardwareException(error::ErrorType::TARGET_TORQUE_EXCEEDS_MAX_TORQUE,
192-
"Target torque of %dA exceeds max torque of %dA", target_torque_ampere, IPEAK);
193-
}
189+
if (target_torque_ampere >= IPEAK)
190+
{
191+
throw error::HardwareException(error::ErrorType::TARGET_TORQUE_EXCEEDS_MAX_TORQUE,
192+
"Target torque of %dA exceeds max torque of %dA", target_torque_ampere, IPEAK);
193+
}
194194
int16_t target_torque = ampereToTorqueIU(target_torque_ampere);
195195

196196
if (this->actuation_mode_ != ActuationMode::torque)
@@ -558,7 +558,7 @@ ActuationMode IMotionCube::getActuationMode() const
558558

559559
int16_t IMotionCube::ampereToTorqueIU(double ampere)
560560
{
561-
// See CoE manual page 222
562-
return AMPERE_TO_IU_FACTOR * ampere / (2 * IPEAK);
561+
// See CoE manual page 222
562+
return AMPERE_TO_IU_FACTOR * ampere / (2 * IPEAK);
563563
}
564564
} // namespace march

march_hardware/src/odrive/odrive_motor.cpp

Lines changed: 32 additions & 33 deletions
Original file line numberDiff line numberDiff line change
@@ -25,10 +25,10 @@ void OdriveMotor::prepareActuation()
2525
}
2626

2727
// to be implemented
28-
void OdriveMotor::reset()
29-
{
30-
return;
31-
}
28+
void OdriveMotor::reset()
29+
{
30+
return;
31+
}
3232

3333
void OdriveMotor::actuateRad(double target_rad)
3434
{
@@ -38,32 +38,31 @@ void OdriveMotor::actuateRad(double target_rad)
3838

3939
void OdriveMotor::actuateTorque(double target_torque_ampere)
4040
{
41-
float target_torque_ampere_float = (float) target_torque_ampere;
42-
std::string command_name_ = this->create_command(O_PM_DESIRED_MOTOR_CURRENT);
43-
if (this->write(command_name_, target_torque_ampere_float) == 1)
44-
{
45-
ROS_ERROR("Could net set target torque; %f to the axis", target_torque_ampere);
46-
}
41+
float target_torque_ampere_float = (float)target_torque_ampere;
42+
std::string command_name_ = this->create_command(O_PM_DESIRED_MOTOR_CURRENT);
43+
if (this->write(command_name_, target_torque_ampere_float) == 1)
44+
{
45+
ROS_ERROR("Could net set target torque; %f to the axis", target_torque_ampere);
46+
}
4747
}
4848

4949
MotorControllerStates& OdriveMotor::getStates()
5050
{
51-
static OdriveStates states;
52-
53-
// Common states
54-
states.motorCurrent = this->getMotorCurrent();
55-
states.controllerVoltage = this->getMotorControllerVoltage();
56-
states.motorVoltage = this->getMotorVoltage();
51+
static OdriveStates states;
5752

58-
states.absoluteEncoderValue = this->getAngleCountsAbsolute();
59-
states.incrementalEncoderValue = this->getAngleCountsIncremental();
60-
states.absoluteVelocity = this->getVelocityRadAbsolute();
61-
states.incrementalVelocity = this->getVelocityRadIncremental();
53+
// Common states
54+
states.motorCurrent = this->getMotorCurrent();
55+
states.controllerVoltage = this->getMotorControllerVoltage();
56+
states.motorVoltage = this->getMotorVoltage();
6257

63-
states.state = States(this->getState());
58+
states.absoluteEncoderValue = this->getAngleCountsAbsolute();
59+
states.incrementalEncoderValue = this->getAngleCountsIncremental();
60+
states.absoluteVelocity = this->getVelocityRadAbsolute();
61+
states.incrementalVelocity = this->getVelocityRadIncremental();
6462

65-
return states;
63+
states.state = States(this->getState());
6664

65+
return states;
6766
}
6867

6968
float OdriveMotor::getMotorControllerVoltage()
@@ -114,13 +113,13 @@ double OdriveMotor::getTorque()
114113

115114
int OdriveMotor::getAngleCountsAbsolute()
116115
{
117-
return 0;
116+
return 0;
118117
}
119118

120119
double OdriveMotor::getAngleRadAbsolute()
121120
{
122-
double angle_rad = this->getAngleCountsAbsolute() * PI_2 / std::pow(2, 17);
123-
return angle_rad;
121+
double angle_rad = this->getAngleCountsAbsolute() * PI_2 / std::pow(2, 17);
122+
return angle_rad;
124123
}
125124

126125
double OdriveMotor::getVelocityRadAbsolute()
@@ -135,14 +134,14 @@ bool OdriveMotor::getIncrementalMorePrecise() const
135134

136135
int OdriveMotor::getAngleCountsIncremental()
137136
{
138-
float iu_position;
139-
std::string command_name_ = this->create_command(O_PM_ENCODER_POSITION_UI);
140-
if (this->read(command_name_, iu_position) == 1)
141-
{
142-
ROS_ERROR("Could not retrieve incremental position of the encoder");
143-
return ODRIVE_ERROR;
144-
}
145-
return iu_position;
137+
float iu_position;
138+
std::string command_name_ = this->create_command(O_PM_ENCODER_POSITION_UI);
139+
if (this->read(command_name_, iu_position) == 1)
140+
{
141+
ROS_ERROR("Could not retrieve incremental position of the encoder");
142+
return ODRIVE_ERROR;
143+
}
144+
return iu_position;
146145
}
147146

148147
double OdriveMotor::getAngleRadIncremental()

march_hardware_builder/src/hardware_builder.cpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -134,7 +134,7 @@ march::Joint HardwareBuilder::createJoint(const YAML::Node& joint_config, const
134134
}
135135
if (joint_config["odrive"])
136136
{
137-
controller = HardwareBuilder::createOdrive(joint_config["odrive"], mode, urdf_joint, usb_master);
137+
controller = HardwareBuilder::createOdrive(joint_config["odrive"], mode, urdf_joint, usb_master);
138138
}
139139
if (!controller)
140140
{

0 commit comments

Comments
 (0)