attempt to make function so robot doenst throw itself from a cliff

This commit is contained in:
2024-10-20 17:35:55 +02:00
parent 856d60eea2
commit b5f5491222
2 changed files with 36 additions and 1 deletions

View File

@@ -586,6 +586,9 @@ void CKobuki::goToXy(long double xx, long double yy)
return;
}
/// @brief Makes the Kobuki go forward
/// @param speedvalue speed of robot in mm/s
/// @param distance distance in meters
void CKobuki::forward(int speedvalue, long double distance) {
// Use the goStraight logic to determine the speed and distance
@@ -615,6 +618,8 @@ message[10] = message[2] ^ message[3] ^ message[4] ^ message[5] ^ message[6] ^
pocet = write(HCom, &message, 11);
}
/// @brief Makes the kobuki rotate
/// @param degrees Rotation in degrees
void CKobuki::Rotate(int degrees) {
// convert raidans to degrees
@@ -625,7 +630,7 @@ void CKobuki::Rotate(int degrees) {
// 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);
@@ -636,3 +641,31 @@ void CKobuki::Rotate(int degrees) {
setRotationSpeed(0);
}
void CKobuki::robotSafety(){
while(true){
if (parser.data.BumperCenter || parser.data.BumperLeft || parser.data.BumperRight || parser.data.CliffLeft || parser.data.CliffCenter || parser.data.CliffRight)
{
sendNullMessage();
forward(100, -0.5);
};
};
}
void CKobuki::sendNullMessage(){
unsigned char message[5] = {
0xaa, // Start byte 1
0x55, // Start byte 2
0x00, // Payload length (the first 2 bytes dont count)
0x00 // Placeholder for checksum
};
// Calculate checksum
message[4] = message[2] ^ message[3];
// Send the message
uint32_t pocet;
pocet = write(HCom, &message, 5);
}

View File

@@ -76,6 +76,8 @@ public:
void goToXy(long double xx, long double yy);
void Rotate(int degrees);
std::ofstream odometry_log;
void robotSafety();
void sendNullMessage();
KobukiParser parser;