What I do is cut the pocket slightly smaller without G41/G42, then do a finishing pass using G41/G42.
The important thing is the lead in. Brian has a post here with comp rules that will apply in the next version of Mach3. I believe you need to apply comp during a straight move, not an arc.
I glanced at your code, and don't have the time to decipher it.

I use a VBA macro in AutoCAD to create my code, and this is how I do it. The example is a 1" circle centered at 0,0. I do a short move to apply the comp above the part, then ramp in along an arc that's 1.5 times the tool diameter. I ramp out along the arc, then turn off comp. This may not work if the circles too small with respect to the cutter, but it works for what I do.
G20 G40 G64 G90 G91.1
(1/4" bit)
M3
F100
G0 Z0.5
G0 X-0.25 Y-0.125 Z0.5
G42P0.125
G1 X-0.375 Y0.125 Z0.0
G2 X0.0 Y0.5 Z0.0 I0.375 J0.0
G2 X0.0 Y0.5 Z-0.1 I0.0 J-0.5
G2 X0.0 Y0.5 Z-0.1 I0.0 J-0.5
G2 X0.5 Y0.0 Z0.5 I0.0 J-0.5
G40
G1 X0.2 Y0.0 Z0.5 I0.0 J-0.5
G0 Z3
M5
M30