Lolibot: Add 'servo' and 'motors' commands

Support directly settings motor PWM controls
motors (1023 0 1023 0) to go forward

Support servo command 'servo [-100..100] for -100
(backward spin) to 100 (forward spin) and 0 for stop

software/lib/lolibot.py

@@ -11,7 +11,15 @@
 #
 # MQTT commands
 # ~~~~~~~~~~~~~
+# Topic: /in  {forward|stop|left|right|reverse}
+#    - set the motors to a particular motion
 # Topic: /in  freq FREQUENCY
+#    - Set the motor PWM frequency
+# Topic: /in  motors (A1 B2 A2 B2)
+#    - Set a particular PWM config directly on the motor pins
+#      for finer control
+# Topic: /in servo [-100..100]
+#    - Set the servo motion from -100 to 100 (0 = stopped)
 #
 # REPL testing
 # ~~~~~~~~~~~~
@@ -32,6 +40,7 @@
 left_motor2 = None
 right_motor1 = None
 right_motor2 = None
+servo = None
 
 duty_cycle_max = 1023
 duty_cycle_min = 200
@@ -53,6 +62,12 @@ def motor_action(motor_command):
   right_motor1.duty(motor_command[2])
   right_motor2.duty(motor_command[3])
 
+def servo_pos(servo_duty):
+  servo.duty(servo_duty)
+
+def map_vals(val, min_in, max_in, min_out, max_out):
+   return (val-min_in)/(max_in-min_in)*(max_out-min_out)+min_out
+
 def on_message_lolibot(topic, payload_in):
   global pwm_frequency
 
@@ -67,6 +82,16 @@ def on_message_lolibot(topic, payload_in):
     print("motor freq: " + str(pwm_frequency))
     return True
 
+  if len(tokens) == 2 and tokens[0] == "servo":
+    servo_position = map_vals(int(tokens[1]), -100, 100, 115, 40)
+    print("Servo control to : " + str(servo_position))
+    servo_pos(int (servo_position))
+    return True
+
+  if tokens[0] == "motors":
+    print("motor settings: " + tokens[1:])
+    motor_action(tokens[1:])
+
   return False
 
 def initialise_motor(settings, motor_pin_name):
@@ -78,6 +103,7 @@ def initialise_motor(settings, motor_pin_name):
 def initialise(settings):
   global duty_cycle_max, duty_cycle_min, pwm_frequency
   global left_motor1, left_motor2, right_motor1, right_motor2
+  global servo
   global i2c_bus
 
   if "duty_cycle_max" in settings:
@@ -92,7 +118,10 @@ def initialise(settings):
   right_motor1 = initialise_motor(settings, "right_motor_pin1")
   right_motor2 = initialise_motor(settings, "right_motor_pin2")
 
-  servo = Pin(settings["servo_pin"], Pin.OUT)
+  servo_pin = Pin(settings["servo_pin"], Pin.OUT)
+  servo = PWM(servo_pin)
+  servo.freq(50) # Set 50Hz PWM
+  servo.duty(77) # Off
 
   scl = settings["scl_pin"]
   sda = settings["sda_pin"]