added read after the initiation.

JorisWeeda · JorisWeeda · commit 892ea9761558 · 2020-08-18T21:19:53.000+02:00
diff --git a/march_hardware/src/odrive/odrive_motor.cpp b/march_hardware/src/odrive/odrive_motor.cpp
@@ -49,6 +49,7 @@ void OdriveMotor::prepareActuation()
     ROS_FATAL("Setting closed loop control was not finished successfully");
     return;
   }
+  this->readValues();
 }
 
 bool OdriveMotor::waitForIdleState(float timeout)
@@ -227,13 +228,14 @@ int OdriveMotor::getAngleCountsIncremental()
 
 double OdriveMotor::getAngleRadIncremental()
 {
-  double angle_rad = this->getAngleCountsIncremental() * PI_2 / (std::pow(2, 12) * 101);
+  double angle_rad = this->getAngleCountsIncremental() * PI_2 / (std::pow(2, 12) * 101 * -1);
   return angle_rad;
 }
 
 double OdriveMotor::getVelocityRadIncremental()
 {
-  return this->velocity_rad_incremental;
+  double velocity_rad_incremental_double = this->velocity_rad_incremental * -1;
+  return velocity_rad_incremental_double;
 }
 
 int OdriveMotor::setState(uint8_t state)

Original file line number	Diff line number	Diff line change
`@@ -49,6 +49,7 @@ void OdriveMotor::prepareActuation()`
`49`	`49`	`ROS_FATAL("Setting closed loop control was not finished successfully");`
`50`	`50`	`return;`
`51`	`51`	`}`
	`52`	`+ this->readValues();`
`52`	`53`	`}`
`53`	`54`
`54`	`55`	`bool OdriveMotor::waitForIdleState(float timeout)`
`@@ -227,13 +228,14 @@ int OdriveMotor::getAngleCountsIncremental()`
`227`	`228`
`228`	`229`	`double OdriveMotor::getAngleRadIncremental()`
`229`	`230`	`{`
`230`		`- double angle_rad = this->getAngleCountsIncremental() * PI_2 / (std::pow(2, 12) * 101);`
	`231`	`+ double angle_rad = this->getAngleCountsIncremental() * PI_2 / (std::pow(2, 12) * 101 * -1);`
`231`	`232`	`return angle_rad;`
`232`	`233`	`}`
`233`	`234`
`234`	`235`	`double OdriveMotor::getVelocityRadIncremental()`
`235`	`236`	`{`
`236`		`- return this->velocity_rad_incremental;`
	`237`	`+ double velocity_rad_incremental_double = this->velocity_rad_incremental * -1;`
	`238`	`+ return velocity_rad_incremental_double;`
`237`	`239`	`}`
`238`	`240`
`239`	`241`	`int OdriveMotor::setState(uint8_t state)`