Merge pull request #7 from mabrobotics/devel

devel to main

CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.5)
-set(CMAKE_CXX_STANDARD 20)
+set(CMAKE_CXX_STANDARD 14)
 
 project(md80)
 

README.md

@@ -1,6 +1,6 @@
 # CANdle library 
 CANdle C++ library is the heart of the MD80 x CANdle ecosystem on the master controller side. 
-It takes care of all actions connected to communication and provides API for higher-level software. For a detailed manual please see [MD80 x CANdle Manual](https://www.mabrobotics.pl/servos/#comp-l6v4io99).
+It takes care of all actions connected to communication and provides API for higher-level software. For a detailed manual please see [MD80 x CANdle Manual](https://www.mabrobotics.pl/servos/manual).
 
 ## C++ library
 To run the examples clone the repo and make sure you're in the `candle` main directory. Execute:
@@ -73,7 +73,6 @@ Similar to `example 6`, but based on the SPI bus available on single-board compu
 Similar to `example 6`, but based on the UART bus available on single-board computers. Before running be sure all actuators are switched to 8M speed. 
 
 ### Example 12
-Register read demo. 
-Note: Currently registers operations are only available in C++!
+Register read-write demo. Please remember that register operations are only valid before candle.begin() is called. 
 
 

examples_cpp/example12.cpp

@@ -1,5 +1,8 @@
 #include "candle.hpp"
 
+bool readAndDisplayRegisters(mab::Candle& candle, uint16_t id);
+void error();
+
 int main()
 {
 	mab::Candle candle(mab::CAN_BAUD_1M, true);
@@ -8,24 +11,48 @@ int main()
 	/* add the first MD80 to the list */
 	candle.addMd80(ids[0]);
 
-	/* get the reference to the regR struct that holds fields of registers */
-	mab::regRead_st& regR = candle.getMd80FromList(ids[0]).getReadReg();
+	std::cout << std::endl;
+
+	// Lets first read some registers NOTE: registers cannot be accessed after candle.begin();
+	if (!readAndDisplayRegisters(candle, ids[0]))
+		std::cout << "Error while reading registers!" << std::endl;
 
-	/* read registers of your choice NOTE: remember that a single read function call cannot exceed 62 bytes (runtime exception will be thrown otherwise),
-	 including the 2 byte reg ID that is attached per register.
-	 In this example the length is: 2 (resistance reg ID) + 4 (float value) + 2 (inductance reg ID) + 4 (float value)  = 12 bytes */
-	if (!candle.readMd80Register(ids[0],
-								 mab::Md80Reg_E::motorResistance, regR.RO.resistance,
-								 mab::Md80Reg_E::motorInductance, regR.RO.inductance))
-	{
-		std::cout << "Reading register failed at ID: " << ids[0] << std::endl;
-		return false;
-	}
+	std::cout << std::endl;
 
-	/* NOTE: please make sure the motor is calibrated or otherwise you will read zeros in these registers */
+	// Then change some parameters
+	if (!candle.writeMd80Register(ids[0], mab::Md80Reg_E::motorName, "EXAMPLE12")) error();
+	if (!candle.writeMd80Register(ids[0], mab::Md80Reg_E::motorImpPidKp, 1.0f)) error();
+	if (!candle.writeMd80Register(ids[0], mab::Md80Reg_E::motorImpPidKd, 0.01f)) error();
+	if (!candle.writeMd80Register(ids[0], mab::Md80Reg_E::motorImpPidOutMax, 1.0f)) error();
 
-	std::cout << "Motor d-axis resistance: " << regR.RO.resistance << " Ohms " << std::endl;
-	std::cout << "Motor d-axis inductance: " << regR.RO.inductance << " H " << std::endl;
+	// And read one more time NOTE: these settings are not saved!
+	if (!readAndDisplayRegisters(candle, ids[0]))
+		std::cout << "Error while reading registers!" << std::endl;
 
 	return EXIT_SUCCESS;
+}
+
+bool readAndDisplayRegisters(mab::Candle& candle, uint16_t id)
+{
+	/* get the reference to the regR struct that holds fields of registers */
+	mab::regRead_st& reg = candle.getMd80FromList(id).getReadReg();
+
+	if (!candle.readMd80Register(id, mab::Md80Reg_E::canId, reg.RW.canId)) return false;
+	if (!candle.readMd80Register(id, mab::Md80Reg_E::motorName, reg.RW.motorName)) return false;
+	if (!candle.readMd80Register(id, mab::Md80Reg_E::motorImpPidKp, reg.RW.impedancePdGains.kp)) return false;
+	if (!candle.readMd80Register(id, mab::Md80Reg_E::motorImpPidKd, reg.RW.impedancePdGains.kd)) return false;
+	if (!candle.readMd80Register(id, mab::Md80Reg_E::motorImpPidOutMax, reg.RW.impedancePdGains.outMax)) return false;
+
+	std::cout << "Drive ID: " << unsigned(reg.RW.canId) << std::endl;
+	std::cout << "Motor name: " << std::string(reg.RW.motorName) << std::endl;
+	std::cout << "Impedance mode Kp gain : " << reg.RW.impedancePdGains.kp << " Nm/rad" << std::endl;
+	std::cout << "Impedance mode Kd gain : " << reg.RW.impedancePdGains.kd << " Nm*s/rad" << std::endl;
+	std::cout << "Impedance mode max out : " << reg.RW.impedancePdGains.outMax << " Nm" << std::endl;
+
+	return true;
+}
+
+void error()
+{
+	std::cout << "Error while writing register!" << std::endl;
 }

examples_cpp/example4.cpp

@@ -33,7 +33,7 @@ int main()
 	for (int i = 0; i < 1000; i++)
 	{
 		t += dt;
-		// After powerup the drive will load set of default parameters for every regulator.
+		// After powerup the drive will load set of default parameters for every controller.
 		// To make the drive move all we got to do is set mode (.controlMd80Mode) and set target
 		// (.setTargetPosition, .setTargetVelocity, .setTargetTorque)
 		candle.md80s[0].setTargetPosition(sin(t) * 3.0f);

examples_cpp/example5.cpp

@@ -46,11 +46,11 @@ int main(int argc, char** argv)
 	candle.controlMd80Mode(ids[0], mab::Md80Mode_E::IMPEDANCE);	 // Set mode to impedance control
 	candle.controlMd80Enable(ids[0], true);						 // Enable the drive
 
-	// Now we modify the Impedance regulator parameters - the drive will behave much different than in
+	// Now we modify the Impedance controller parameters - the drive will behave much different than in
 	// previous examples. The drive will change default params to the ones we select below.
 	candle.md80s[0].setImpedanceControllerParams(kp, kd);
 
-	// To reload default regulator parameters, simply disable the drive (contorlMd80Enable(id, false)),
+	// To reload default controller parameters, simply disable the drive (contorlMd80Enable(id, false)),
 	// stop the communications (candle.end()) or power cycle the drive (off-on).
 
 	float t = 0.0f;

examples_cpp/example7.cpp

@@ -24,11 +24,10 @@ int main()
 	candle.controlMd80Mode(ids[0], mab::Md80Mode_E::VELOCITY_PID);	// Set mode to velocity PID
 	candle.controlMd80Enable(ids[0], true);							// Enable the drive
 
-	// Now we set up Velocity Regulator, and additionaly max output velocity just to be on a safe side
-	candle.md80s[0].setVelocityControllerParams(0.05f, 0.25f, 0.0, 1.0f);
-	candle.md80s[0].setMaxVelocity(30.0f);
+	// Uncomment the line below to change the *.cfg file defaults
+	// candle.md80s[0].setVelocityControllerParams(0.05f, 0.25f, 0.0, 1.0f);
 
-	// To reload default regulator parameters, simply disable the drive (contorlMd80Enable(id, false)),
+	// To reload default controller parameters, simply disable the drive (contorlMd80Enable(id, false)),
 	// stop the communications (candle.end()) or power cycle the drive (off-on).
 
 	float t = 0.0f;

examples_cpp/example8.cpp

@@ -27,11 +27,11 @@ int main()
 	// We will run both Position PID and Velocity PID at default settings. If you wish you can play with the parameters
 	// Using the methods below:
 	// candle.md80s[0].setPositionControllerParams(20.0f, 0.2f, 0.0f, 15.0f);
-	// candle.md80s[0].setVelocityControllerParams(0.0f, 0.1f, 0.0f, 1.5f);
-	// candle.md80s[0].setMaxVelocity(50.0);
+	// candle.md80s[0].setVelocityControllerParams(0.5f, 0.1f, 0.0f, 1.5f);
+	// candle.md80s[0].setMaxVelocity(5.0);
 	// candle.md80s[0].setMaxTorque(0.5f);
 
-	// To reload default regulator parameters, simply disable the drive (contorlMd80Enable(id, false)),
+	// To reload default controller parameters, simply disable the drive (contorlMd80Enable(id, false)),
 	// stop the communications (candle.end()) or power cycle the drive (off-on).
 
 	float t = 0.0f;
@@ -43,7 +43,7 @@ int main()
 	for (int i = 0; i < 1000; i++)
 	{
 		t += dt;
-		candle.md80s[0].setTargetPosition(sin(t) * 10.0f);
+		candle.md80s[0].setTargetPosition(sin(t) * 2.0f);
 		std::cout << "Drive ID = " << candle.md80s[0].getId() << " Velocity: " << candle.md80s[0].getVelocity() << std::endl;
 		usleep(10000);
 	}

examples_python/example12.py

@@ -0,0 +1,38 @@
+import pyCandle
+import sys
+
+# Create CANdle object and ping FDCAN bus in search of drives. 
+# Any found drives will be printed out by the ping() method.
+
+candle = pyCandle.Candle(pyCandle.CAN_BAUD_1M,True)
+ids = candle.ping()
+
+# Blink LEDs on each drive found
+
+candle.addMd80(ids[0])
+
+reg = candle.getMd80FromList(ids[0]).getReadReg()
+print("")
+
+# Lets first read some registers NOTE: registers cannot be accessed after candle.begin();
+print("Drive ID: " + str(candle.readMd80Register(ids[0],pyCandle.Md80Reg_E.canId, reg.RW.canId)))
+print("Motor name: " + candle.readMd80Register(ids[0],pyCandle.Md80Reg_E.motorName, ""))
+print("Impedance mode Kp gain : " + "{:.2f}".format(candle.readMd80Register(ids[0],pyCandle.Md80Reg_E.motorImpPidKp, reg.RW.impedancePdGains.kp)) + " Nm/rad")
+print("Impedance mode Kd gain : " + "{:.2f}".format(candle.readMd80Register(ids[0],pyCandle.Md80Reg_E.motorImpPidKd, reg.RW.impedancePdGains.kd)) + " Nm*s/rad")
+print("Impedance mode max out : " + "{:.2f}".format(candle.readMd80Register(ids[0],pyCandle.Md80Reg_E.motorImpPidOutMax, reg.RW.impedancePdGains.outMax)) + " Nm")
+
+print("")
+# Then change some parameters 
+candle.writeMd80Register(ids[0],pyCandle.Md80Reg_E.motorName, "EXAMPLE12")
+candle.writeMd80Register(ids[0],pyCandle.Md80Reg_E.motorImpPidKp, 1.0)
+candle.writeMd80Register(ids[0],pyCandle.Md80Reg_E.motorImpPidKd, 0.01)
+candle.writeMd80Register(ids[0],pyCandle.Md80Reg_E.motorImpPidOutMax, 1.0)
+
+# And read one more time NOTE: these settings are not saved!
+print("Drive ID: " + str(candle.readMd80Register(ids[0],pyCandle.Md80Reg_E.canId, reg.RW.canId)))
+print("Motor name: " + candle.readMd80Register(ids[0],pyCandle.Md80Reg_E.motorName, ""))
+print("Impedance mode Kp gain : " + "{:.2f}".format(candle.readMd80Register(ids[0],pyCandle.Md80Reg_E.motorImpPidKp, reg.RW.impedancePdGains.kp)) + " Nm/rad")
+print("Impedance mode Kd gain : " + "{:.2f}".format(candle.readMd80Register(ids[0],pyCandle.Md80Reg_E.motorImpPidKd, reg.RW.impedancePdGains.kd)) + " Nm*s/rad")
+print("Impedance mode max out : " + "{:.2f}".format(candle.readMd80Register(ids[0],pyCandle.Md80Reg_E.motorImpPidOutMax, reg.RW.impedancePdGains.outMax)) + " Nm")
+
+sys.exit("EXIT SUCCESS")

examples_python/example4.py

@@ -31,7 +31,7 @@
 
 for i in range(1000):
     t=t+dt
-    #  After powerup the drive will load set of default parameters for every regulator.
+    #  After powerup the drive will load set of default parameters for every controller.
     #  To make the drive move all we got to do is set mode (.controlMd80Mode) and set target
     #  (.setTargetPosition, .setTargetVelocity, .setTargetTorque)
     candle.md80s[0].setTargetPosition(math.sin(t) * 3.0)

examples_python/example5.py

@@ -44,16 +44,16 @@ def _range(x, min, max):
 for id in ids:
     candle.addMd80(id)
 
-candle.controlMd80SetEncoderZero(ids[0]);       #  Reset encoder at current position
+candle.controlMd80SetEncoderZero(ids[0]);                #  Reset encoder at current position
 
-candle.controlMd80Mode(ids[0], pyCandle.IMPEDANCE)     # Set mode to impedance control
-candle.controlMd80Enable(ids[0], True)     # Enable the drive
+candle.controlMd80Mode(ids[0], pyCandle.IMPEDANCE)       # Set mode to impedance control
+candle.controlMd80Enable(ids[0], True)                   # Enable the drive
 
-#  Now we modify the Impedance regulator parameters - the drive will behave much different than in 
+#  Now we modify the Impedance controller parameters - the drive will behave much different than in 
 #  previous examples. The drive will change default params to the ones we select below.
 candle.md80s[0].setImpedanceControllerParams(kp, kd)
 
-#  To reload default regulator parameters, simply disable the drive (contorlMd80Enable(id, false)), 
+#  To reload default controller parameters, simply disable the drive (contorlMd80Enable(id, false)), 
 #  stop the communications (candle.end()) or power cycle the drive (off-on).
 
 t = 0.0

examples_python/example7.py

@@ -16,16 +16,15 @@
 for id in ids:
     candle.addMd80(id)
 
-candle.controlMd80SetEncoderZero(ids[0])       #  Reset encoder at current position
+candle.controlMd80SetEncoderZero(ids[0])               #  Reset encoder at current position
 
-candle.controlMd80Mode(ids[0], pyCandle.VELOCITY_PID)     # Set mode to velocity control
-candle.controlMd80Enable(ids[0], True)     # Enable the drive
+candle.controlMd80Mode(ids[0], pyCandle.VELOCITY_PID)  # Set mode to velocity control
+candle.controlMd80Enable(ids[0], True)                 # Enable the drive
 
-#  Now we set up Velocity Regulator, and additionaly max output velocity just to be on a safe side
-candle.md80s[0].setVelocityControllerParams(0.05, 0.25, 0.0, 1.0)
-candle.md80s[0].setMaxVelocity(30.0)
+#  Uncomment the line below to change the *.cfg file defaults
+# candle.md80s[0].setVelocityControllerParams(0.05, 0.25, 0.0, 1.0)
 
-#  To reload default regulator parameters, simply disable the drive (contorlMd80Enable(id, false)), 
+#  To reload default controller parameters, simply disable the drive (contorlMd80Enable(id, false)), 
 #  stop the communications (candle.end()) or power cycle the drive (off-on).
 
 t = 0.0

examples_python/example8.py

@@ -23,12 +23,12 @@
 
 #  We will run both Position PID and Velocity PID at default settings. If you wish you can play with the parameters
 #  Using the methods below:
-#  candle.md80s[0].setPositionControllerParams(20.0, 0.2, 0.0, 15.0)
-#  candle.md80s[0].setVelocityControllerParams(0.0, 0.1, 0.0, 1.5)
-#  candle.md80s[0].setMaxVelocity(50.0)
-#  candle.md80s[0].setMaxTorque(0.5)
+# candle.md80s[0].setPositionControllerParams(20.0, 0.2, 0.0, 15.0)
+# candle.md80s[0].setVelocityControllerParams(0.0, 0.1, 0.0, 1.5)
+# candle.md80s[0].setMaxVelocity(5.0)
+# candle.md80s[0].setMaxTorque(0.5)
 
-#  To reload default regulator parameters, simply disable the drive (contorlMd80Enable(id, false)), 
+#  To reload default controller parameters, simply disable the drive (contorlMd80Enable(id, false)), 
 #  stop the communications (candle.end()) or power cycle the drive (off-on).
 
 t = 0.0
@@ -39,7 +39,7 @@
 
 for i in range(1000):
     t = t  + dt
-    candle.md80s[0].setTargetPosition(math.sin(t) * 10.0)  
+    candle.md80s[0].setTargetPosition(math.sin(t) * 2.0)  
     print("Drive ID = " + str(candle.md80s[0].getId()) + " Velocity: "  + str(candle.md80s[0].getVelocity()))
     time.sleep(0.01)  # Add some delay