Putting safety system under git control

robotlab_safety/robot.fps

+LengthOK(const max_length,length_0_0,length_0_1,length_1_0,length_1_1,length_2_0,length_2_1,length_3_0,length_3_1,length_4_0,length_4_1,out bits_ok, out_reg length)
+descr.en Uses length_*_1 bits as a binary coded value of 
+descr.en length (in 100 mm increments) and chacks that it is 
+descr.en less than max_length.
+descr.en
+descr.en For each length_* bit, either length_*_0 or length_*_1
+descr.en has to be connected, but not both.
+{ 
+  !Result = length_ok
+  !r = encoded
+  !m = tmp1
+  !m = tmp2
+
+  ; Check that each length bit is either 0 or 1
+  tmp1 = (length_0_0 + length_0_1) * (length_1_0 + length_1_1) * (length_2_0 + length_2_1) * (length_3_0 + length_3_1) * (length_4_0 + length_4_1)
+  tmp2 = (length_0_0 * length_0_1) + (length_1_0 * length_1_1) + (length_2_0 * length_2_1) + (length_3_0 * length_3_1) + (length_4_0 * length_4_1)
+
+  bits_ok = tmp1 * /tmp2
+
+  ; Calculate encoded length (in 100 mm increments)
+  (encoded=0)
+  (encoded+=100) = length_0_1
+  (encoded+=200) = length_1_1
+  (encoded+=400) = length_2_1
+  (encoded+=800) = length_3_1
+  (encoded+=1600) = length_4_1
+  (length=encoded)
+
+  ; Length is OK if all bits are correctly wired and the
+  ; encoded length is less than the maximum length
+  Result = bits_ok * (length < max_length)
+}