mirror of
https://gitlab.fdmci.hva.nl/technische-informatica-sm3/ti-projectten/rooziinuubii79.git
synced 2025-08-03 20:04:58 +00:00
fixed safety of robot and that it always drives when its told to drive
This commit is contained in:
@@ -589,7 +589,7 @@ void CKobuki::goToXy(long double xx, long double yy)
|
||||
/// @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) {
|
||||
void CKobuki::forward(int speedvalue) {
|
||||
// Use the goStraight logic to determine the speed and distance
|
||||
|
||||
// Calculate the actual speed and radius values based on the conversion table
|
||||
@@ -616,6 +616,8 @@ message[10] = message[2] ^ message[3] ^ message[4] ^ message[5] ^ message[6] ^
|
||||
// Send the message
|
||||
uint32_t pocet;
|
||||
pocet = write(HCom, &message, 11);
|
||||
pocet = write(HCom, &message, 11);
|
||||
|
||||
}
|
||||
|
||||
/// @brief Makes the kobuki rotate
|
||||
@@ -644,28 +646,37 @@ void CKobuki::Rotate(int degrees) {
|
||||
|
||||
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);
|
||||
};
|
||||
};
|
||||
|
||||
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(){
|
||||
|
||||
unsigned char message[5] = {
|
||||
unsigned char message[11] = {
|
||||
0xaa, // Start byte 1
|
||||
0x55, // Start byte 2
|
||||
0x00, // Payload length (the first 2 bytes dont count)
|
||||
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
|
||||
};
|
||||
|
||||
// Calculate checksum
|
||||
message[4] = message[2] ^ message[3];
|
||||
|
||||
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, 5);
|
||||
pocet = write(HCom, &message, 11);
|
||||
}
|
||||
|
@@ -71,7 +71,7 @@ public:
|
||||
|
||||
// control functions
|
||||
void goStraight(long double distance);
|
||||
void forward(int speedvalue, long double distance);
|
||||
void forward(int speedvalue);
|
||||
void doRotation(long double th);
|
||||
void goToXy(long double xx, long double yy);
|
||||
void Rotate(int degrees);
|
||||
|
@@ -15,13 +15,11 @@ int main()
|
||||
unsigned char *null_ptr(0);
|
||||
robot.startCommunication("/dev/ttyUSB0", true, null_ptr);
|
||||
sleep(1);
|
||||
// thread mv(movement);
|
||||
// mv.join(); //only exit once thread one is done running
|
||||
// checkCenterCliff();
|
||||
// logToFile();
|
||||
//seperate thread so sleep doesnt block main thread
|
||||
// thread logger(logToFile);
|
||||
robot.Rotate(90);
|
||||
std::thread safety([&robot]() { robot.robotSafety(); }); // use a lambda function to call the member function
|
||||
safety.detach();
|
||||
thread movementThread(movement);
|
||||
|
||||
movementThread.join();
|
||||
return 0;
|
||||
}
|
||||
|
||||
@@ -38,11 +36,13 @@ int movement()
|
||||
int text;
|
||||
while (true)
|
||||
{
|
||||
cout << "gimme input: ";
|
||||
cin >> text;
|
||||
|
||||
if (text == 1)
|
||||
{
|
||||
robot.forward(1024, 1);
|
||||
robot.forward(400);
|
||||
|
||||
}
|
||||
else if (text == 2)
|
||||
{
|
||||
|
Reference in New Issue
Block a user