Refactor safety checks and improve message handling in CKobuki class

This commit is contained in:
ishak jmilou.ishak
2024-11-06 14:06:15 +01:00
parent 7424c2d033
commit d534940370
5 changed files with 154 additions and 121 deletions

View File

@@ -282,7 +282,6 @@ int CKobuki::measure() {
return 0;
}
long double CKobuki::gyroToRad(signed short GyroAngle) {
long double rad;
@@ -569,76 +568,73 @@ void CKobuki::forward(int speedvalue) {
/// @param degrees Rotation in degrees
void CKobuki::Rotate(int degrees) {
// convert raidans to degrees
float radians = degrees * PI / 180.0;
// convert raidans to degrees
float radians = degrees * PI / 180.0;
// Calculate the rotation speed in radians per second
double radpersec = 1;
// Calculate the rotation speed in radians per second
double radpersec = 1;
// calculator rotation time and give absolute value
float rotation_time = std::abs(radians / radpersec);
// Use original function to set the rotation speed in mm/s
setRotationSpeed(radians);
// calculator rotation time and give absolute value
float rotation_time = std::abs(radians / radpersec);
// Sleep for the calculated rotation time
std::this_thread::sleep_for(std::chrono::milliseconds(static_cast<int>(rotation_time * 1000)));
// Use original function to set the rotation speed in mm/s
setRotationSpeed(radians);
// Stop the robot after the rotation
setRotationSpeed(0);
// Sleep for the calculated rotation time
std::this_thread::sleep_for(
std::chrono::milliseconds(static_cast<int>(rotation_time * 1000)));
// Stop the robot after the rotation
setRotationSpeed(0);
}
void CKobuki::robotSafety(std::string *pointerToMessage) {
while (true) {
if (parser.data.BumperCenter || parser.data.BumperLeft || parser.data.BumperRight ||
parser.data.CliffLeft || parser.data.CliffCenter || parser.data.CliffRight) {
std::cout << "Safety condition triggered!" << std::endl; // Debug print
*pointerToMessage = "estop";
forward(-100); // reverse the robot
}
std::this_thread::sleep_for(std::chrono::milliseconds(static_cast<int>(100)));
while (true) {
if (parser.data.BumperCenter || parser.data.BumperLeft ||
parser.data.BumperRight || parser.data.CliffLeft ||
parser.data.CliffCenter || parser.data.CliffRight) {
std::cout << "Safety condition triggered!" << std::endl; // Debug print
*pointerToMessage = "estop";
safetyActive = true;
forward(-100); // reverse the robot
safetyActive = false;
}
std::this_thread::sleep_for(std::chrono::milliseconds(static_cast<int>(100)));
}
}
void CKobuki::robotSafety() {
while (true) {
if (parser.data.BumperCenter || parser.data.BumperLeft || parser.data.BumperRight ||
parser.data.CliffLeft || parser.data.CliffCenter || parser.data.CliffRight) {
std::cout << "Safety condition triggered!" << std::endl; // Debug print
forward(-100); // reverse the robot
}
while (true) {
if (parser.data.BumperCenter || parser.data.BumperLeft ||
parser.data.BumperRight || parser.data.CliffLeft ||
parser.data.CliffCenter || parser.data.CliffRight) {
std::cout << "Safety condition triggered!" << std::endl; // Debug print
forward(-100); // reverse the robot
}
}
}
void CKobuki::sendNullMessage(){
void CKobuki::sendNullMessage() {
unsigned char message[11] = {
0xaa, // Start byte 1
0x55, // Start byte 2
0x08, // Payload length (the first 2 bytes dont count)
0x01, // payload type (0x01 = control command)
0x04, // Control byte or additional identifier
0x00, // Lower byte of speed value
0x00, // Upper byte of speed value
0x00, // Placeholder for radius
0x00, // Placeholder for radius
0x00 // Placeholder for checksum
};
unsigned char message[11] = {
0xaa, // Start byte 1
0x55, // Start byte 2
0x08, // Payload length (the first 2 bytes dont count)
0x01, // payload type (0x01 = control command)
0x04, // Control byte or additional identifier
0x00, // Lower byte of speed value
0x00, // Upper byte of speed value
0x00, // Placeholder for radius
0x00, // Placeholder for radius
0x00 // Placeholder for checksum
};
message[10] = message[2] ^ message[3] ^ message[4] ^ message[5] ^ message[6] ^
message[7] ^ message[8] ^ message[9];
message[10] = message[2] ^ message[3] ^ message[4] ^ message[5] ^ message[6] ^
message[7] ^ message[8] ^ message[9];
// Send the message
uint32_t pocet;
pocet = write(HCom, &message, 11);
}

View File

@@ -79,6 +79,7 @@ public:
void robotSafety(std::string *pointerToMessage);
void robotSafety(); //overload
void sendNullMessage();
bool safetyActive = false;
KobukiParser parser;

View File

@@ -49,6 +49,10 @@ std::string readMQTT()
void parseMQTT(std::string message)
{
if(robot.safetyActive){
std::cout << "Safety mode active. Ignoring command: " << message << std::endl;
return;
}
if (message == "up")
{
robot.forward(1024);