Gabriel/PFC
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Code: Slip correction

Browse Source
This commit is contained in:
Gabriel Lima
2023-10-13 17:44:53 -03:00
parent 7411cb5e79
commit 70cf89daa9
1 changed files with 9 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
Code/STM32/Core/Src/Components/Robot.cpp
View File

@@ -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