Minor changes related to ForceSensor (#43)

* default calibration value is added * no controller mode and can parameter for sim is added * X2 Force sensor node id is fixed. Default calibration Offset value of 1500 is added. a new function to send SDO message for internal calibration is added. * small comment change * small change for pull request
UniMelbHumanRoboticsLab · Mar 4, 2021 · 41f31ff · 41f31ff
1 parent d79c8cf
commit 41f31ff
Show file tree

Hide file tree

Showing 4 changed files with 34 additions and 4 deletions.
diff --git a/config/x2_dynamic_params.cfg b/config/x2_dynamic_params.cfg
@@ -12,6 +12,7 @@ gen.add("m_admittance", double_t, 2, "Admittance Mass", 5, 1, 20)
 gen.add("b_admittance", double_t, 3, "Admittance damper", 3, 0, 20)
 
 interaction_enum = gen.enum([
+                   gen.const("no_control", int_t, 0, "controller 0"),
                    gen.const("zero_torque", int_t, 1, "controller 1"),
                    gen.const("zero_velocity", int_t, 2, "controller 2"),
                    gen.const("ff_model_compensation", int_t, 3, "controller 3"),
@@ -21,6 +22,6 @@ interaction_enum = gen.enum([
                    gen.const("chirp_torque", int_t, 7, "controller 7"),],
                    "Test Parameters Mode")
 
-gen.add("controller_mode", int_t, 0, "Controller Mode", 2, 1, 7, edit_method=interaction_enum)
+gen.add("controller_mode", int_t, 0, "Controller Mode", 0, 0, 7, edit_method=interaction_enum)
 
 exit(gen.generate(PACKAGE, "CORC", "dynamic_params"))
diff --git a/src/hardware/IO/FourierForceSensor.cpp b/src/hardware/IO/FourierForceSensor.cpp
@@ -4,7 +4,8 @@ FourierForceSensor::FourierForceSensor(int sensor_can_node_ID, double scale_fact
                                                                                                         sensorNodeID(sensor_can_node_ID),
                                                                                                         scaleFactor(scale_factor),
                                                                                                         calibrated(false),
-                                                                                                        calibrationTime(calib_time) {
+                                                                                                        calibrationTime(calib_time),
+                                                                                                        calibrationOffset(1500){
 }
 
 bool FourierForceSensor::configureMasterPDOs() {
@@ -22,7 +23,7 @@ void FourierForceSensor::updateInput() {
 }
 
 bool FourierForceSensor::calibrate(double calib_time) {
-    spdlog::debug("[FourierForceSensor::calibrate]: Force Sensor {} Zeroing", sensorNodeID);
+    spdlog::debug("[FourierForceSensor::calibrate]: Force Sensor with nodeID {} Zeroing", sensorNodeID);
 
     if(calib_time>0) {
         calibrationTime = calib_time;
@@ -59,3 +60,24 @@ double FourierForceSensor::sensorValueToNewton(int sensorValue) {
     return (sensorValue - calibrationOffset) * scaleFactor;
 }
 
+bool FourierForceSensor::sendInternalCalibrateSDOMessage() {
+
+    spdlog::debug("[FourierForceSensor::sendInternalCalibrateSDOMessage]: Force Sensor with nodeID {} Internal calibration", sensorNodeID);
+
+    std::stringstream sstream;
+    char *returnMessage;
+
+    sstream << "[1] " << sensorNodeID << " read 0x7050 255 i8";
+    std::string strCommand = sstream.str();
+    char *SDO_Message = (char *)(strCommand.c_str());
+    cancomm_socketFree(SDO_Message, &returnMessage);
+    std::string retMsg = returnMessage;
+    spdlog::debug(retMsg);
+    if (retMsg.find("ERROR") != std::string::npos) {
+        spdlog::error("[X2ForceSensor::calibrate]: Force Sensor {} error occured during zeroing", sensorNodeID);
+        return false;
+    }
+
+    sleep(1.5); // this is required because after calibration command, sensor values do not get update around 1.2 seconds
+}
+
diff --git a/src/hardware/IO/FourierForceSensor.h b/src/hardware/IO/FourierForceSensor.h
@@ -57,6 +57,13 @@ class FourierForceSensor : public InputDevice {
     */
     double getForce();
 
+    /**
+    * send SDO command to shift the measurement to a value around 1500.
+    *
+    * \return bool success of internal calibration
+    */
+    bool sendInternalCalibrateSDOMessage();
+
   protected:
     virtual double sensorValueToNewton(int sensorValue);
 

diff --git a/src/hardware/platforms/X2/X2Robot.cpp b/src/hardware/platforms/X2/X2Robot.cpp
@@ -477,7 +477,7 @@ bool X2Robot::initialiseInputs() {
     inputs.push_back(keyboard = new Keyboard());
 
     for (int id = 0; id < X2_NUM_FORCE_SENSORS; id++) {
-        forceSensors.push_back(new FourierForceSensor(id, x2Parameters.forceSensorScaleFactor[id]));
+        forceSensors.push_back(new FourierForceSensor(id + 17, x2Parameters.forceSensorScaleFactor[id]));
         inputs.push_back(forceSensors[id]);
     }