add getEncoders

Author: leixiaowei

Arduino/MycobotBasic/CommunicateDefine.h

@@ -112,6 +112,11 @@
 #define SET_ENCODERS				0x3C
 #define SET_ENCODERS_LEN			15
 
+#define GET_ENCODERS                 0x3D
+#define GET_ENCODERS_LEN             2
+#define GET_ENCODES_RETURN_LEN       14
+
+
 
 // Running Status and Settings
 #define GET_SPEED					0x40

Arduino/MycobotBasic/MycobotBasic.cpp

@@ -298,7 +298,6 @@ void* MycobotBasic::readData()
 				*pEncoder = encoder_low + encoder_high * 256;
 				return pEncoder;
 			}
-
 			case GET_GRIPPER_VALUE:
 			{
 				int* returnValue = new int;
@@ -390,7 +389,35 @@ void* MycobotBasic::readData()
 
 				return pCoords;
 			}
+			case GET_ENCODERS:
+			{
+					byte encode_1_high = r_data_14[1];
+					byte encode_1_low = r_data_14[2];
+
+					byte encode_2_high = r_data_14[3];
+					byte encode_2_low = r_data_14[4];
+
+					byte encode_3_high = r_data_14[5];
+					byte encode_3_low = r_data_14[6];
 
+					byte encode_4_high = r_data_14[7];
+					byte encode_4_low = r_data_14[8];
+
+					byte encode_5_high = r_data_14[9];
+					byte encode_5_low = r_data_14[10];
+
+					byte encode_6_high = r_data_14[11];
+					byte encode_6_low = r_data_14[12];
+
+					Angles* pEncoders = new Angles;
+					pEncoders->at(0) = encode_1_low + encode_1_high * 256;
+					pEncoders->at(1) = encode_2_low + encode_2_high * 256;
+					pEncoders->at(2) = encode_3_low + encode_3_high * 256;
+					pEncoders->at(3) = encode_4_low + encode_4_high * 256;
+					pEncoders->at(4) = encode_5_low + encode_5_high * 256;
+					pEncoders->at(5) = encode_6_low + encode_6_high * 256;
+					return pEncoders;
+			}
 			case GET_TOOL_REF:
 			{
 				byte x_high = r_data_14[1];
@@ -872,6 +899,38 @@ int MycobotBasic::getEncoder(int joint)
 	return -1;
 }
 
+Angles MycobotBasic::getEncoders()
+{
+	Serial2.write(header);
+	Serial2.write(header);
+	Serial2.write(GET_ENCODERS_LEN);
+	Serial2.write(GET_ENCODERS);
+	Serial2.write(footer);
+
+	unsigned long t_begin = millis();
+	void* tempPtr = nullptr;
+	Angles* pEncoders = nullptr;
+	Angles encoders;
+
+	while (true)
+	{
+		if (millis() - t_begin > 40)
+			break;
+		tempPtr = readData();
+		if (tempPtr == nullptr)
+			continue;
+		else
+		{
+			pEncoders = (Angles*)tempPtr;
+			for (int i = 0; i < 6; ++i)
+				encoders[i] = pEncoders->at(i);
+			delete pEncoders;
+			return encoders;
+		}
+	}
+	return error_encoders;
+}
+
 void MycobotBasic::setEncoders(Angles angleEncoders, int speed)
 {
 	byte angle_1_high = highByte(static_cast<int>(angleEncoders[0]));

Arduino/MycobotBasic/MycobotBasic.h

@@ -67,9 +67,12 @@ class MycobotBasic
 	void jogAngle(int joint, int direction, int speed);
 	void jogCoord(Axis axis, int direction, int speed);
 	void jogStop();
+
+	//Encoder mode and operation
 	void setEncoder(int joint, int encoder);
 	int getEncoder(int joint);
 	void setEncoders(Angles angleEncoders, int speed);
+	Angles getEncoders();
 
 
 
@@ -156,6 +159,7 @@ class MycobotBasic
 
 	Angles error_angles;
 	Coords error_coords;
+	Angles error_encoders;
 
 	std::map<int, std::string> messages_map;
 };

Arduino/MycobotBasic/examples/getEncoders/getEncoders.ino

@@ -0,0 +1,21 @@
+#include <MycobotBasic.h>
+#include <ParameterList.h>
+
+MycobotBasic myCobot;
+
+Encoders encoders;
+
+void setup() {
+  // 打开Atom链接
+  myCobot.setup();
+  myCobot.powerOn();
+  Serial.begin(9600);
+}
+void loop() {
+  encoders = myCobot.getEncoders();
+  for(auto encoder : encoders) {
+      Serial.print(encoder);
+      Serial.print("   ");;
+  }
+  Serial.println();
+}