I attempted to build a simulation of the robot, and eventually the controller software in Matlab. The goal is to make all of the computations as modular as possible by using symbols from DH Parameters where possible.
The current working modules (or functions) are:
Generalised Forward Kinematics using DH Parameters.
Generalised Inverse Kinematics using DH Parameters.
Trajectory planning Linear and Arc (for G01 and G02 GCODE).
Scrap GCODE parser.
Generalised Jacobian calculation and inverse Jacobian calculation (closed form solution).
The simulation uses RoboAnalyzer VRM at http://www.roboanalyzer.com/vrm-rtb.html. This helps to accelerate the testing as the graphics in Matlab is very slow.