diff --git a/PseudoSensorControl/PseudoSensorControl.ino b/PseudoSensorControl/PseudoSensorControl.ino index 50fd073..1d17bc0 100644 --- a/PseudoSensorControl/PseudoSensorControl.ino +++ b/PseudoSensorControl/PseudoSensorControl.ino @@ -4,8 +4,8 @@ float refs[4] = {10.83,10.83,10.83,10.83}; -Constants repelling = {40, 0.01, 7}; -Constants attracting = {20, 0.01, 20}; +Constants repelling = {10000, 0, 50000}; +Constants attracting = {10000, 0, 50000}; K_MAP consts = {repelling, attracting}; diff --git a/lib/PseudoSensorControl.cpp b/lib/PseudoSensorControl.cpp index 3b68923..18f7f94 100644 --- a/lib/PseudoSensorControl.cpp +++ b/lib/PseudoSensorControl.cpp @@ -39,10 +39,10 @@ void PseudoSensorController::sendOutputs() { void PseudoSensorController::control() { float avg = (Left.mmVal + Right.mmVal + Front.mmVal + Back.mmVal) * 0.25f; - float pseudos[4] = {Front.mmVal + Left.mmVal - avg, - Front.mmVal + Right.mmVal - avg, - Back.mmVal + Left.mmVal - avg, - Back.mmVal + Right.mmVal - avg}; + float pseudos[4] = {Front.mmVal + Left.mmVal - avg, // FL + Front.mmVal + Right.mmVal - avg, // FR + Back.mmVal + Left.mmVal - avg, // BL + Back.mmVal + Right.mmVal - avg}; // BR for (uint8_t i = 0; i < 4; i++) { float eCurr = Refs[i] - pseudos[i]; // Above reference is positive error.