diff --git a/Code/STM32/Core/Src/Components/Robot.cpp b/Code/STM32/Core/Src/Components/Robot.cpp index d4b7a1b..8830761 100644 --- a/Code/STM32/Core/Src/Components/Robot.cpp +++ b/Code/STM32/Core/Src/Components/Robot.cpp @@ -37,13 +37,21 @@ void Robot::controlCallback(){ //float desiredSpeed0 = strtof((const char*)rxBuffer, NULL); float desiredSpeed0; float desiredSpeed1; - sscanf((char*)rxBuffer, "%f,%f", &desiredSpeed0, &desiredSpeed1); // Desired speed in mm/s + uint32_t slipping; + sscanf((char*)rxBuffer, "%f,%f,%08X", &desiredSpeed0, &desiredSpeed1, &slipping); // Desired speed in mm/s float convertedSpeed0 = desiredSpeed0*CONVERSION; // Converted speed in rev/tick float convertedSpeed1 = desiredSpeed1*CONVERSION; #ifdef CONTROL_DISABLED motor0.setPower(desiredSpeed0); motor1.setPower(desiredSpeed1); #else + if(slipping){ + motor0.setEnabled(false); + motor1.setEnabled(false); + }else{ + motor0.setEnabled(true); + motor1.setEnabled(true); + } motor0.pid(convertedSpeed0); motor1.pid(convertedSpeed1); #endif