Fight Detection in Software

Hardware Autosteer
19

The complete guide for inserting this code into the standard AIO v4 firmware:

In autosteer.ino, below the #define and #include declarations (line 63), add this code:

float steerAngleLast = 0;           //For fight detection
float angleDeriv = 0;               //Fight detection - steering angle rate of change
float slippage = 0;                 //Output for the comparison function
int steerKillCount = 0;     
float setPointDeriv = 0;
float setPointLast = 0;
elapsedMillis steerEnableTimer;

Then delete the contents of the if (steerConfig.CurrentSensor) conditional block (lines 380-392), and add the following code so it looks like this:

if (steerConfig.CurrentSensor)
  {     
      angleDeriv = abs(steerAngleActual - steerAngleLast);                            //Fight Detection code - Calculate rate of change of the steering angle
      setPointDeriv = abs(steerAngleSetPoint - setPointLast);                         //Is the setpoint moving fast?
      slippage = abs((angleDeriv * (steerConfig.PulseCountMax * 0.1)) - abs(pValue));      //Compare pValue to change in steering rate. Multiply by kP to make a reasonable slippage factor
      sensorReading = constrain(slippage, 0, 255);                                    //Send slippage to AOG
      if ((slippage > 200) && steerEnableTimer > 3000) steerKillCount++;              //If slippage is over threshold, and we're more than 3s into steering
      if (slippage < (180)) steerKillCount = steerKillCount * 0.5;                    //If slippage returns to normal, decay.
      if (setPointDeriv > 1) steerKillCount = steerKillCount * 0.5;                   //Follow the setpoint instead of kicking out
      if (steerKillCount > 4)                                                         //If slippage is not just a spurious value, and there is no step response, cut steering.
      {     
        watchdogTimer = WATCHDOG_FORCE_VALUE; 
        steerKillCount = 0;
        steerAngleError = 0;
        pwmDrive = 0;
      }
      steerAngleLast = steerAngleActual;
      setPointLast = steerAngleSetPoint;
    }

And add the steerEnableTimer = 0; line in this block like so (line 350):

if (reading == LOW && previous == HIGH)
    {
        if (currentState == 1)
        {
            currentState = 0;
            steerSwitch = 0;
        }
        else
        {
            steerEnableTimer = 0;
            currentState = 1;
            steerSwitch = 1;
        }
    }
    previous = reading;

Now when you enable Current Sensor in AOG, set the counts slider to 50%. This value adjusts the left/right bias of the system, to make sure the system disengages equally no matter which way you turn the wheel.

In order for this code to work properly, the steering will need to be fully set up first. This is the last adjustment to make, after you are happy with how the machine is steering. Spend time getting your proportional gain, minimum to move and max limits dialled in.

To increase the sensitivity, you can increase the proportional gain slightly, or reduce max limit.
In general use, you will find the green bar sitting at around 20%, and it will shoot up to 99% when you make a swift and deliberate motion on the steering wheel. The steering will then cut out.

the main variable to adjust is max limit, as this puts a ceiling on the amount the valve can fight your actions, and therefore allows the code to discern your input.
Using one of my valves from cseq.co.uk, I tend to set P gain to 50, min to move at 60, and max limit around 110.

4 Likes