mirror of
https://gitlab.fdmci.hva.nl/technische-informatica-sm3/ti-projectten/rooziinuubii79.git
synced 2025-08-04 12:24:57 +00:00
Refactor safety checks and improve message handling in CKobuki class
This commit is contained in:
@@ -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);
|
||||
}
|
||||
|
||||
|
@@ -79,6 +79,7 @@ public:
|
||||
void robotSafety(std::string *pointerToMessage);
|
||||
void robotSafety(); //overload
|
||||
void sendNullMessage();
|
||||
bool safetyActive = false;
|
||||
KobukiParser parser;
|
||||
|
||||
|
||||
|
@@ -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);
|
||||
|
Reference in New Issue
Block a user