""" V2.0 Robot Instructables 12/Nov/2021 v1.0 24/Feb/2021 PID & Controller Code Routines Author: Azya Color & Distance Sensor Code & Auto start & Safe Stop Code Author: Jorge Moreno The code of this robot is based on the MOC 58271 code: Gyro Inventor by azya (https://rebrickable.com/mocs/MOC-58271/azya/gyro-inventor/#details), making the following modifications: Added color sensor code and distance sensor code. The selection routines are changed from keyboard commands to selection through the color sensor. Color Commands: Red : Satay White : Spinning Top Yellow : Round Blue : Line """ import math import hub import time # Initialize the Color Sensor. #color = ColorSensor('C') Color_Sensor = hub.port.D.device Color_Sensor.mode(0) # Initialize the Distance Sensor. Distance_Sensor = hub.port.C.device Distance_Sensor.mode(0) """ Images for light matrix. Each digit from 0 to 9 sets the brightness of one pixel, the first digit is the upper left pixel (near port A), the lines are separated by a colon. """ IMAGE_FACE = hub.Image("99000:97007:00009:99007:97000") IMAGE_FACE_WOW = hub.Image("89800:89809:00009:89809:89800") IMAGE_UP_PLS = hub.Image.ARROW_W IMAGE_LINE_TURN = hub.Image("00000:06960:99999:06960:00000") IMAGE_TURN = hub.Image("00000:06960:09990:06960:00000") IMAGE_RECT = hub.Image.SQUARE IMAGE_ROUND = hub.Image("08980:80008:90009:80008:08980") IMAGE_LINE = hub.Image("00000:00000:99999:00000:00000") def clamp(value, min_value, max_value): return min(max(round(value), min_value), max_value) class PID: """ Simple proportional–integral–derivative (PID) controller """ def __init__(self, kp, ki, kd): self._kp = kp self._ki = ki self._kd = kd self.reset() def process(self, setpoint, process_value, dt): error = setpoint - process_value self._integral += error * dt self._derivative = (error - self._error_prev) / dt self._error_prev = error return self._kp * error + self._ki * self._integral + self._kd * self._derivative def reset(self): self._derivative = 0 self._integral = 0 self._error_prev = 0 class Action: """ A Base class for various types of actions, described below """ def __init__(self, duration, wait = True, loop = False): self._duration = duration self._wait = wait self._loop = loop self._time = 0 def _on_end_action(self, parent): pass def _on_start_action(self, parent): pass def process(self, dt, parent): if (self._time == 0): self._on_start_action(parent) self._time += dt if (self._time >= self._duration): self._time = 0 self._on_end_action(parent) return False return True def is_wait(self): return self._wait def is_loop(self): return self._loop class Face(Action): """ Just show an "image" on the hub display loop - if set to True, the Controller will perform this action again, after completing the rest """ def __init__(self, image, loop = False): super().__init__(0, True, loop) self._image = image def _on_start_action(self, parent): parent.show_image(self._image) class Stay(Action): """ Do nothing for "duration" seconds loop - if set to True, the Controller will perform this action again, after completing the rest """ def __init__(self, duration, loop = False): super().__init__(duration, True, loop) class Straight(Action): """ Go straight at "speed" (-100 to 100) for "duration" seconds. Negative speed make the robot move forward, positive speed make the robot move backward. wait - if set True, the Controller will wait for this action to complete before starting the next loop - if set to True, the Controller will perform this action again, after completing the rest """ def __init__(self, speed = 50, duration = 0, wait = True, loop = False): super().__init__(duration, wait, loop) self._speed = clamp(speed, -100, 100) / 100 * 35 #percentage speed to the speed_setpoint, 35 is the max at which the robot remains stable def _on_end_action(self, parent): if (self._duration > 0): parent.set_speed_setpoint(0) def process(self, dt, parent): parent.set_speed_setpoint(self._speed) return super().process(dt, parent) class Turn(Action): """ Turn the robot at "angle" with "speed". Negative angles make the robot turn left, positive angles make the robot turn right. wait - if set True, the Controller will wait for this action to complete before starting the next loop - if set to True, the Controller will perform this action again, after completing the rest """ def __init__(self, angle, speed = 50, wait = True, loop = False): duration = (abs(angle) / 360 * 1.5) * (100 / clamp(speed, -100, 100)) super().__init__(duration, wait, loop) self._angle = angle * 4 #degries angle to the difference between wheels angular distance def process(self, dt, parent): parent.set_yaw_setpoint(parent.get_yaw_setpoint() + self._angle * (dt/self._duration)) return super().process(dt, parent) class Controller: """ Class for control the Gyro Inventor. Support few simple actions: straight, turn, stay, face. Exapmle: gyro_inventor.controller.set_actions(Straight(100, 3)) #Move forward 3 seconds at maximum speed gyro_inventor.controller.set_actions(Straight(-50)) #Move backward continuously at medium speed Actions can be combined: gyro_inventor.controller.set_actions([Straight(50, 2), Turn(180)]) #Move forward 2 seconds and then turn right 180 degrees Actions can be looped: gyro_inventor.controller.set_actions([Straight(50, 5, loop = True), Straight(-50, 5, loop = True)]) #The robot will move back and forth non-stop For some actions, you do not have to wait until the end, but immediately start the next: gyro_inventor.controller.set_actions([Straight(80, 5, wait = False), Turn(90, wait = False)]) #Move forward at 80% speed and turn left at the same time """ def __init__(self, parent): self._action_list = [] self._parent = parent #Add a new Action (or list of Actions) to the queue def add_actions(self, actions): if (isinstance(actions, list)): self._action_list.extend(actions) else: self._action_list.append(actions) #Clear action queue and add a new Action (or list of Actions) to the queue def set_actions(self, actions): self.clear_actions() self.add_actions(actions) self.update(0) #Clear action queue def clear_actions(self): self._parent.set_speed_setpoint(0) self._action_list = [] def update(self, dt): for action in self._action_list: if (not(action.process(dt, self._parent))): self._action_list.remove(action) if (action.is_loop()): self._action_list.append(action) if (action.is_wait()): break class GyroInventor: """ Base Gyro Inventor class that implements balancing and basic controls. """ HPF = 0.995 LPF = 1-HPF RAD2DEG = 180/math.pi def __init__(self, motor_left, motor_right, neutral_pitch = 0): self._motor_left = motor_left self._motor_right = motor_right self._neutral_pitch = neutral_pitch self._speed_setpoint = 0 self._yaw_setpoint = 0 #set PID constants self._pitch_PID = PID(kp = 13.5, ki = 155, kd = 0.19) self._speed_PID = PID(kp = 0.04, ki = 0.03, kd = 0.00055) self._yaw_PID = PID(kp = 0.9, ki = 2, kd = 0.01) self.controller = Controller(self); def get_pitch(self): try: dt = time.ticks_diff(time.ticks_us(), self._last_gyro_time) * 0.000001 self._last_gyro_time = time.ticks_us() accel = hub.motion.accelerometer() gyroPitch = self._pitch + hub.motion.gyroscope()[1] * dt accelPitch = -math.atan2(accel[2], math.sqrt(accel[0]*accel[0] + accel[1]*accel[1])) * self.RAD2DEG self._pitch = (self.HPF * gyroPitch + self.LPF * accelPitch) except AttributeError: self._pitch = (hub.motion.yaw_pitch_roll()[2]-90) self._last_gyro_time = time.ticks_us() return self._pitch def get_yaw(self): return -(self._motor_left.device.get()[1] + self._motor_right.device.get()[1]) def get_speed(self): return (self._motor_right.device.get()[0]-self._motor_left.device.get()[0]) / 2 def _motor_pwm(self, main, correction = 0): self._motor_left.pwm(clamp(main - correction, -127, 127)) self._motor_right.pwm(-clamp(main + correction, -127, 127)) def set_speed_setpoint(self, value): self._speed_setpoint = value def set_yaw_setpoint(self, value): self._yaw_setpoint = value def get_yaw_setpoint(self): return self._yaw_setpoint def show_image(self, image): hub.display.show(image) #main method, starts an endless loop def balance(self): state_active = False self.show_image(IMAGE_UP_PLS) lastTime = time.ticks_us() while (True): dt = time.ticks_diff(time.ticks_us(), lastTime) * 0.000001 lastTime = time.ticks_us() color = Color_Sensor.get()[0] distance = Distance_Sensor.get()[0] #distance = Distance_Sensor.get_distance_cm() pitch = self.get_pitch() if (state_active): if (abs(pitch)<45): self.controller.update(dt) #speed control #if the robot's speed deviates from the target, calculate a new target pitch angle speed_feedback = self._speed_PID.process(self._speed_setpoint, self.get_speed(), dt) #pitch control #determine the power of the motors to compensate for deviation from the target pitch angle pitch_feedback = self._pitch_PID.process(speed_feedback + self._neutral_pitch, pitch, dt) #yaw control #keep the robot on a given course yaw_feedback = self._yaw_PID.process(self._yaw_setpoint, self.get_yaw(), dt) self._motor_pwm(pitch_feedback, yaw_feedback) #print('distancia = '+str(distance)) #if (distance <= 15): # Distance_Sensor.light_up(100,50,25,0) #if (distance >= 20): # Distance_Sensor.light_up_all(0) if (color == 3): #if color = Blue the current preset = Line current_preset = 1 print('current preset = '+str(current_preset)) gyro_inventor.controller.set_actions(actions_preset[current_preset]) hub.led(3) if (color == 7): #if color = Yellow the current preset = Round current_preset = 4 print('current preset = '+str(current_preset)) gyro_inventor.controller.set_actions(actions_preset[current_preset]) hub.led(7) if (color == 9): #if color = Red the current preset = Stay current_preset = 0 print('current preset = '+str(current_preset)) gyro_inventor.controller.set_actions(actions_preset[current_preset]) hub.led(9) if (color == 10): #if color = White the current preset = Spinning Top current_preset = 5 print('current preset = '+str(current_preset)) gyro_inventor.controller.set_actions(actions_preset[current_preset]) hub.led(10) else: #stop all if the deviation is more than 45 deg state_active = False self.controller.clear_actions() self._motor_pwm(0) self._pitch_PID.reset() self._speed_PID.reset() self._yaw_PID.reset() self.show_image(IMAGE_UP_PLS) else: if (abs(pitch)<3): #start balancing if the pitch is small state_active = True self.show_image(IMAGE_FACE) gyro_inventor = GyroInventor(hub.port.B, hub.port.F, 1.6) #List of demonstrations of various actions actions_preset = [ [#just show robot face and balancing Stay(2), Face(IMAGE_FACE) ],[#line Face(IMAGE_LINE), Stay(2), Face(IMAGE_FACE), Straight(70, 5, loop = True), Straight(-70, 5, loop = True) ], [#J-turn Face(IMAGE_LINE_TURN), Stay(2), Face(IMAGE_FACE, loop = True), Straight(85, 3, loop = True), Face(IMAGE_FACE_WOW, loop = True), Turn(180, 100, wait = False, loop = True), Straight(-80, 3, loop = True), Face(IMAGE_FACE, loop = True), ], [#perimeter security Face(IMAGE_RECT), Stay(2), Face(IMAGE_FACE), Straight(80, 4, loop = True), Turn(90, loop = True) ], [#round Face(IMAGE_ROUND), Stay(2), Face(IMAGE_FACE), Straight(100, 5, wait = False, loop = True), Turn(360, 20, wait = False, loop = True) ], [#spinning top Face(IMAGE_TURN), Stay(2), Face(IMAGE_FACE), Turn(360, 80, loop = True) ] ] current_preset = 0 #movement selection by pressing the left-right hub buttons def on_left_click(ms): global current_preset if (ms == 0): current_preset = (current_preset + 1) % len(actions_preset) gyro_inventor.controller.set_actions(actions_preset[current_preset]) def on_right_click(ms): global current_preset if (ms == 0): current_preset = (current_preset - 1) % len(actions_preset) gyro_inventor.controller.set_actions(actions_preset[current_preset]) hub.button.left.on_change(on_left_click) hub.button.right.on_change(on_right_click) hub.led(0) #put your code here #for example, uncomment the line below and your robot will slowly move forward immediately after starting #gyro_inventor.controller.set_actions(Straight(40)) #see more in the description of the Controller class #start balancing gyro_inventor.balance()