Added new arduino code

src/dcaitirobot/embedded/include/communication/UCommands.hpp

@@ -31,6 +31,13 @@ public:
 	 * @param c 		motor to apply the value to
 	 */
 	static void setSetpoint (uint8_t* d, U_Component c);
+	/**
+	 * @brief 			sets the PID parameters
+	 * 
+	 * @param d 		uart data package to read the pid parameters from
+	 * @param c 		motor to apply the value to
+	 */
+	static void setPIDParameter(uint8_t* d, U_Component c);
 };
 
 #endif

src/dcaitirobot/embedded/include/constants.h

@@ -6,7 +6,7 @@
  * 
  */
 
-#define DCAITI_DEBUG			0
+//#define DCAITI_DEBUG			0
 
 /* 1 is on the right, 2 on the left */
 
@@ -89,7 +89,7 @@ typedef enum components {
 } U_Component;
 
 typedef enum requests {
-	DEFAULT_REQUEST, DRIVE_MOTOR, RESET_TIMESTAMP
+	DEFAULT_REQUEST, DRIVE_MOTOR, RESET_TIMESTAMP, DRIVE_ENCODER, PID_PARAMETER
 } U_Request;
 
 #endif

src/dcaitirobot/embedded/src/communication/UARTCom.cpp

@@ -1,5 +1,6 @@
 #include "communication/UARTCom.hpp"
 #include "communication/UCommands.hpp"
+#include "communication/UValue.hpp"
 #include "misc/Utils.hpp"
 #include "constants.h"
 
@@ -7,6 +8,7 @@ uint8_t UARTCom::data[MAX_PACKET_LENGTH];
 uint8_t currentParseIndex;
 int payloadLength;
 uint32_t currentTimestamp = 0;
+extern R2WD _2WD;
 
 /**
  * @brief extracts frametype from data package
@@ -115,7 +117,7 @@ uint16_t computeChecksum (uint8_t* d, uint8_t pl)
 int correctChecksum (uint8_t* d)
 {
 	uint8_t payloadLength = d[PAYLOAD_LENGTH_POS];
-	uint16_t checksumRead = d[PACKAGE_LENGTH_NO_P + payloadLength - 2] | d[PACKAGE_LENGTH_NO_P + payloadLength - 1] << 8;
+	uint16_t checksumRead = d[PACKAGE_LENGTH_NO_P + payloadLength - 1] | d[PACKAGE_LENGTH_NO_P + payloadLength - 2] << 8;
 	return checksumRead == computeChecksum(d, payloadLength);
 }
 
@@ -166,6 +168,14 @@ void handleRequest (uint8_t* d)
 			currentTimestamp = 0;
 			break;
 		}
+		case DRIVE_ENCODER: {
+			UValue::sendBothEncoderValues(_2WD.wheelLeftGetSpeedMMPS()/1000.0, _2WD.wheelRightGetSpeedMMPS()/1000.0);
+			break;
+		}
+		case PID_PARAMETER: {
+			UCommands::setPIDParameter(d, parseComponent(d));
+			break;
+		}
 		default: {
 			Serial.println("UNKNOWN REQUEST");
 			break;

src/dcaitirobot/embedded/src/communication/UCommands.cpp

@@ -31,4 +31,37 @@ void UCommands::setSetpoint (uint8_t* d, U_Component c)
 			break;
 		}
 	}
+}
+
+void UCommands::setPIDParameter (uint8_t* d, U_Component c)
+{
+	float P = Utils::bytesToFloat(d[COMMAND_POS + 1],
+									  d[COMMAND_POS + 2],
+									  d[COMMAND_POS + 3],
+									  d[COMMAND_POS + 4]);
+	float I = Utils::bytesToFloat(d[COMMAND_POS + 5],
+									  d[COMMAND_POS + 6],
+									  d[COMMAND_POS + 7],
+									  d[COMMAND_POS + 8]);
+	float D = Utils::bytesToFloat(d[COMMAND_POS + 9],
+									  d[COMMAND_POS + 10],
+									  d[COMMAND_POS + 11],
+									  d[COMMAND_POS + 12]);
+	switch (c) {
+		case ALL_MOTORS: {
+			_2WD.PIDTune(P,I,D,10);
+			break;
+		}
+		case RIGHT_MOTOR: {
+			_2WD.wheelLeftPIDTune(P,I,D,10);
+			break;
+		}
+		case LEFT_MOTOR: {
+			_2WD.wheelRightPIDTune(P,I,D,10);
+			break;
+		}
+		default: {
+			break;
+		}
+	}
 }

src/dcaitirobot/embedded/src/main.cpp

@@ -14,7 +14,6 @@ int speed = 0;
 #ifndef MICROS_PER_SEC
 #define MICROS_PER_SEC 1000000
 #endif
-# define DEBUG true
 irqISR(irq1, isr1);
 irqISR(irq2, isr2);
 MotorWheel wheelLeft(9, 8, 4, 5, &irq1, REDUCTION_RATIO, 300);	// Motor PWM:Pin9, DIR:Pin8, Encoder A:Pin6, B:Pin7
@@ -35,16 +34,10 @@ void setup(void)
 	uint8_t pl[1];
 	pl[0] = REQUEST_RESET_TIMESTAMP;
 	UARTCom::send(REQUEST, 1, DEFAULT_COMPONENT, pl);
-	_2WD.setCarAdvance(50);
 }
 
 void loop(void)
 {
 	_2WD.PIDRegulate();
 	UARTCom::read(UARTCom::data);
-
-	if (millis() - uartTransmitTimer > 2 * UART_TRANSMIT_MS) {
-		UValue::sendBothEncoderValues(_2WD.wheelLeftGetSpeedMMPS()/1000.0, _2WD.wheelRightGetSpeedMMPS()/1000.0);
-		uartTransmitTimer = millis();
-	}
 }

src/diffdrive_arduino/src/diffdrive_arduino.cpp

@@ -101,7 +101,6 @@ return_type DiffDriveArduino::read(const rclcpp::Time & /* time */, const rclcpp
   double deltaSeconds = diff.count();
   time_ = new_time;
 
-
   if (!arduino_.connected())
   {
     return return_type::ERROR;
@@ -112,31 +111,17 @@ return_type DiffDriveArduino::read(const rclcpp::Time & /* time */, const rclcpp
     // Force the wheel position
   l_wheel_.pos = l_wheel_.pos + l_wheel_.vel * deltaSeconds;
   r_wheel_.pos = r_wheel_.pos + r_wheel_.vel * deltaSeconds;
-
+  return return_type::OK;  
-
-
-  return return_type::OK;
-
-  
 }
 
 return_type DiffDriveArduino::write(const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */)
 {
-
   if (!arduino_.connected())
   {
     return return_type::ERROR;
   }
-
   arduino_.setMotorValues(l_wheel_.cmd, r_wheel_.cmd);
-
+  return return_type::OK;  
-
-
-
-  return return_type::OK;
-
-
-  
 }
 
 } // namespace diffdrive_arduino