We now have a PID controller, huzzah!
M .hgignore +5 -0
@@ 2,6 2,11 @@ 
 glob:*.rs.bk
 Cargo.lock
 
+
 ^ebin/
 ^_build/
+^apps/.*/doc/
+config/vm.args.src
 glob:*.beam
+
+

          
M README.md +6 -0
@@ 45,6 45,12 @@ Possible alternatives:
 
 Use `rebar3`: <https://rebar3.org/>
 
+Install deps:
+
+```sh
+sudo apt-get install libopenblas-dev liblapacke-dev
+```
+
 Building:
 
  * `rebar3 compile`

          
A => apps/goatherd/src/alg_ekf.erl +12 -0
@@ 0,0 1,12 @@ 
+%% @doc An Extended Kalman Filter.
+%%
+%% Has no messaging or framework around it, just implements the core algorithm.
+%% Let's just make that work and then go from there.
+%%
+%% Math Stuff:
+%% https://github.com/tanguyl/numerl
+%% https://www.info.ucl.ac.be/~pvr/Brunet_26481700_Couplet_20371700_2022.pdf
+%%
+%%
+
+-module(ekf).

          
A => apps/goatherd/src/alg_pid.erl +46 -0
@@ 0,0 1,46 @@ 
+%% @doc A basic PID controller.
+%%
+%% (Proportional/Integral/Derivative, not process identifier.)
+%%
+%% Has no messaging or framework around it, just implements the core algorithm.
+%% Let's just make that work and then go from there.
+%% For now it only works with floating point vars, extending it to
+%% vectors or such is beyond this atm.
+
+-module(pid).
+-export([new_state/0, new_tuning/3, update/5, run/5]).
+
+-record(tuning, {p=0.0, i=0.0, d=0.0}).
+-record(pid_state, {last_err, accumulated_err}).
+
+new_state() ->
+    #pid_state{last_err = 0.0, accumulated_err = 0.0}.
+
+new_tuning(P, I, D) ->
+    #tuning {p = P, i = I, d = D}.
+
+%% @doc Takes an existing PID controller state, a timestamp (integer),
+%% and a new value for the process variable.  Computes a new state
+%% using the given tuning parameters and returns it.
+-spec update(#pid_state{}, #tuning{}, float(), float(), float()) -> {float(), #pid_state{}}.
+update(State, Tuning, ProcessVar, SetPoint, Dt) ->
+    %Error = ProcessVar - SetPoint,
+    Error = SetPoint - ProcessVar,
+    AccumulatedError = State#pid_state.accumulated_err + Error * Dt,
+    DerivativeError = (Error - State#pid_state.last_err) / Dt,
+    Output = (Error * Tuning#tuning.p) +
+             (AccumulatedError * Tuning#tuning.i) +
+             (DerivativeError * Tuning#tuning.d),
+    NewState = State#pid_state{last_err = Error, accumulated_err =
+                               AccumulatedError},
+    {Output, NewState}.
+
+run(Rounds, State, Tuning, ProcessVar, SetPoint) ->
+    if
+        Rounds == 0 -> {};
+        true -> {NewPv, NewState} = update(State, Tuning, ProcessVar,
+                                           SetPoint, 1),
+                io:format("Iteration ~p, PV: ~p State: ~p~n", [Rounds, NewPv,
+                                                               NewState]),
+                run(Rounds - 1, NewState, Tuning, NewPv, SetPoint)
+    end.

          
M apps/goatherd/src/gh_time.erl +6 -2
@@ 31,9 31,13 @@ 
 %% milliseconds.  The user may not assume that they know what the rate or
 %% resolution of the timer is.  Just that, as far as the robot is
 %% concerned, each message is "exact enough".
+%%
+%% TODO: This might be able to be turned into a gen_server, we'll see.
+%% https://www.erlang.org/doc/man/gen_server.html for reference, and
+%% https://www.erlang.org/doc/design_principles/gen_server_concepts.html
 -module(gh_time).
--export([init/0, start/0, start_link/0, raw_time/0, start_timeserver/0,
-        timeserver/1, timeserver_time/1]).
+-export([init/0, start/0, start_link/0, start_timeserver/0,
+         timeserver_time/1, timeserver/1]).
 
 %% @doc Start the timer service
 start() ->

          
M apps/goatherd/src/imu.erl +1 -1
@@ 3,7 3,7 @@ 
 -module(imu).
 -export([start/2]).
 
-% Starts producing GPS messages and sending them to
+% Starts producing IMU messages and sending them to
 % ParentPid at the given rate (in Hz)
 start(ParentPid, Hz) ->
     Ms = 1000 div Hz,

          
M apps/goatherd/src/localization.erl +13 -9
@@ 1,18 1,18 @@ 
-% Localization rewritten as a gen_server
-%
-% Also I guess forces me to learn how to write an extended
-% Kalman filter.  For now though it just does basically nothing
-% but remember the things sent to it.
+%% @doc Localization rewritten as a gen_server
+%%
+%% Also I guess forces me to learn how to write an extended
+%% Kalman filter.  For now though it just does basically nothing
+%% but remember the things sent to it.
 -module(localization).
 -behavior(gen_server).
 -export([init/1, handle_call/3, handle_cast/2,
          handle_info/2, terminate/2]).
 -export([start_link/0, send_gps/3, send_imu/2, get_state/1]).
 
-% State that the localization process trakcs.
-% Currently just position, velocity, acceleration.
-%
-% Position is in ECEF, I suppose?  Hmmm.  For now, sure.
+%% @doc State that the localization process trakcs.
+%% Currently just position, velocity, acceleration.
+%%
+%% Position is in ECEF, I suppose?  Hmmm.  For now, sure.
 -record(loc_state, {pos, vel, accel}).
 
 

          
@@ 50,12 50,16 @@ init([]) ->
     %_ImuDriver = spawn_link(imu, start, [ImuPid, ImuHz]),
     {ok, State}.
 
+%% @doc Get the current state from the localization system
 handle_call(get, _From, State) ->
     {reply, State, State}.
 
+
+%% @doc Receive a GPS message
 handle_cast({gps, Pos, Vel}, State) ->
     {noreply, State#loc_state{pos=Pos, vel=Vel}};
 
+%% @doc Receive an IMU message
 handle_cast({imu, Accel}, State) ->
     {noreply, State#loc_state{accel=Accel}}.
 

          
M apps/goatherd/src/rate_receiver.erl +13 -12
@@ 1,17 1,17 @@ 
-% A process that tries to get messages at a fixed
-% rate, and dies if it doesn't receive messages
-% fast enough.
-%
+%% @doc A process that tries to get messages at a fixed
+%% rate, and dies if it doesn't receive messages
+%% fast enough.  Sends the messages on to the parent process.
 
 -module(rate_receiver).
 -export([start/3]).
 
-% Starts a new thing that listens for messages.
-%
-% Any messages received get forwarded on to the ParentPid.
-% It must receive messages at least as fast
-% as the TargetHz, and will die if it misses MaxMissed,
-% messages in a row.
+%% @doc Starts a new thing that listens for messages.
+%%
+%% Any messages received get forwarded on to the ParentPid.
+%% It must receive messages at least as fast
+%% as the TargetHz, and will die if it misses MaxMissed,
+%% messages in a row.
+-spec start(integer(), integer(), pid) -> {}.
 start(TargetHz, MaxMissed, ParentPid) ->
     Ms = 1000 div TargetHz,
     % We wait for the first message before starting to do timing,

          
@@ 21,10 21,11 @@ start(TargetHz, MaxMissed, ParentPid) ->
     loop(Ms, MaxMissed, 0, ParentPid).
 
 
-% If our misses = MaxMissed, we die with a timeout.
+%% @private
+%% If our misses = MaxMissed, we die with a timeout.
 loop(_Ms, MaxMissed, MaxMissed, _ParentPid) ->
     exit(rate_receiver_timeout);
-% Otherwise we wait for a message
+%% Otherwise we wait for a message
 loop(Ms, MaxMissed, CurrentMissed, ParentPid) ->
     receive
         Any ->

          
M rebar.config +1 -0
@@ 2,6 2,7 @@ 
 {deps, [
     %{circuits_uart, "1.4.2"}
     %{circuits_uart, {elixir, "circuits_uart", "1.4.2"}}
+     {numerl, {git, "https://github.com/tanguyl/numerl.git", {branch, "master"}}}
 ]}.
 
 {relx, [{release, {goatherd, "0.1.0"},

          
M rebar.lock +4 -1
@@ 1,1 1,4 @@ 
-[].
+[{<<"numerl">>,
+  {git,"https://github.com/tanguyl/numerl.git",
+       {ref,"c0ca907ef54b05eb2e466a1de971361f10965086"}},
+  0}].