SHARE
TWEET

Untitled

Alekal Dec 16th, 2018 68 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. import rospy
  2. from PiBot import PiBot
  3.  
  4.  
  5. class Robot:
  6.     """
  7.     The three primitives robotic paradigm: sense, plan, act [1].
  8.     Sense - gather information using the sensors
  9.     Plan - create a world model using all the information and plan the next move
  10.     Act - carry out the next step of the plan using actuators
  11.     [1] https://en.wikipedia.org/wiki/Robotic_paradigm
  12.     """
  13.     def __init__(self):
  14.         print("Initializing...")
  15.         self.robot = PiBot()
  16.         self.start_timestamp = rospy.get_time()
  17.         self.problem_solved = False
  18.         self.left_speed = 0
  19.         self.scan_speed = 14
  20.         self.right_speed = 0
  21.         self.speed = 14
  22.         self.mid = self.robot.get_front_middle_ir()
  23.  
  24.     def calculate_real_speed(self):
  25.         left_encoder_0 = self.robot.get_left_wheel_encoder()
  26.         right_encoder_0 = self.robot.get_right_wheel_encoder()
  27.         drive = True
  28.         while drive:
  29.             left_encoder_new = self.robot.get_left_wheel_encoder()
  30.             right_encoder_new = self.robot.get_right_wheel_encoder()
  31.             left_error = left_encoder_new - left_encoder_0
  32.             right_error = right_encoder_new - right_encoder_0
  33.             error = right_error - left_error
  34.             left_encoder_0 = self.robot.get_left_wheel_encoder()
  35.             right_encoder_0 = self.robot.get_right_wheel_encoder()
  36.             self.left_speed = (int(self.speed + round((error / 2) / self.speed)))
  37.             self.right_speed = (int(self.speed - round((error / 2) / self.speed)))
  38.             return self.left_speed, self.right_speed
  39.  
  40.     def buffer(self, speed, turn):  # scans for object, returns dict of left coder and seen distance
  41.         count = 1
  42.         results = {}
  43.         buffer = [0, 0, 0]
  44.         last_ok_value = 0
  45.         turning = True
  46.         print(turn)
  47.         left_encoder_before_looking = self.robot.get_left_wheel_encoder()
  48.         right_encoder_before_looking = self.robot.get_right_wheel_encoder()
  49.         # print(left_encoder_before_looking)
  50.         # print(right_encoder_before_looking)
  51.         rospy.sleep(1)
  52.         while turning is True:
  53.             # print('left: ' + str(self.robot.get_left_wheel_encoder()))
  54.             # print(left_encoder_before_looking + turn)
  55.             # print(right_encoder_before_looking + turn)
  56.             print(self.robot.get_right_wheel_encoder())
  57.             print(self.robot.get_left_wheel_encoder())
  58.             self.robot.set_left_wheel_speed(speed)
  59.             self.robot.set_right_wheel_speed(-speed)
  60.             encoder = self.robot.get_left_wheel_encoder()
  61.             distance = self.robot.get_front_middle_ir()
  62.             if count == 1:
  63.                 buffer = [distance] * 3
  64.                 last_ok_value = distance
  65.             count += 1
  66.             buffer = buffer[1:]
  67.             buffer.append(distance)
  68.             # print(buffer)
  69.             if self.robot.get_left_wheel_encoder() == turn + left_encoder_before_looking:
  70.                 turning = False
  71.             if abs(distance - round(sum(buffer) / 3, 3)) <= 0.3:
  72.                 results[encoder] = distance
  73.                 last_ok_value = distance
  74.             else:
  75.                 results[encoder] = last_ok_value
  76.         self.robot.set_wheels_speed(0)
  77.         # print("results:")
  78.         # print(results)
  79.         # print()
  80.         print(results)
  81.         rospy.sleep(2)
  82.         return results
  83.  
  84.     def find_object(self, dict):
  85.         a = min(dict, key=lambda x: dict.get(x))
  86.         turning1 = True
  87.         while turning1 is True:
  88.             self.robot.get_front_middle_ir()
  89.             self.robot.get_right_wheel_encoder()
  90.             self.robot.set_right_wheel_speed(self.right_speed)
  91.             self.robot.set_left_wheel_speed(-self.left_speed)
  92.             print(self.robot.get_front_middle_ir())
  93.             if self.robot.get_right_wheel_encoder() >= a + 1895:
  94.                 turning1 = False
  95.                 self.robot.set_wheels_speed(0)
  96.         rospy.sleep(1)
  97.  
  98.     def sense(self):
  99.         self.mid = self.robot.get_front_middle_ir()
  100.         return self.mid
  101.  
  102.     def scan(self, scan_speed):  # full 360 deg scan
  103.         # print("Scanning...")
  104.         # print("*bass-boosted ♫ Deja Vu♫ plays in the background*")
  105.         return self.buffer(scan_speed, 2016)
  106.  
  107.  
  108.     def main(self):
  109.         self.sense()
  110.         dict = self.scan(scan_speed=14)
  111.         self.find_object(dict)
  112.         rospy.sleep(0.05)  # 20 Hz
  113.         if self.problem_solved:
  114.             print("Solved! Good job, robot!")
  115.             exit()
  116.         else:
  117.             print("Unable to solve! :(")
  118.  
  119.  
  120.  
  121. if __name__ == "__main__":
  122.     robot = Robot()
  123.     robot.main()
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top