Test CSP, temporarily disable legacy PT/PVT submodes

roboticslab-uc3m · Jan 14, 2020 · 7233a84 · 7233a84
1 parent 43ac09b
commit 7233a84
Show file tree

Hide file tree

Showing 8 changed files with 30 additions and 13 deletions.
diff --git a/libraries/YarpPlugins/CanBusControlboard/DeviceDriverImpl.cpp b/libraries/YarpPlugins/CanBusControlboard/DeviceDriverImpl.cpp
@@ -293,8 +293,10 @@ bool CanBusControlboard::open(yarp::os::Searchable & config)
                 true);
     }
 
-    posdThread = new PositionDirectThread(deviceMapper);
-    return posdThread->configure(config) && (!syncTimer || syncTimer->start());
+    //posdThread = new PositionDirectThread(deviceMapper);
+    //return posdThread->configure(config) && (!syncTimer || syncTimer->start());
+
+    return !syncTimer || syncTimer->start();
 }
 
 // -----------------------------------------------------------------------------

diff --git a/libraries/YarpPlugins/CanBusControlboard/IControlModeImpl.cpp b/libraries/YarpPlugins/CanBusControlboard/IControlModeImpl.cpp
@@ -41,7 +41,7 @@ bool CanBusControlboard::setControlMode(int j, int mode)
     CHECK_JOINT(j);
 
     return deviceMapper.mapSingleJoint(&yarp::dev::IControlModeRaw::setControlModeRaw, j, mode)
-            && posdThread->updateControlModeRegister(j, mode);
+            ;//&& posdThread->updateControlModeRegister(j, mode); // FIXME
 }
 
 // -----------------------------------------------------------------------------
@@ -51,7 +51,7 @@ bool CanBusControlboard::setControlModes(int * modes)
     CD_DEBUG("\n");
 
     return deviceMapper.mapAllJoints(&yarp::dev::IControlModeRaw::setControlModesRaw, modes)
-            && posdThread->updateControlModeRegister(modes, nodeDevices.size());
+            ;//&& posdThread->updateControlModeRegister(modes, nodeDevices.size()); // FIXME
 }
 
 // -----------------------------------------------------------------------------
@@ -61,7 +61,7 @@ bool CanBusControlboard::setControlModes(int n_joint, const int * joints, int *
     CD_DEBUG("(%d)\n",n_joint);
 
     return deviceMapper.mapJointGroup(&yarp::dev::IControlModeRaw::setControlModesRaw, n_joint, joints, modes)
-            && posdThread->updateControlModeRegister(n_joint, joints, modes);
+            ;//&& posdThread->updateControlModeRegister(n_joint, joints, modes); // FIXME
 }
 
 // -----------------------------------------------------------------------------
diff --git a/libraries/YarpPlugins/TechnosoftIpos/DeviceDriverImpl.cpp b/libraries/YarpPlugins/TechnosoftIpos/DeviceDriverImpl.cpp
@@ -51,6 +51,7 @@ bool TechnosoftIpos::open(yarp::os::Searchable & config)
     vars.refSpeed = config.check("refSpeed", yarp::os::Value(0.0), "ref speed (meters/second or degrees/second)").asFloat64();
     vars.refAcceleration = config.check("refAcceleration", yarp::os::Value(0.0), "ref acceleration (meters/second^2 or degrees/second^2)").asFloat64();
     vars.heartbeatPeriod = config.check("heartbeatPeriod", yarp::os::Value(100), "CAN heartbeat period (ms)").asInt32(); // TODO
+    vars.syncPeriod = config.check("syncPeriod", yarp::os::Value(0.0), "SYNC message period (s)").asDouble(); // TODO
     vars.initialMode = config.check("initialMode", yarp::os::Value(VOCAB_CM_POSITION), "initial YARP control mode vocab").asVocab(); // TODO
 
     if (!vars.validateInitialState(canId))

diff --git a/libraries/YarpPlugins/TechnosoftIpos/ICanBusSharerImpl.cpp b/libraries/YarpPlugins/TechnosoftIpos/ICanBusSharerImpl.cpp
@@ -187,6 +187,12 @@ bool TechnosoftIpos::interpretMessage(const yarp::dev::CanMessage & message)
 
 bool TechnosoftIpos::synchronize()
 {
+    if (vars.actualControlMode == VOCAB_CM_POSITION_DIRECT)
+    {
+        std::int32_t value = vars.degreesToInternalUnits(vars.synchronousCommandTarget);
+        return can->rpdo3()->write(value);
+    }
+
     return true;
 }
 

diff --git a/libraries/YarpPlugins/TechnosoftIpos/IControlModeRawImpl.cpp b/libraries/YarpPlugins/TechnosoftIpos/IControlModeRawImpl.cpp
@@ -122,8 +122,13 @@ bool TechnosoftIpos::setControlModeRaw(int j, int mode)
                 && vars.awaitControlMode(mode);
 
     case VOCAB_CM_POSITION_DIRECT:
-        return can->driveStatus()->requestState(DriveState::OPERATION_ENABLED)
-                && setPositionDirectModeRaw()
+        vars.synchronousCommandTarget = vars.internalUnitsToDegrees(vars.lastEncoderRead.queryPosition());
+
+        return can->rpdo3()->configure(PdoConfiguration().addMapping<std::int32_t>(0x607A))
+                && can->driveStatus()->requestState(DriveState::OPERATION_ENABLED)
+                && can->sdo()->download<std::uint8_t>("Interpolation time period", vars.syncPeriod * 1000, 0x60C2, 0x01)
+                && can->sdo()->download<std::int8_t>("Interpolation time period", -3, 0x60C2, 0x02)
+                && can->sdo()->download<std::int8_t>("Modes of Operation", 8, 0x6060)
                 && vars.awaitControlMode(mode);
 
     case VOCAB_CM_FORCE_IDLE:

diff --git a/libraries/YarpPlugins/TechnosoftIpos/IPositionDirectRawImpl.cpp b/libraries/YarpPlugins/TechnosoftIpos/IPositionDirectRawImpl.cpp
@@ -13,7 +13,7 @@ bool TechnosoftIpos::setPositionRaw(int j, double ref)
     CD_DEBUG("(%d, %f)\n", j, ref);
     CHECK_JOINT(j);
     CHECK_MODE(VOCAB_CM_POSITION_DIRECT);
-    linInterpBuffer->updateTarget(ref);
+    vars.synchronousCommandTarget = ref;
     return true;
 }
 // -----------------------------------------------------------------------------
@@ -39,7 +39,7 @@ bool TechnosoftIpos::getRefPositionRaw(int joint, double * ref)
     CD_DEBUG("(%d)\n", joint);
     CHECK_JOINT(joint);
     CHECK_MODE(VOCAB_CM_POSITION_DIRECT);
-    *ref = linInterpBuffer->getLastTarget();
+    *ref = vars.synchronousCommandTarget;
     return true;
 }
 

diff --git a/libraries/YarpPlugins/TechnosoftIpos/StateVariables.hpp b/libraries/YarpPlugins/TechnosoftIpos/StateVariables.hpp
@@ -128,6 +128,8 @@ struct StateVariables
     std::atomic<double> lastHeartbeat;
     std::atomic<std::uint8_t> lastNmtState;
 
+    std::atomic<double> synchronousCommandTarget;
+
     // read only, conceptually immutable
 
     yarp::conf::vocab32_t initialMode = 0;
@@ -145,6 +147,7 @@ struct StateVariables
     PdoConfiguration tpdo3Conf;
 
     std::uint16_t heartbeatPeriod = 0;
+    double syncPeriod = 0.0;
 };
 
 } // namespace roboticslab

diff --git a/libraries/YarpPlugins/TechnosoftIpos/TechnosoftIpos.cpp b/libraries/YarpPlugins/TechnosoftIpos/TechnosoftIpos.cpp
@@ -326,6 +326,10 @@ void TechnosoftIpos::interpretModesOfOperation(std::int8_t modesOfOperation)
         break;
     case 7:
         CD_INFO("Interpolated Position Mode. canId: %d.\n", can->getId());
+        vars.actualControlMode = VOCAB_CM_UNKNOWN; // FIXME
+        break;
+    case 8:
+        CD_INFO("Cyclic Synchronous Position Mode. canId: %d.\n", can->getId());
         vars.actualControlMode = VOCAB_CM_POSITION_DIRECT;
         break;
     // unhandled
@@ -349,10 +353,6 @@ void TechnosoftIpos::interpretModesOfOperation(std::int8_t modesOfOperation)
         CD_INFO("Homing Mode. canId: %d.\n", can->getId());
         vars.actualControlMode = VOCAB_CM_UNKNOWN;
         break;
-    case 8:
-        CD_INFO("Cyclic Synchronous Position Mode. canId: %d.\n", can->getId());
-        vars.actualControlMode = VOCAB_CM_UNKNOWN;
-        break;
     default:
         CD_WARNING("No mode set. canId: %d.\n", can->getId());
         vars.actualControlMode = VOCAB_CM_UNKNOWN;