Got solver to work for initial calibration
Halfway done debugging double solver
Full test of double solver