Demo works!

GCode_Interpreterdc.py

@@ -48,11 +48,11 @@ class GCodeToMotors:
     Z_STEPS_PER_MM = z_units = 188.97
     Z_MOTOR_STEPS: int = 200
 
-    FAST_XY_FEEDRATE: float = 1 # in m/mn
+    FAST_XY_FEEDRATE: float = 60 # in m/mn
     FAST_Z_FEEDRATE: float = 1
 
-    CURVE_SECTION_INCHES = curve_section = .019685
-    CURVE_SECTION_MM: float = .5
+    CURVE_SECTION_INCHES = curve_section = 4*.019685
+    CURVE_SECTION_MM: float = .5*4
 
     SENSORS_INVERTING: bool = False
 
@@ -181,8 +181,9 @@ class GCodeToMotors:
             match code:
                 case 0 | 1:
                     self.set_target(fp[0], fp[1], fp[2])
-                    if has_command('G', instruction) and code == 1:
+                    if has_command('F', instruction) and code == 1:
                         self.feedrate = search_string('F', instruction)
+                        print("set feedrate to", self.feedrate)
                     if self.feedrate == 0:
                         self.feedrate = self.get_max_speed()
                     self.move()
@@ -198,16 +199,22 @@ class GCodeToMotors:
                     bY = fp[1] - center[1]
 
                     if code == 2:  # If in fucked up anti-trigonometric direction
-                        angleA = np.atan2(bY, bX)
-                        angleB = np.atan2(aY, aX)
+                        angleA = np.arctan2(bY, bX)
+                        angleB = np.arctan2(aY, aX)
                     else:
-                        angleA = np.atan2(aY, aX)
-                        angleB = np.atan2(bY, bX)
+                        angleA = np.arctan2(aY, aX)
+                        angleB = np.arctan2(bY, bX)
 
                     if angleB <= angleA:
                         angleB += 2 * np.pi
                     angle = angleB - angleA
 
+                    if has_command('F', instruction):
+                        self.feedrate = search_string('F', instruction)
+                        print("set feedrate to", self.feedrate)
+                    if self.feedrate == 0:
+                        self.feedrate = self.get_max_speed()
+
                     radius = np.linalg.norm([aX, aY])
                     length = radius * angle
                     steps = int(length / self.curve_section) + 1

controller.py

@@ -52,11 +52,12 @@ class PIDController(Controller):
 
 
 class DummyController(Controller):
+    last_delta = np.nan
+
     def __init__(self):
         super().__init__()
 
     def __call__(self, gtm, *args, **kwargs):
-        print("Call Me")
 
         feed = gtm.feedrate
         dx, dy, dz = gtm.delta_units
@@ -89,8 +90,16 @@ class DummyController(Controller):
 
         # If we are close enough, we're good
         if(gtm.x_throttle == 0 and gtm.y_throttle == 0 and gtm.z_throttle == 0):
+            last_delta = np.nan
             return True
 
+        delta = (dx**2 + dy**2 + dz**2)**.5
+        if(delta > self.last_delta and self.last_delta < 2.0):
+            self.last_delta = np.nan
+            return True
+
+        self.last_delta = delta
+
         print("Expected move (mm/call)", gtm.ctrl_step*feed*1000./60)
         print("Delta (mm) ", (dx**2 + dy**2 + dz**2)**.5) 
-        return (dx**2+dy**2+dz**2)**.5 <= 1.
+        return False

main.py

@@ -6,14 +6,12 @@ b = "socket.sock"
 sh = hw.SimuHardware(b)
 ctrl = ctl.DummyController()
 gtm1 = gci.GCodeToMotors(ctrl, sh)
-gtm1.instruction_converter("G0 X25.000 Y25.000 F0.5000")
-gtm1.instruction_converter("G0 X30.000 Y25.000 F0.5000")
-gtm1.instruction_converter("G2 X30.000 Y45.000 I0 J10 F0.5000")
+gtm1.instruction_converter("G1 X25.000 Y25.000 F2.000")
+gtm1.instruction_converter("G1 X30.000 Y25.000 F2.000")
+gtm1.instruction_converter("G3 X30.000 Y45.000 I0 J10 F0.100")
 #  gtm1.instruction_converter("G5 X50.000 Y45.000 I-5 J-5 P10 J5 F0.5000")
-gtm1.instruction_converter("G0 X50.000 Y50.000 F0.5000")
-gtm1.instruction_converter("G0 X50.000 Y200.000 F0.5000")
-gtm1.instruction_converter("G0 X200.000 Y200.000 F0.5000")
-gtm1.instruction_converter("G30 X200.000 Y50.000 F0.5000")
-
-while True:
-    pass
+gtm1.instruction_converter("G1 X50.000 Y50.000 F2.000")
+gtm1.instruction_converter("G1 X50.000 Y100.000 F2.000")
+gtm1.instruction_converter("G1 X100.000 Y-50.000 F2.000")
+gtm1.instruction_converter("G1 X-50.000 Y-100.000 F2.000")
+gtm1.instruction_converter("G1 X-100.000 Y50.000 F2.000")

simulator.py

@@ -116,7 +116,7 @@ class NaiveSimulator(Simulator, Douche):
     # which can be expressed as
     # Jv' = (R*Gamma)(motor) - f0*v
 
-    J = np.array([1., 1., 1.]) # Moment of inertia vector
+    J = np.array([.1, .1, .1]) # Moment of inertia vector
     f = np.array([0., 0., 0.]) # viscous friction coefficient
     G = np.array([1., 1., 1.]) # Motor gain : Gamma*R = G * delta_v
     # FIXME: this is not very realistic, this looks like a model of