If you connect the 2nd CANBUS port to the tractor ISOBUS there is code in the standard Teensy to use the hitch height for work switch.
If you select “pressure sensor” the hitch height will show in the bar graph and the use the slider to set the height you want the work switch to trigger at.