oui

DynamixelSDK

@@ -1 +0,0 @@
-Subproject commit ef7ae1f6fc7f58f7276859244acc231297632d4d

Makefile

@@ -1,11 +1,11 @@
 all: dynamixel main
 	g++ DynamixelHandler.o main.o -L/usr/local/lib -ldxl_x64_cpp -lrt
 
-dynamixel: /home/estevan/EENG3/robotarm/toolkit-dynamixel/src/DynamixelHandler.cpp
+dynamixel: /home/ros/robotarm/toolkit-dynamixel/src/DynamixelHandler.cpp
-	g++ -c /home/estevan/EENG3/robotarm/toolkit-dynamixel/src/DynamixelHandler.cpp -I./toolkit-dynamixel/include toolkit-dynamixel/src/DynamixelHandler.cpp
+	g++ -c /home/ros/robotarm/toolkit-dynamixel/src/DynamixelHandler.cpp -I./toolkit-dynamixel/include toolkit-dynamixel/src/DynamixelHandler.cpp
 
 main: main.cpp
 	g++ -c main.cpp -I./toolkit-dynamixel/include main.cpp
 clean:
 	rm *.o
-	rm *.exe
+	rm *.out

README.md

@@ -1,3 +1,3 @@
 # robotarm
-We love robots UwU       
+
 make a robot arm works

main.cpp

@@ -2,108 +2,56 @@
 #include <chrono>
 #include <thread>
 #include "DynamixelHandler.h"
-
 // Global variables
 DynamixelHandler _oDxlHandler;
 std::string _poppyDxlPortName = "/dev/ttyUSB0";
 float _poppyDxlProtocol = 2.0;
 int _poppyDxlBaudRate = 1000000;
 int _nbJoints = 6;
-int i = 0;
 float _minJointCmd = 0;
 float _maxJointCmd = 1023;
 float _minJointAngle = -180.0f;
 float _maxJointAngle = 180.0f;
-
 int convertAnglesToJointCmd(float fJointAngle)
-{
+{ // y = ax + b
-    // y = ax + b
+float a = (_maxJointCmd-_minJointCmd) / (_maxJointAngle - _minJointAngle);
-    float a = (_maxJointCmd-_minJointCmd) / (_maxJointAngle - _minJointAngle);
+float b = _minJointCmd - a * _minJointAngle;
-    float b = _minJointCmd - a * _minJointAngle;
+float jointCmd = a * fJointAngle + b;
-    float jointCmd = a * fJointAngle + b;
+return (int)jointCmd;
-    return (int)jointCmd;
 }
-
+void goToHomePosition()
-void goToHomePosition() // This function sets all the servos to their home position except the gripper servo
+{ std::vector<uint16_t> l_vTargetJointPosition;
-{ 
+for (int l_joint = 0; l_joint < _nbJoints; l_joint++)
-	std::vector<uint16_t> l_vTargetJointPosition;
+l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f));
-	for (int l_joint = 1; l_joint < _nbJoints; l_joint++)
+_oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
-	{
-	l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f));
-	}
-	int l_joint=6;
-	if (l_joint= 6)	
-	{
-		l_vTargetJointPosition.push_back(convertAnglesToJointCmd(30.0f));
-	}
-    	_oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
 }
-
-void gripperControl() // This function sets all the servos to their current position(meaning they wont move from whichever position they were in) And closes the gripper to hold the cube
-{
-	std::vector<uint16_t> l_vCurrentJointPosition;
-	_oDxlHandler.readCurrentJointPosition(l_vCurrentJointPosition);
-	// display current joint position
-	// Calculate the target joint command based on the desired angle
-	std::vector<uint16_t> l_vTargetJointPosition;
-	for (int l_joint = 1; l_joint < _nbJoints; l_joint++)
-	{
-		l_vTargetJointPosition.push_back(convertAnglesToJointCmd(l_vCurrentJointPosition[l_joint]));
-	}
-	int l_joint=6;
-	if (l_joint= 6)
-	{
-		l_vTargetJointPosition.push_back(convertAnglesToJointCmd(30.0f));
-	}
-	
-	_oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
-}
-
-
 int main()
-{ 
+{ std::cout << "===Initialization of the Dynamixel Motor communication====" << std::endl;
-	std::cout << "===Initialization of the Dynamixel Motor communication====" << std::endl;
+_oDxlHandler.setDeviceName(_poppyDxlPortName);
-	_oDxlHandler.setDeviceName(_poppyDxlPortName);
+_oDxlHandler.setProtocolVersion(_poppyDxlProtocol);
-	_oDxlHandler.setProtocolVersion(_poppyDxlProtocol);
+_oDxlHandler.openPort();
-	_oDxlHandler.openPort();
+_oDxlHandler.setBaudRate(_poppyDxlBaudRate);
-	_oDxlHandler.setBaudRate(_poppyDxlBaudRate);
+_oDxlHandler.enableTorque(true);
-	_oDxlHandler.enableTorque(true);
+std::cout << std::endl;
-	std::cout << std::endl;
+// read current joint position
-
+std::vector<uint16_t> l_vCurrentJointPosition;
-	// read current joint position
+_oDxlHandler.readCurrentJointPosition(l_vCurrentJointPosition);
-	std::vector<uint16_t> l_vCurrentJointPosition;
+// display current joint position
-	_oDxlHandler.readCurrentJointPosition(l_vCurrentJointPosition);
+std::cout << "vCurrentJointPosition= (" << std::endl;
-	// display current joint position
+for (int l_joint=0; l_joint < l_vCurrentJointPosition.size(); l_joint++)
-	std::cout << "vCurrentJointPosition= (" << std::endl;
+std::cout << l_vCurrentJointPosition[l_joint] << ", ";
-	for (int l_joint=0; l_joint < l_vCurrentJointPosition.size(); l_joint++)
+std::cout << ")" << std::endl;
-	{
+// wait 1s
-		std::cout << l_vCurrentJointPosition[l_joint] << ", ";
+goToHomePosition();
-	}
+// display current joint position
-	std::cout << ")" << std::endl;
+std::cout << "vCurrentJointPosition= (" << std::endl;
-
+for (int l_joint=0; l_joint < l_vCurrentJointPosition.size(); l_joint++)
-
+std::cout << l_vCurrentJointPosition[l_joint] << ", ";
-
+std::cout << ")" << std::endl;
-	std::vector<uint16_t> l_vTargetJointPosition;
+std::this_thread::sleep_for(std::chrono::milliseconds(100000));
-	_oDxlHandler.readCurrentJointPosition(l_vTargetJointPosition);
+std::cout << "===Closing the Dynamixel Motor communication====" << std::endl;
-	std::cout << "vCurrentJointPosition= (" << std::endl;
+_oDxlHandler.enableTorque(false);
-
+_oDxlHandler.closePort();
-	for (int l_joint=0; l_joint < l_vCurrentJointPosition.size(); l_joint++)
+std::cout << std::endl;
-		std::cout << l_vCurrentJointPosition[l_joint] << ", ";
+return 0;
-		std::cout << ")" << std::endl;
-
-
-	
-	gripperControl(); // Catch the cube
-
-	std::this_thread::sleep_for(std::chrono::milliseconds(2000));
-	goToHomePosition(); //Take the cube up to home position
-	
-		// wait 1s
-	std::this_thread::sleep_for(std::chrono::milliseconds(2000));
-	std::cout << "===Closing the Dynamixel Motor communication====" << std::endl;
-	_oDxlHandler.enableTorque(false);
-	_oDxlHandler.closePort();
-	std::cout << std::endl;
-	return 0;
 }
-