From 41f31ff0a336a85c664df8ece0d73369db7b5704 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Emek=20Bar=C4=B1=C5=9F=20K=C3=BC=C3=A7=C3=BCktabak?=
 <32764586+emekBaris@users.noreply.github.com>
Date: Thu, 4 Mar 2021 15:16:38 -0600
Subject: [PATCH] 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
---
 config/x2_dynamic_params.cfg           |  3 ++-
 src/hardware/IO/FourierForceSensor.cpp | 26 ++++++++++++++++++++++++--
 src/hardware/IO/FourierForceSensor.h   |  7 +++++++
 src/hardware/platforms/X2/X2Robot.cpp  |  2 +-
 4 files changed, 34 insertions(+), 4 deletions(-)

diff --git a/config/x2_dynamic_params.cfg b/config/x2_dynamic_params.cfg
index 8938c636a..c3b3d8b0a 100644
--- 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"))
\ No newline at end of file
diff --git a/src/hardware/IO/FourierForceSensor.cpp b/src/hardware/IO/FourierForceSensor.cpp
index 77a3a6244..947da19b7 100644
--- 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
index c3c4e7933..4774f32f0 100644
--- 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
index 96e56f5f3..aa98e256b 100644
--- 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]);
     }