encoder, first commit

Author: previ

coderbot.cfg

@@ -1,2 +1 @@
 {"move_tr_speed": "80", "move_fw_elapse": "1", "camera_color_object_size_min": "4000", "camera_path_object_size_min": "4000", "load_at_start": "", "move_tr_elapse": "0.5", "sound_stop": "$shutdown.mp3", "show_control_move_commands": "true", "prog_level": "adv", "prog_scrollbars": "true", "move_fw_speed": "100", "camera_color_object_size_max": "160000", "sound_shutter": "$shutter.mp3", "show_page_prefs": "true", "cv_image_factor": "4", "ctrl_hud_image": "", "button_func": "none", "ctrl_fw_elapse": "-1", "ctrl_tr_elapse": "-1", "move_power_angle_2": "60", "move_power_angle_3": "60", "move_power_angle_1": "45", "move_motor_trim": "1", "show_page_program": "true", "sound_start": "$startup.mp3", "camera_exposure_mode": "auto", "ctrl_tr_speed": "80", "prog_move_mpu": "yes", "ctrl_fw_speed": "100", "camera_refresh_timeout": "0.1", "camera_jpeg_quality": "20", "prog_maxblocks": "-1", "move_motor_mode": "dc", "camera_path_object_size_max": "160000", "show_page_control": "true"}
-{"move_tr_speed": "80", "move_fw_elapse": "1", "camera_color_object_size_min": "4000", "camera_path_object_size_min": "4000", "load_at_start": "", "move_tr_elapse": "0.5", "sound_stop": "$shutdown.mp3", "prog_video_rec": "true", "show_control_move_commands": "true", "prog_level": "adv", "prog_scrollbars": "true", "move_fw_speed": "100", "camera_color_object_size_max": "160000", "sound_shutter": "$shutter.mp3", "show_page_prefs": "true", "cv_image_factor": "4", "ctrl_hud_image": "", "button_func": "none", "ctrl_fw_elapse": "-1", "ctrl_tr_elapse": "-1", "move_power_angle_2": "60", "move_power_angle_3": "60", "move_power_angle_1": "40", "move_motor_trim": "1.1", "show_page_program": "true", "sound_start": "$startup.mp3", "camera_exposure_mode": "auto", "ctrl_tr_speed": "80", "ctrl_fw_speed": "100", "camera_refresh_timeout": "0.1", "camera_jpeg_quality": "20", "prog_maxblocks": "-1", "move_motor_mode": "dc", "camera_path_object_size_max": "160000", "show_page_control": "true"}

coderbot.py

@@ -19,6 +19,7 @@
 
 import os
 import time
+import threading
 import pigpio
 import config
 import logging
@@ -40,8 +41,8 @@
 PIN_SONAR_2_ECHO = 8
 PIN_SONAR_3_TRIGGER = 18
 PIN_SONAR_3_ECHO = 23
-PIN_ENCODER_LEFT = 14
-PIN_ENCODER_RIGHT = 15
+PIN_ENCODER_LEFT = 15
+PIN_ENCODER_RIGHT = 14
 
 PWM_FREQUENCY = 100 #Hz
 PWM_RANGE = 100 #0-100
@@ -82,7 +83,11 @@ def __init__(self, servo=False, motor_trim_factor=1.0):
     self._encoder_cur_right = 0
     self._encoder_target_left = -1
     self._encoder_target_right = -1
-    self._ag = mpu.AccelGyro()
+    self._encoder_k_s_1 = 20
+    self._encoder_k_v_1 = 80
+    self._encoder_sem = threading.Condition()
+   
+    #self._ag = mpu.AccelGyro()
 
   the_bot = None
 
@@ -99,11 +104,13 @@ def get_instance(cls, servo=False, motor_trim_factor=1.0):
       cls.the_bot = CoderBot(servo, motor_trim_factor)
     return cls.the_bot
 
-  def move(self, speed=100, elapse=-1):
-    self.motor_control(speed_left=min(100, max(-100, speed * self._motor_trim_factor)), speed_right=min(100, max(-100, speed / self._motor_trim_factor)), elapse=elapse)
+  def move(self, speed=100, elapse=-1, steps_left=-1, steps_right=-1):
+    self.motor_control(speed_left=min(100, max(-100, speed * self._motor_trim_factor)), speed_right=min(100, max(-100, speed / self._motor_trim_factor)), elapse=elapse, steps_left=steps_left, steps_right=steps_right)
 
-  def turn(self, speed=100, elapse=-1):
-    self.motor_control(speed_left=min(100, max(-100, speed / self._motor_trim_factor)), speed_right=-min(100, max(-100, speed * self._motor_trim_factor)), elapse=elapse)
+  def turn(self, speed=100, elapse=-1, steps=-1):
+    steps_left = steps
+    steps_right = -steps if steps > 0 else -1
+    self.motor_control(speed_left=min(100, max(-100, speed / self._motor_trim_factor)), speed_right=-min(100, max(-100, speed * self._motor_trim_factor)), elapse=elapse, steps_left=steps_left, steps_right=steps_right)
 
   def turn_angle(self, speed=100, angle=0):
     z = self._ag.get_gyro_data()['z']
@@ -139,7 +146,17 @@ def _dc_motor(self, speed_left=100, speed_right=100, elapse=-1, steps_left=-1, s
     self._encoder_cur_right = 0
     self._encoder_target_left = steps_left
     self._encoder_target_right = steps_right
-    
+    self._encoder_dir_left = cmp(speed_left, 0)
+    self._encoder_dir_right = cmp(speed_right, 0)
+    self._encoder_last_tick_time_left = 0
+    self._encoder_last_tick_time_right = 0
+    self._encoder_motor_stopping_left = False
+    self._encoder_motor_stopping_right = False
+    self._encoder_motor_stopped_left = False
+    self._encoder_motor_stopped_right = False
+    if steps_left > 0 or steps_right > 0:
+      self._encoder_sem.acquire()
+
     self._is_moving = True
     if speed_left < 0:
       speed_left = abs(speed_left)
@@ -162,7 +179,11 @@ def _dc_motor(self, speed_left=100, speed_right=100, elapse=-1, steps_left=-1, s
       time.sleep(elapse)
       self.stop()
 
-  def _servo_motor(self, speed_left=100, speed_right=100, elapse=-1):
+    if steps_left > 0 or steps_right > 0:
+      self._encoder_sem.wait()
+      self._encoder_sem.release()
+
+  def _servo_motor(self, speed_left=100, speed_right=100, elapse=-1, steps_left=-1, steps_right=-1):
     self._is_moving = True
     speed_left = -speed_left
 
@@ -206,16 +227,56 @@ def set_callback(self, gpio, callback, elapse):
     self._cb_last_tick[gpio] = 0
 
   def callback(self, gpio, level, tick):
-    if gpio == PIN_ENCODER_LEFT:
+    if gpio == PIN_ENCODER_LEFT and self._encoder_target_left >= 0:
       self._encoder_cur_left += 1
-      if self._encoder_target_left >= self._encoder_cur_left:
+      delta_ticks_left = tick - self._encoder_last_tick_time_left if tick > self._encoder_last_tick_time_left else tick - self._encoder_last_tick_time_left + 4294967295
+      self._encoder_last_tick_time_left = tick
+      self._encoder_speed_left = 1000000.0 / delta_ticks_left #convert speed in steps per second
+      delta_s = self._encoder_speed_left / self._encoder_k_s_1 #delta_s is the delta (in steps)before the target  to reverse the motor in order to arrive at target
+      if (self._encoder_cur_left >= self._encoder_target_left - delta_s and 
+          not self._encoder_motor_stopping_left):
+        self._encoder_motor_stopping_left = True
+        if self._encoder_dir_left > 0:
+          self.pi.write(PIN_LEFT_FORWARD, 0)
+          self.pi.set_PWM_dutycycle(PIN_LEFT_BACKWARD, 100)
+        else:
+          self.pi.set_PWM_dutycycle(PIN_LEFT_FORWARD, 100) 
+	  self.pi.write(PIN_LEFT_BACKWARD, 0)
+      elif (self._encoder_motor_stopped_left == False and
+            ((self._encoder_motor_stopping_left and 
+             self._encoder_speed_left < self._encoder_k_v_1) or
+            (self._encoder_motor_stopping_left and
+             self._encoder_cur_left >= self._encoder_target_left))):
         self.pi.write(PIN_LEFT_FORWARD, 0)
-	self.pi.write(PIN_LEFT_BACKWARD, 0)
-    elif gpio == PIN_ENCODER_RIGHT:
+        self.pi.write(PIN_LEFT_BACKWARD, 0)
+        self._encoder_motor_stopped_left = True
+        self._encoder_check_stopped_and_notify()
+        print "LEFT: " + str(self._encoder_cur_left) + " speed: " + str(self._encoder_speed_left) 
+    elif gpio == PIN_ENCODER_RIGHT and self._encoder_target_right >= 0:
       self._encoder_cur_right += 1
-      if self._encoder_target_right >= self._encoder_cur_right:
+      delta_ticks_right = tick - self._encoder_last_tick_time_right if tick > self._encoder_last_tick_time_right else tick - self._encoder_last_tick_time_right + 4294967295
+      self._encoder_last_tick_time_right = tick
+      self._encoder_speed_right = 1000000.0 / delta_ticks_right #convert speed in steps per second
+      delta_s = self._encoder_speed_right / self._encoder_k_s_1 #delta_s is the delta (in steps)before the target  to reverse the motor in order to arrive at target
+      if (self._encoder_cur_right >= self._encoder_target_right - delta_s and
+          not self._encoder_motor_stopping_right):
+        self._encoder_motor_stopping_right = True
+        if self._encoder_dir_right > 0:
+          self.pi.write(PIN_RIGHT_FORWARD, 0)
+          self.pi.set_PWM_dutycycle(PIN_RIGHT_BACKWARD, 100)
+        else:
+          self.pi.set_PWM_dutycycle(PIN_RIGHT_FORWARD, 100)
+          self.pi.write(PIN_RIGHT_BACKWARD, 0)
+      elif (self._encoder_motor_stopped_right == False and
+            ((self._encoder_motor_stopping_right and 
+             self._encoder_speed_right < self._encoder_k_v_1) or
+            (self._encoder_motor_stopping_right and
+             self._encoder_cur_right >= self._encoder_target_right))):
         self.pi.write(PIN_RIGHT_FORWARD, 0)
-	self.pi.write(PIN_RIGHT_BACKWARD, 0)
+        self.pi.write(PIN_RIGHT_BACKWARD, 0)
+        self._encoder_motor_stopped_right = True
+        self._encoder_check_stopped_and_notify()
+        print "RIGHT: " + str(self._encoder_cur_right) + " speed: " + str(self._encoder_speed_right)
     else:
       cb = self._cb.get(gpio)
       if cb:
@@ -226,6 +287,12 @@ def callback(self, gpio, level, tick):
           self._cb_last_tick[gpio] = tick
           print "pushed: ", level, tick
           cb()
+ 
+  def _encoder_check_stopped_and_notify(self):
+    if self._encoder_motor_stopped_left and self._encoder_motor_stopped_right:
+      self._encoder_sem.acquire()
+      self._encoder_sem.notify()
+      self._encoder_sem.release()
 
   def halt(self):
     os.system ('sudo halt')

main.py

@@ -323,6 +323,7 @@ def run_server():
       logging.error(e)
 
     bot.set_callback(PIN_PUSHBUTTON, button_pushed, 100)
+    print "run"
     app.run(host="0.0.0.0", port=8080, debug=True, use_reloader=False, threaded=True)
   finally:
     if cam: