mirror of
https://gitlab.fdmci.hva.nl/technische-informatica-sm3/ti-projectten/rooziinuubii79.git
synced 2025-08-04 04:14:58 +00:00
attempt to make function so robot doenst throw itself from a cliff
This commit is contained in:
@@ -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);
|
||||
}
|
||||
|
@@ -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;
|
||||
|
||||
|
||||
|
Reference in New Issue
Block a user