Need Probe Coding Help Marlin

Hello, I am not good enough in code to figure this out. I built a solenoid type probe. I have a MOSFET control a solenoid. When the gate pin is triggered from the mainboard, the solenoid is powered and the probe drops to the build plate. The print head moves down and the endstop is triggered with an optical sensor. After probing the print head moves farther down and a magnet grabs the probes rod. It "works" for the most part. The printer drops the probe, triggers the sensor correctly, and sometimes it successfully retracts, other times the probing goes fine and when it goes to retract it moves the Z axis way to far and the head crashes into the bed. I've messed around with probe code and I am just not good enough to figure it out. I have it configured as a Solenoid Probe with elements of the TouchMi Probe.

probe.h https://pastebin.com/KH0ntid9 probe.cpp https://pastebin.com/U94DurJE config.h https://pastebin.com/RCnDEqSV configadv.h https://pastebin.com/Y4jxxJzU