├── .gitignore
├── LICENSE
├── README.md
├── chapter10
├── circle_pan_tilt_behavior.py
├── leds_led_shim.py
├── robot.py
├── servo_type_position.py
├── servos.py
└── sonar_scan.py
├── chapter11
├── drive_distance.py
├── drive_square.py
├── encoder_counter.py
├── leds_led_shim.py
├── pid_controller.py
├── robot.py
├── servos.py
├── straight_line_drive.py
├── test_distance_travelled.py
└── test_encoders.py
├── chapter12
├── accelerometer_vector.py
├── magnetometer_vector.py
├── plot_gyroscope.py
├── plot_temperature.py
├── robot_imu.py
└── robot_pose.py
├── chapter13
├── camera_stream.py
├── color_track_behavior.py
├── control_image_behavior.py
├── face_track_behavior.py
├── image_app_core.py
├── image_server.py
├── pid_controller.py
└── templates
│ ├── color_track_behavior.html
│ ├── control_image_behavior.html
│ └── image_server.html
├── chapter14
├── camera_stream.py
├── image_app_core.py
├── line_follow_behavior.py
├── pid_controller.py
├── robot.py
├── servos.py
├── templates
│ └── color_track_behavior.html
└── test_line_find.py
├── chapter15
├── control_server.py
├── my-robot-skill
│ ├── __init__.py
│ ├── dialog
│ │ └── en-us
│ │ │ ├── Robot.dialog
│ │ │ ├── TestingRainbow.dialog
│ │ │ ├── UnableToReach.dialog
│ │ │ └── stopping.dialog
│ ├── requirements.txt
│ ├── settingsmeta.json
│ └── vocab
│ │ └── en-us
│ │ ├── TestRainbow.voc
│ │ ├── robot.voc
│ │ └── stop.voc
└── robot_modes.py
├── chapter16
├── calibrate_gyro.py
├── delta_timer.py
├── drive_north_behavior.py
├── imu_settings.py
├── magnetometer_calibration.py
├── pid_controller.py
├── plot_acc_gyro_fusion.py
├── plot_accel_debug_pitch_and_roll.py
├── plot_accel_pitch_and_roll.py
├── plot_calibrated_gyroscope.py
├── plot_gyroscope.py
├── plot_integrated_gyro.py
├── plot_mag_heading.py
├── plot_pitch_and_roll.py
├── robot.py
├── robot_imu.py
├── robot_pose.py
├── servos.py
├── virtual_robot.py
├── visual_filtered_accelerometer.py
├── visual_fusion.py
└── visual_gyroscope.py
├── chapter17
├── 0_starting_point
│ ├── accelerometer_vector.py
│ ├── avoid_behavior.py
│ ├── avoid_with_rainbows.py
│ ├── behavior_line.py
│ ├── behavior_path.py
│ ├── calibrate_gyro.py
│ ├── calibrate_magnetometer.py
│ ├── camera_stream.py
│ ├── circle_pan_tilt_behavior.py
│ ├── color_track_behavior.py
│ ├── control_image_behavior.py
│ ├── control_server.py
│ ├── delta_timer.py
│ ├── drive_distance.py
│ ├── drive_north_behavior.py
│ ├── drive_square.py
│ ├── encoder_counter.py
│ ├── face_track_behavior.py
│ ├── image_app_core.py
│ ├── image_server.py
│ ├── imu_settings.py
│ ├── led_rainbow.py
│ ├── leds_led_shim.py
│ ├── leds_test.py
│ ├── line_follow_behavior.py
│ ├── magnetometer_vector.py
│ ├── my-robot-skill
│ │ ├── __init__.py
│ │ ├── dialog
│ │ │ └── en-us
│ │ │ │ ├── Robot.dialog
│ │ │ │ ├── TestingRainbow.dialog
│ │ │ │ ├── UnableToReach.dialog
│ │ │ │ └── stopping.dialog
│ │ ├── requirements.txt
│ │ ├── settingsmeta.json
│ │ └── vocab
│ │ │ └── en-us
│ │ │ ├── TestRainbow.voc
│ │ │ ├── robot.voc
│ │ │ └── stop.voc
│ ├── pid_controller.py
│ ├── plot_acc_gyro_fusion.py
│ ├── plot_accel_debug_pitch_and_roll.py
│ ├── plot_accel_pitch_and_roll.py
│ ├── plot_calibrated_gyroscope.py
│ ├── plot_gyroscope.py
│ ├── plot_imu_fusion.py
│ ├── plot_integrated_gyro.py
│ ├── plot_mag_heading.py
│ ├── plot_temperature.py
│ ├── robot.py
│ ├── robot_imu.py
│ ├── robot_modes.py
│ ├── robot_pose.py
│ ├── servo_type_position.py
│ ├── servos.py
│ ├── simple_avoid_behavior.py
│ ├── sonar_scan.py
│ ├── straight_line_drive.py
│ ├── templates
│ │ ├── color_track_behavior.html
│ │ ├── control_image_behavior.html
│ │ └── image_server.html
│ ├── test_distance_sensors.py
│ ├── test_distance_travelled.py
│ ├── test_encoders.py
│ ├── test_line_find.py
│ ├── test_motors.py
│ ├── test_rainbow.py
│ ├── virtual_robot.py
│ ├── visual_filtered_accelerometer.py
│ ├── visual_fusion.py
│ └── visual_gyroscope.py
└── full_system
│ ├── accelerometer_vector.py
│ ├── avoid_behavior.py
│ ├── avoid_with_rainbows.py
│ ├── behavior_line.py
│ ├── behavior_path.py
│ ├── calibrate_gyro.py
│ ├── calibrate_magnetometer.py
│ ├── camera_stream.py
│ ├── circle_pan_tilt_behavior.py
│ ├── color_track_behavior.py
│ ├── control_image_behavior.py
│ ├── control_server.py
│ ├── delta_timer.py
│ ├── drive_distance.py
│ ├── drive_north_behavior.py
│ ├── drive_square.py
│ ├── encoder_counter.py
│ ├── face_track_behavior.py
│ ├── image_app_core.py
│ ├── image_server.py
│ ├── imu_settings.py
│ ├── led_rainbow.py
│ ├── leds_led_shim.py
│ ├── leds_test.py
│ ├── line_follow_behavior.py
│ ├── magnetometer_vector.py
│ ├── manual_drive.py
│ ├── menu_server.service
│ ├── my-robot-skill
│ ├── __init__.py
│ ├── dialog
│ │ └── en-us
│ │ │ ├── Robot.dialog
│ │ │ ├── TestingRainbow.dialog
│ │ │ ├── UnableToReach.dialog
│ │ │ └── stopping.dialog
│ ├── requirements.txt
│ ├── settingsmeta.json
│ └── vocab
│ │ └── en-us
│ │ ├── TestRainbow.voc
│ │ ├── robot.voc
│ │ └── stop.voc
│ ├── pid_controller.py
│ ├── plot_acc_gyro_fusion.py
│ ├── plot_accel_debug_pitch_and_roll.py
│ ├── plot_accel_pitch_and_roll.py
│ ├── plot_calibrated_gyroscope.py
│ ├── plot_gyroscope.py
│ ├── plot_imu_fusion.py
│ ├── plot_integrated_gyro.py
│ ├── plot_mag_heading.py
│ ├── plot_temperature.py
│ ├── robot.py
│ ├── robot_imu.py
│ ├── robot_modes.py
│ ├── robot_pose.py
│ ├── servo_type_position.py
│ ├── servos.py
│ ├── simple_avoid_behavior.py
│ ├── sonar_scan.py
│ ├── static
│ ├── display.css
│ └── touch-slider.js
│ ├── straight_line_drive.py
│ ├── templates
│ ├── color_track_behavior.html
│ ├── control_image_behavior.html
│ ├── image_server.html
│ ├── manual_drive.html
│ └── menu.html
│ ├── test_distance_sensors.py
│ ├── test_distance_travelled.py
│ ├── test_encoders.py
│ ├── test_line_find.py
│ ├── test_motors.py
│ ├── test_rainbow.py
│ ├── virtual_robot.py
│ ├── visual_filtered_accelerometer.py
│ ├── visual_fusion.py
│ └── visual_gyroscope.py
├── chapter4
├── readme.md
└── wpa_supplicant_example.conf
├── chapter5
├── hello.py
└── readme.md
├── chapter7
├── behavior_line.py
├── behavior_path.py
├── robot.py
└── test_motors.py
├── chapter8
├── avoid_behavior.py
├── robot.py
├── simple_avoid_behavior.py
└── test_distance_sensors.py
└── chapter9
├── avoid_behavior.py
├── avoid_with_rainbows.py
├── diagrams
└── generate_colour_wheel.py
├── led_rainbow.py
├── leds_led_shim.py
├── leds_test.py
├── robot.py
└── test_rainbow.py
/.gitignore:
--------------------------------------------------------------------------------
1 | .DS_Store
2 | /.venv/
3 | .idea/
4 | /ext_lib/
5 | *.pyc
6 | __pycache__
7 | .vscode
8 |
--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
1 | MIT License
2 |
3 | Copyright (c) 2019 Packt
4 |
5 | Permission is hereby granted, free of charge, to any person obtaining a copy
6 | of this software and associated documentation files (the "Software"), to deal
7 | in the Software without restriction, including without limitation the rights
8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
9 | copies of the Software, and to permit persons to whom the Software is
10 | furnished to do so, subject to the following conditions:
11 |
12 | The above copyright notice and this permission notice shall be included in all
13 | copies or substantial portions of the Software.
14 |
15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 | SOFTWARE.
22 |
--------------------------------------------------------------------------------
/chapter10/circle_pan_tilt_behavior.py:
--------------------------------------------------------------------------------
1 | from time import sleep
2 | import math
3 |
4 | from robot import Robot
5 | class CirclePanTiltBehavior:
6 | def __init__(self, the_robot):
7 | self.robot = the_robot
8 | self.current_time = 0
9 | self.frames_per_circle = 50
10 | self.radians_per_frame = (2 * math.pi) / self.frames_per_circle
11 | self.radius = 30
12 |
13 | def run(self):
14 | while True:
15 | frame_number = self.current_time % self.frames_per_circle
16 | frame_in_radians = frame_number * self.radians_per_frame
17 | self.robot.set_pan(self.radius * math.cos(frame_in_radians))
18 | self.robot.set_tilt(self.radius * math.sin(frame_in_radians))
19 | sleep(0.05)
20 | self.current_time += 1
21 |
22 | bot = Robot()
23 | behavior = CirclePanTiltBehavior(bot)
24 | behavior.run()
25 |
26 |
--------------------------------------------------------------------------------
/chapter10/leds_led_shim.py:
--------------------------------------------------------------------------------
1 | import ledshim
2 |
3 |
4 | class Leds:
5 | @property
6 | def count(self):
7 | return ledshim.width
8 |
9 | def set_one(self, led_number, color):
10 | ledshim.set_pixel(led_number, *color)
11 |
12 | def set_range(self, led_range, color):
13 | for pixel in led_range:
14 | ledshim.set_pixel(pixel, *color)
15 |
16 | def set_all(self, color):
17 | ledshim.set_all(*color)
18 |
19 | def clear(self):
20 | ledshim.clear()
21 |
22 | def show(self):
23 | ledshim.show()
24 |
--------------------------------------------------------------------------------
/chapter10/robot.py:
--------------------------------------------------------------------------------
1 | from Raspi_MotorHAT import Raspi_MotorHAT
2 | from gpiozero import DistanceSensor
3 |
4 | import atexit
5 | import leds_led_shim
6 | from servos import Servos
7 |
8 |
9 | class Robot:
10 | def __init__(self, motorhat_addr=0x6f):
11 | # Setup the motorhat with the passed in address
12 | self._mh = Raspi_MotorHAT(addr=motorhat_addr)
13 |
14 | # get local variable for each motor
15 | self.left_motor = self._mh.getMotor(1)
16 | self.right_motor = self._mh.getMotor(2)
17 |
18 | # Setup The Distance Sensors
19 | self.left_distance_sensor = DistanceSensor(echo=17, trigger=27, queue_len=2)
20 | self.right_distance_sensor = DistanceSensor(echo=5, trigger=6, queue_len=2)
21 |
22 | # Setup the Leds
23 | self.leds = leds_led_shim.Leds()
24 | # Set up servo motors for pan and tilt.
25 | self.servos = Servos(addr=motorhat_addr)
26 | # ensure the motors get stopped when the code exits
27 | atexit.register(self.stop_all)
28 |
29 | def convert_speed(self, speed):
30 | # Choose the running mode
31 | mode = Raspi_MotorHAT.RELEASE
32 | if speed > 0:
33 | mode = Raspi_MotorHAT.FORWARD
34 | elif speed < 0:
35 | mode = Raspi_MotorHAT.BACKWARD
36 |
37 | # Scale the speed
38 | output_speed = (abs(speed) * 255) // 100
39 | return mode, int(output_speed)
40 |
41 | def set_left(self, speed):
42 | mode, output_speed = self.convert_speed(speed)
43 | self.left_motor.setSpeed(output_speed)
44 | self.left_motor.run(mode)
45 |
46 | def set_right(self, speed):
47 | mode, output_speed = self.convert_speed(speed)
48 | self.right_motor.setSpeed(output_speed)
49 | self.right_motor.run(mode)
50 |
51 | def stop_motors(self):
52 | self.left_motor.run(Raspi_MotorHAT.RELEASE)
53 | self.right_motor.run(Raspi_MotorHAT.RELEASE)
54 |
55 | def stop_all(self):
56 | self.stop_motors()
57 |
58 | # Clear the display
59 | self.leds.clear()
60 | self.leds.show()
61 |
62 | # Reset the servos
63 | self.servos.stop_all()
64 |
65 | def set_pan(self, angle):
66 | self.servos.set_servo_angle(1, angle)
67 |
68 | def set_tilt(self, angle):
69 | self.servos.set_servo_angle(0, angle)
70 |
71 |
--------------------------------------------------------------------------------
/chapter10/servo_type_position.py:
--------------------------------------------------------------------------------
1 | from Raspi_MotorHAT.Raspi_PWM_Servo_Driver import PWM
2 | import atexit
3 |
4 | pwm = PWM(0x6f)
5 | # This sets the timebase for it all
6 | pwm_frequency = 100
7 | pwm.setPWMFreq(pwm_frequency)
8 | # Mid-point of the servo pulse length in milliseconds.
9 | servo_mid_point_ms = 1.5
10 | # What a deflection of 90 degrees is in pulse length in milliseconds
11 | deflect_90_in_ms = 0.5
12 | # Frequency is 1 divided by period, but working ms, we can use 1000
13 | period_in_ms = 1000 / pwm_frequency
14 | # The chip has 4096 steps in each period.
15 | pulse_steps = 4096
16 | # Steps for every millisecond.
17 | steps_per_ms = pulse_steps / period_in_ms
18 | # Steps for a degree.
19 | steps_per_degree = (deflect_90_in_ms * steps_per_ms) / 90
20 | # Mid-point of the servo in steps
21 | servo_mid_point_steps = servo_mid_point_ms * steps_per_ms
22 |
23 | def convert_degrees_to_steps(position):
24 | return int(servo_mid_point_steps + (position * steps_per_degree))
25 |
26 | atexit.register(pwm.setPWM, 0, 0, 4096)
27 |
28 | while True:
29 | position = int(input("Type your position in degrees (90 to -90, 0 is middle): "))
30 | end_step = convert_degrees_to_steps(position)
31 | pwm.setPWM(0, 0, end_step)
32 |
--------------------------------------------------------------------------------
/chapter10/servos.py:
--------------------------------------------------------------------------------
1 | from Raspi_MotorHAT.Raspi_PWM_Servo_Driver import PWM
2 |
3 | class Servos:
4 | """PCA 9865 Servo motors"""
5 |
6 | def __init__(self, addr=0x6f, deflect_90_in_ms = 0.5):
7 | """addr: The i2c address of the PWM chip.
8 | deflect_90_in_ms: set this to calibrate the servo motors.
9 | it is what a deflection of 90 degrees is
10 | in terms of a pulse length in milliseconds."""
11 | self._pwm = PWM(addr)
12 | # This sets the timebase for it all
13 | pwm_frequency = 100
14 | self._pwm.setPWMFreq(pwm_frequency)
15 | # Mid-point of the servo pulse length in milliseconds.
16 | servo_mid_point_ms = 1.5
17 | # Frequency is 1/period, but working ms, we can use 1000
18 | period_in_ms = 1000 / pwm_frequency
19 | # The chip has 4096 steps in each period.
20 | pulse_steps = 4096
21 | # Steps for every millisecond.
22 | steps_per_ms = pulse_steps / period_in_ms
23 | # Steps for a degree
24 | self.steps_per_degree = (deflect_90_in_ms * steps_per_ms) / 90
25 | # Mid-point of the servo in steps
26 | self.servo_mid_point_steps = servo_mid_point_ms * steps_per_ms
27 |
28 | # Map for channels
29 | self.channels = [0, 1, 14, 15]
30 |
31 | def stop_all(self):
32 | # 0 in start is nothing
33 | off_bit = 4096 # bit 12 is the OFF bit.
34 | self._pwm.setPWM(self.channels[0], 0, off_bit)
35 | self._pwm.setPWM(self.channels[1], 0, off_bit)
36 | self._pwm.setPWM(self.channels[2], 0, off_bit)
37 | self._pwm.setPWM(self.channels[3], 0, off_bit)
38 |
39 | def _convert_degrees_to_steps(self, position):
40 | return int(self.servo_mid_point_steps + (position * self.steps_per_degree))
41 |
42 | def set_servo_angle(self, channel, angle):
43 | """position: The position in degrees from the center. -90 to 90"""
44 | # Validate
45 | if angle > 90 or angle < -90:
46 | raise ValueError("Angle outside of range")
47 | # Then set the position
48 | off_step = self._convert_degrees_to_steps(angle)
49 | self._pwm.setPWM(self.channels[channel], 0, off_step)
50 |
51 |
--------------------------------------------------------------------------------
/chapter10/sonar_scan.py:
--------------------------------------------------------------------------------
1 | import time
2 | import math
3 |
4 | import matplotlib.pyplot as plt
5 |
6 | from robot import Robot
7 |
8 | start_scan =0
9 | lower_bound = -90
10 | upper_bound = 90
11 | scan_step = 5
12 |
13 | the_robot = Robot()
14 | the_robot.set_tilt(0)
15 |
16 | scan_data = {}
17 | # Make the sensor scan
18 | for facing in range(lower_bound, upper_bound, scan_step):
19 | the_robot.set_pan(-facing)
20 | time.sleep(0.1)
21 | scan_data[facing] = the_robot.left_distance_sensor.distance * 100
22 | # make plot
23 | axis = [math.radians(facing) for facing in scan_data.keys()]
24 | print(axis)
25 | print(scan_data.values())
26 | plt.polar(axis, list(scan_data.values()), 'g-')
27 | # dump to png
28 | plt.savefig("scan.png")
29 |
--------------------------------------------------------------------------------
/chapter11/drive_distance.py:
--------------------------------------------------------------------------------
1 | from robot import Robot, EncoderCounter
2 | from pid_controller import PIController
3 | import time
4 | import logging
5 | logger = logging.getLogger("drive_distance")
6 |
7 | def drive_distance(bot, distance, speed=80):
8 | # Use left as "primary" motor, the right is keeping up
9 | set_primary = bot.set_left
10 | primary_encoder = bot.left_encoder
11 | set_secondary = bot.set_right
12 | secondary_encoder = bot.right_encoder
13 | controller = PIController(proportional_constant=5, integral_constant=0.3)
14 |
15 | # start the motors, and start the loop
16 | set_primary(speed)
17 | set_secondary(speed)
18 | while primary_encoder.pulse_count < distance or secondary_encoder.pulse_count < distance:
19 | time.sleep(0.01)
20 | # How far off are we?
21 | error = primary_encoder.pulse_count - secondary_encoder.pulse_count
22 | adjustment = controller.get_value(error)
23 | # How fast should the motor move to get there?
24 | set_primary(int(speed - adjustment))
25 | set_secondary(int(speed + adjustment))
26 | # Some debug
27 | logger.debug(f"Encoders: primary: {primary_encoder.pulse_count}, secondary: {secondary_encoder.pulse_count},"
28 | f"e:{error} adjustment: {adjustment:.2f}")
29 | logger.info(f"Distances: primary: {primary_encoder.distance_in_mm()} mm, secondary: {secondary_encoder.distance_in_mm()} mm")
30 |
31 | logging.basicConfig(level=logging.DEBUG)
32 | bot = Robot()
33 | distance_to_drive = 1000 # in mm - this is a meter
34 | distance_in_ticks = EncoderCounter.mm_to_ticks(distance_to_drive)
35 | drive_distance(bot, distance_in_ticks)
36 |
--------------------------------------------------------------------------------
/chapter11/encoder_counter.py:
--------------------------------------------------------------------------------
1 | from gpiozero import DigitalInputDevice
2 | import math
3 | class EncoderCounter:
4 | ticks_to_mm_const = None # you must set this up before using distance methods
5 | def __init__(self, pin_number):
6 | self.pulse_count = 0
7 | self.direction = 1
8 | self.device = DigitalInputDevice(pin=pin_number)
9 | self.device.pin.when_changed = self.when_changed
10 |
11 | def when_changed(self, time_ticks, state):
12 | self.pulse_count += self.direction
13 |
14 | def set_direction(self, direction):
15 | """This should be -1 or 1. """
16 | assert abs(direction)==1, "Direction %s should be 1 or -1" % direction
17 | self.direction = direction
18 |
19 | def reset(self):
20 | self.pulse_count = 0
21 |
22 | def stop(self):
23 | self.device.close()
24 |
25 | def distance_in_mm(self):
26 | return int(self.pulse_count * EncoderCounter.ticks_to_mm_const)
27 |
28 | @staticmethod
29 | def mm_to_ticks(mm):
30 | return mm / EncoderCounter.ticks_to_mm_const
31 |
32 | @staticmethod
33 | def set_constants(wheel_diameter_mm, ticks_per_revolution):
34 | EncoderCounter.ticks_to_mm_const = (math.pi / ticks_per_revolution) * wheel_diameter_mm
35 |
--------------------------------------------------------------------------------
/chapter11/leds_led_shim.py:
--------------------------------------------------------------------------------
1 | import ledshim
2 |
3 |
4 | class Leds:
5 | @property
6 | def count(self):
7 | return ledshim.width
8 |
9 | def set_one(self, led_number, color):
10 | ledshim.set_pixel(led_number, *color)
11 |
12 | def set_range(self, led_range, color):
13 | for pixel in led_range:
14 | ledshim.set_pixel(pixel, *color)
15 |
16 | def set_all(self, color):
17 | ledshim.set_all(*color)
18 |
19 | def clear(self):
20 | ledshim.clear()
21 |
22 | def show(self):
23 | ledshim.show()
24 |
--------------------------------------------------------------------------------
/chapter11/pid_controller.py:
--------------------------------------------------------------------------------
1 | import logging
2 | logger = logging.getLogger("pid_controller")
3 |
4 | class PIController:
5 | def __init__(self, proportional_constant=0, integral_constant=0):
6 | self.proportional_constant = proportional_constant
7 | self.integral_constant = integral_constant
8 |
9 | # Running sums
10 | self.integral_sum = 0
11 |
12 | def handle_proportional(self, error):
13 | return self.proportional_constant * error
14 |
15 | def handle_integral(self, error):
16 | self.integral_sum += error
17 | return self.integral_constant * self.integral_sum
18 |
19 | def get_value(self, error):
20 | p = self.handle_proportional(error)
21 | i = self.handle_integral(error)
22 | logger.debug(f"P: {p}, I: {i:.2f}")
23 | return p + i
24 |
--------------------------------------------------------------------------------
/chapter11/robot.py:
--------------------------------------------------------------------------------
1 | from Raspi_MotorHAT import Raspi_MotorHAT
2 | from gpiozero import DistanceSensor
3 |
4 | import atexit
5 | import leds_led_shim
6 | from servos import Servos
7 | from encoder_counter import EncoderCounter
8 |
9 |
10 | class Robot:
11 | wheel_diameter_mm = 70.0
12 | ticks_per_revolution = 40.0
13 | wheel_distance_mm = 132.0
14 | def __init__(self, motorhat_addr=0x6f):
15 | # Setup the motorhat with the passed in address
16 | self._mh = Raspi_MotorHAT(addr=motorhat_addr)
17 |
18 | self.left_motor = self._mh.getMotor(1)
19 | self.right_motor = self._mh.getMotor(2)
20 |
21 | # Setup the Leds
22 | self.leds = leds_led_shim.Leds()
23 | # Set up servo motors for pan and tilt.
24 | self.servos = Servos(addr=motorhat_addr)
25 |
26 | # Setup The Distance Sensors
27 | self.left_distance_sensor = DistanceSensor(echo=17, trigger=27, queue_len=2)
28 | self.right_distance_sensor = DistanceSensor(echo=5, trigger=6, queue_len=2)
29 |
30 | # Setup the Encoders
31 | EncoderCounter.set_constants(self.wheel_diameter_mm, self.ticks_per_revolution)
32 | self.left_encoder = EncoderCounter(4)
33 | self.right_encoder = EncoderCounter(26)
34 |
35 | # ensure the motors get stopped when the code exits
36 | atexit.register(self.stop_all)
37 |
38 | def convert_speed(self, speed):
39 | # Choose the running mode
40 | mode = Raspi_MotorHAT.RELEASE
41 | if speed > 0:
42 | mode = Raspi_MotorHAT.FORWARD
43 | elif speed < 0:
44 | mode = Raspi_MotorHAT.BACKWARD
45 |
46 | # Scale the speed
47 | output_speed = (abs(speed) * 255) // 100
48 | return mode, int(output_speed)
49 |
50 | def set_left(self, speed):
51 | mode, output_speed = self.convert_speed(speed)
52 | self.left_motor.setSpeed(output_speed)
53 | self.left_motor.run(mode)
54 |
55 | def set_right(self, speed):
56 | mode, output_speed = self.convert_speed(speed)
57 | self.right_motor.setSpeed(output_speed)
58 | self.right_motor.run(mode)
59 |
60 | def stop_motors(self):
61 | self.left_motor.run(Raspi_MotorHAT.RELEASE)
62 | self.right_motor.run(Raspi_MotorHAT.RELEASE)
63 |
64 | def stop_all(self):
65 | self.stop_motors()
66 |
67 | # Clear the display
68 | self.leds.clear()
69 | self.leds.show()
70 |
71 | # Clear any sensor handlers
72 | self.left_encoder.stop()
73 | self.right_encoder.stop()
74 |
75 | # Reset the servos
76 | self.servos.stop_all()
77 |
78 | def set_pan(self, angle):
79 | self.servos.set_servo_angle(1, angle)
80 |
81 | def set_tilt(self, angle):
82 | self.servos.set_servo_angle(0, angle)
83 |
84 |
--------------------------------------------------------------------------------
/chapter11/servos.py:
--------------------------------------------------------------------------------
1 | from Raspi_MotorHAT.Raspi_PWM_Servo_Driver import PWM
2 |
3 | class Servos:
4 | """PCA 9865 Servo motors"""
5 |
6 | def __init__(self, addr=0x6f, deflect_90_in_ms = 0.5):
7 | """addr: The i2c address of the PWM chip.
8 | deflect_90_in_ms: set this to calibrate the servo motors.
9 | it is what a deflection of 90 degrees is
10 | in terms of a pulse length in milliseconds."""
11 | self._pwm = PWM(addr)
12 | # This sets the timebase for it all
13 | pwm_frequency = 100
14 | self._pwm.setPWMFreq(pwm_frequency)
15 | # Mid-point of the servo pulse length in milliseconds.
16 | servo_mid_point_ms = 1.5
17 | # Frequency is 1/period, but working ms, we can use 1000
18 | period_in_ms = 1000 / pwm_frequency
19 | # The chip has 4096 steps in each period.
20 | pulse_steps = 4096
21 | # Steps for every millisecond.
22 | steps_per_ms = pulse_steps / period_in_ms
23 | # Steps for a degree
24 | self.steps_per_degree = (deflect_90_in_ms * steps_per_ms) / 90
25 | # Mid-point of the servo in steps
26 | self.servo_mid_point_steps = servo_mid_point_ms * steps_per_ms
27 |
28 | # Map for channels
29 | self.channels = [0, 1, 14, 15]
30 |
31 | def stop_all(self):
32 | # 0 in start is nothing
33 | off_bit = 4096 # bit 12 is the OFF bit.
34 | self._pwm.setPWM(self.channels[0], 0, off_bit)
35 | self._pwm.setPWM(self.channels[1], 0, off_bit)
36 | self._pwm.setPWM(self.channels[2], 0, off_bit)
37 | self._pwm.setPWM(self.channels[3], 0, off_bit)
38 |
39 | def _convert_degrees_to_steps(self, position):
40 | return int(self.servo_mid_point_steps + (position * self.steps_per_degree))
41 |
42 | def set_servo_angle(self, channel, angle):
43 | """position: The position in degrees from the center. -90 to 90"""
44 | # Validate
45 | if angle > 90 or angle < -90:
46 | raise ValueError("Angle outside of range")
47 | # Then set the position
48 | off_step = self._convert_degrees_to_steps(angle)
49 | self._pwm.setPWM(self.channels[channel], 0, off_step)
50 |
51 |
--------------------------------------------------------------------------------
/chapter11/straight_line_drive.py:
--------------------------------------------------------------------------------
1 | from robot import Robot
2 | from pid_controller import PIController
3 | import time
4 | import logging
5 |
6 | logger = logging.getLogger("straight_line")
7 | logging.basicConfig(level=logging.INFO)
8 | logging.getLogger("pid_controller").setLevel(logging.DEBUG)
9 |
10 | bot = Robot()
11 | stop_at_time = time.time() + 15
12 |
13 | speed = 80
14 | bot.set_left(speed)
15 | bot.set_right(speed)
16 |
17 | pid = PIController(proportional_constant=4, integral_constant=0.3)
18 |
19 | while time.time() < stop_at_time:
20 | time.sleep(0.01)
21 | # Calculate the error
22 | left = bot.left_encoder.pulse_count
23 | right = bot.right_encoder.pulse_count
24 | error = left - right
25 |
26 | # Get the speed
27 | adjustment = pid.get_value(error)
28 | right_speed = int(speed + adjustment)
29 | left_speed = int(speed - adjustment)
30 |
31 | logger.debug(f"error: {error} adjustment: {adjustment:.2f}")
32 | logger.info(f"left: {left} right: {right}, left_speed: {left_speed} right_speed: {right_speed}")
33 | bot.set_left(left_speed)
34 | bot.set_right(right_speed)
35 |
--------------------------------------------------------------------------------
/chapter11/test_distance_travelled.py:
--------------------------------------------------------------------------------
1 | from robot import Robot
2 | import time
3 | import math
4 | import logging
5 | logger = logging.getLogger("test_distance_travelled")
6 |
7 | wheel_diameter_mm = 70.0
8 | ticks_per_revolution = 40.0
9 | ticks_to_mm_const = (math.pi / ticks_per_revolution) * wheel_diameter_mm
10 |
11 | def ticks_to_mm(ticks):
12 | return int(ticks_to_mm_const * ticks)
13 |
14 | bot = Robot()
15 | stop_at_time = time.time() + 1
16 |
17 | logging.basicConfig(level=logging.INFO)
18 | bot.set_left(90)
19 | bot.set_right(90)
20 |
21 | while time.time() < stop_at_time:
22 | logger.info("Left: {} Right: {}".format(
23 | ticks_to_mm(bot.left_encoder.pulse_count),
24 | ticks_to_mm(bot.right_encoder.pulse_count)))
25 | time.sleep(0.05)
26 |
27 |
--------------------------------------------------------------------------------
/chapter11/test_encoders.py:
--------------------------------------------------------------------------------
1 | from robot import Robot
2 | import time
3 | import logging
4 |
5 | from gpiozero import DigitalInputDevice
6 |
7 | logger = logging.getLogger("test_encoders")
8 |
9 | class EncoderCounter(object):
10 | def __init__(self, pin_number):
11 | self.pulse_count = 0
12 | self.device = DigitalInputDevice(pin=pin_number)
13 | self.device.pin.when_changed = self.when_changed
14 |
15 | def when_changed(self, time_ticks, state):
16 | self.pulse_count += 1
17 |
18 | bot = Robot()
19 | left_encoder = EncoderCounter(4)
20 | right_encoder = EncoderCounter(26)
21 |
22 | stop_at_time = time.time() + 1
23 |
24 | logging.basicConfig(level=logging.INFO)
25 | bot.set_left(90)
26 | bot.set_right(90)
27 | while time.time() < stop_at_time:
28 | logger.info(f"Left: {left_encoder.pulse_count} Right: {right_encoder.pulse_count}")
29 | time.sleep(0.05)
30 |
--------------------------------------------------------------------------------
/chapter12/accelerometer_vector.py:
--------------------------------------------------------------------------------
1 | import vpython as vp
2 | import logging
3 | from robot_imu import RobotImu
4 | from robot_pose import robot_view
5 |
6 | logging.basicConfig(level=logging.INFO)
7 | imu = RobotImu()
8 | robot_view()
9 |
10 | accel_arrow = vp.arrow(axis=vp.vector(0, 1, 0))
11 | x_arrow = vp.arrow(axis=vp.vector(1, 0, 0), color=vp.color.red)
12 | y_arrow = vp.arrow(axis=vp.vector(0, 1, 0), color=vp.color.green)
13 | z_arrow = vp.arrow(axis=vp.vector(0, 0, 1), color=vp.color.blue)
14 |
15 | while True:
16 | vp.rate(100)
17 | accel = imu.read_accelerometer()
18 | print(f"Accelerometer: {accel}")
19 | accel_arrow.axis = accel.norm()
20 |
--------------------------------------------------------------------------------
/chapter12/magnetometer_vector.py:
--------------------------------------------------------------------------------
1 | import vpython as vp
2 | import logging
3 | from robot_imu import RobotImu
4 | from robot_pose import robot_view
5 |
6 | logging.basicConfig(level=logging.INFO)
7 | imu = RobotImu()
8 | robot_view()
9 |
10 | mag_arrow = vp.arrow(pos=vp.vector(0, 0, 0))
11 | x_arrow = vp.arrow(axis=vp.vector(1, 0, 0), color=vp.color.red)
12 | y_arrow = vp.arrow(axis=vp.vector(0, 1, 0), color=vp.color.green)
13 | z_arrow = vp.arrow(axis=vp.vector(0, 0, 1), color=vp.color.blue)
14 |
15 | while True:
16 | vp.rate(100)
17 |
18 | mag = imu.read_magnetometer()
19 | mag_arrow.axis = mag.norm()
20 | print(f"Magnetometer: {mag}")
21 |
22 |
--------------------------------------------------------------------------------
/chapter12/plot_gyroscope.py:
--------------------------------------------------------------------------------
1 | import vpython as vp
2 | import time
3 | from robot_imu import RobotImu
4 |
5 | imu = RobotImu()
6 |
7 | vp.graph(xmin=0, xmax=60, scroll=True)
8 | graph_x = vp.gcurve(color=vp.color.red)
9 | graph_y = vp.gcurve(color=vp.color.green)
10 | graph_z = vp.gcurve(color=vp.color.blue)
11 |
12 | start = time.time()
13 | while True:
14 | vp.rate(100)
15 | elapsed = time.time() - start
16 | gyro = imu.read_gyroscope()
17 | graph_x.plot(elapsed, gyro.x)
18 | graph_y.plot(elapsed, gyro.y)
19 | graph_z.plot(elapsed, gyro.z)
20 |
--------------------------------------------------------------------------------
/chapter12/plot_temperature.py:
--------------------------------------------------------------------------------
1 | """When running,
2 | use VPYTHON_PORT=9020 VPYTHON_NOBROWSER=true"""
3 | import vpython as vp
4 | import time
5 | import logging
6 | from robot_imu import RobotImu
7 |
8 |
9 | logging.basicConfig(level=logging.INFO)
10 | imu = RobotImu()
11 | vp.graph(xmin=0, xmax=60, scroll=True)
12 | temp_graph = vp.gcurve()
13 | start = time.time()
14 | while True:
15 | vp.rate(100)
16 | temperature = imu.read_temperature()
17 | logging.info("Temperature {}".format(temperature))
18 | elapsed = time.time() - start
19 | temp_graph.plot(elapsed, temperature)
20 |
21 |
--------------------------------------------------------------------------------
/chapter12/robot_imu.py:
--------------------------------------------------------------------------------
1 | from icm20948 import ICM20948
2 | from vpython import vector
3 |
4 |
5 | class RobotImu:
6 | """Define a common interface to an inertial measurement unit with temperature"""
7 | def __init__(self):
8 | self._imu = ICM20948()
9 |
10 | def read_temperature(self):
11 | """Read a temperature in degrees C."""
12 | return self._imu.read_temperature()
13 |
14 | def read_gyroscope(self):
15 | """Return prescaled gyro data"""
16 | _, _, _, x, y, z = self._imu.read_accelerometer_gyro_data()
17 | return vector(x, y, z)
18 |
19 | def read_accelerometer(self):
20 | """Return accelerometer data"""
21 | accel_x, accel_y, accel_z, _, _, _ = self._imu.read_accelerometer_gyro_data()
22 | return vector(accel_x, accel_y, accel_z)
23 |
24 | def read_magnetometer(self):
25 | """Return magnetometer data"""
26 | mag_x, mag_y, mag_z = self._imu.read_magnetometer_data()
27 | return vector(mag_x, -mag_y, -mag_z)
28 |
29 |
--------------------------------------------------------------------------------
/chapter12/robot_pose.py:
--------------------------------------------------------------------------------
1 | import vpython as vp
2 |
3 |
4 | def robot_view():
5 | vp.scene.forward = vp.vector(-3, -1, -1)
6 | vp.scene.up = vp.vector(0, 0, 1)
7 |
--------------------------------------------------------------------------------
/chapter13/camera_stream.py:
--------------------------------------------------------------------------------
1 | from picamera.array import PiRGBArray
2 | from picamera import PiCamera
3 | import numpy as np
4 |
5 | import cv2
6 |
7 | size = (320, 240)
8 | encode_param = [int(cv2.IMWRITE_JPEG_QUALITY), 90]
9 |
10 |
11 | def setup_camera():
12 | camera = PiCamera()
13 | camera.resolution = size
14 | camera.framerate = 24
15 | camera.rotation = 180
16 | return camera
17 |
18 |
19 | def start_stream(camera):
20 | image_storage = PiRGBArray(camera, size=size)
21 |
22 | cam_stream = camera.capture_continuous(image_storage, format="bgr", use_video_port=True)
23 | for raw_frame in cam_stream:
24 | yield raw_frame.array
25 | image_storage.truncate(0)
26 |
27 |
28 | def get_encoded_bytes_for_frame(frame):
29 | result, encoded_image = cv2.imencode('.jpg', frame, encode_param)
30 | return encoded_image.tostring()
31 |
--------------------------------------------------------------------------------
/chapter13/control_image_behavior.py:
--------------------------------------------------------------------------------
1 | import time
2 |
3 | from image_app_core import start_server_process, get_control_instruction, put_output_image
4 | import camera_stream
5 |
6 | def controlled_image_server_behavior():
7 | camera = camera_stream.setup_camera()
8 | time.sleep(0.1)
9 |
10 | for frame in camera_stream.start_stream(camera):
11 | encoded_bytes = camera_stream.get_encoded_bytes_for_frame(frame)
12 | put_output_image(encoded_bytes)
13 |
14 | instruction = get_control_instruction()
15 | if instruction and instruction['command'] == "exit":
16 | print("Stopping")
17 | return
18 |
19 | process = start_server_process('control_image_behavior.html')
20 |
21 | try:
22 | controlled_image_server_behavior()
23 | finally:
24 | process.terminate()
25 |
--------------------------------------------------------------------------------
/chapter13/image_app_core.py:
--------------------------------------------------------------------------------
1 | import time
2 | from multiprocessing import Process, Queue
3 |
4 | from flask import Flask, render_template, Response, request
5 |
6 | app = Flask(__name__)
7 | control_queue = Queue()
8 | display_queue = Queue(maxsize=2)
9 | display_template = 'image_server.html'
10 |
11 | @app.route('/')
12 | def index():
13 | return render_template(display_template)
14 |
15 | def frame_generator():
16 | while True:
17 | time.sleep(0.05)
18 | encoded_bytes = display_queue.get()
19 | yield (b'--frame\r\n'
20 | b'Content-Type: image/jpeg\r\n\r\n' + encoded_bytes + b'\r\n')
21 |
22 | @app.route('/display')
23 | def display():
24 | return Response(frame_generator(),
25 | mimetype='multipart/x-mixed-replace; boundary=frame')
26 |
27 | @app.route('/control', methods=['POST'])
28 | def control():
29 | control_queue.put(request.form)
30 | return Response('queued')
31 |
32 | def start_server_process(template_name):
33 | """Start the process, call .terminate to close it"""
34 | global display_template
35 | display_template = template_name
36 | server = Process(target=app.run, kwargs={"host": "0.0.0.0", "port": 5001})
37 | server.start()
38 | return server
39 |
40 | def put_output_image(encoded_bytes):
41 | """Queue an output image"""
42 | if display_queue.empty():
43 | display_queue.put(encoded_bytes)
44 |
45 | def get_control_instruction():
46 | if control_queue.empty():
47 | return None
48 | else:
49 | return control_queue.get()
50 |
--------------------------------------------------------------------------------
/chapter13/image_server.py:
--------------------------------------------------------------------------------
1 | from flask import Flask, render_template, Response
2 | import camera_stream
3 | import time
4 |
5 | app = Flask(__name__)
6 |
7 | @app.route('/')
8 | def index():
9 | return render_template('image_server.html')
10 |
11 | def frame_generator():
12 | """This is our main video feed"""
13 | camera = camera_stream.setup_camera()
14 |
15 | # allow the camera to warm up
16 | time.sleep(0.1)
17 | for frame in camera_stream.start_stream(camera):
18 | encoded_bytes = camera_stream.get_encoded_bytes_for_frame(frame)
19 | yield (b'--frame\r\n'
20 | b'Content-Type: image/jpeg\r\n\r\n' + encoded_bytes + b'\r\n')
21 |
22 | @app.route('/display')
23 | def display():
24 | return Response(frame_generator(),
25 | mimetype='multipart/x-mixed-replace; boundary=frame')
26 |
27 | app.run(host="0.0.0.0", debug=True, port=5001)
28 |
--------------------------------------------------------------------------------
/chapter13/pid_controller.py:
--------------------------------------------------------------------------------
1 | import logging
2 | logger = logging.getLogger("pid_controller")
3 |
4 | class PIController:
5 | def __init__(self, proportional_constant=0, integral_constant=0, windup_limit=None):
6 | self.proportional_constant = proportional_constant
7 | self.integral_constant = integral_constant
8 | self.windup_limit = windup_limit
9 | # Running sums
10 | self.integral_sum = 0
11 |
12 | def handle_proportional(self, error):
13 | return self.proportional_constant * error
14 |
15 | def handle_integral(self, error):
16 | if self.windup_limit is None or \
17 | (abs(self.integral_sum) < self.windup_limit) or \
18 | ((error > 0) != (self.integral_sum > 0)):
19 | self.integral_sum += error
20 | return self.integral_constant * self.integral_sum
21 |
22 | def get_value(self, error):
23 | p = self.handle_proportional(error)
24 | i = self.handle_integral(error)
25 | logger.debug(f"P: {p}, I: {i:.2f}")
26 | return p + i
27 |
28 | def reset(self):
29 | self.integral_sum = 0
30 |
--------------------------------------------------------------------------------
/chapter13/templates/color_track_behavior.html:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 | Robot Image Server
5 |
6 |
7 | Robot Image Server
8 |  }})
9 | Start
10 | Stop
11 | Exit
12 |
13 |
14 |
--------------------------------------------------------------------------------
/chapter13/templates/control_image_behavior.html:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 | Robot Image Server
5 |
6 |
7 | Robot Image Server
8 |
9 | Exit
10 |
11 |
12 |
--------------------------------------------------------------------------------
/chapter13/templates/image_server.html:
--------------------------------------------------------------------------------
1 |
2 |
3 | Robot Image Server
4 |
5 |
6 | Robot Image Server
7 |
8 |
9 |
10 |
--------------------------------------------------------------------------------
/chapter14/camera_stream.py:
--------------------------------------------------------------------------------
1 | from picamera.array import PiRGBArray
2 | from picamera import PiCamera
3 | import numpy as np
4 |
5 | import cv2
6 |
7 | size = (320, 240)
8 | encode_param = [int(cv2.IMWRITE_JPEG_QUALITY), 90]
9 |
10 |
11 | def setup_camera():
12 | camera = PiCamera()
13 | camera.resolution = size
14 | camera.rotation = 180
15 | return camera
16 |
17 |
18 | def start_stream(camera):
19 | image_storage = PiRGBArray(camera, size=size)
20 |
21 | cam_stream = camera.capture_continuous(image_storage, format="bgr", use_video_port=True)
22 | for raw_frame in cam_stream:
23 | yield raw_frame.array
24 | image_storage.truncate(0)
25 |
26 |
27 | def get_encoded_bytes_for_frame(frame):
28 | result, encoded_image = cv2.imencode('.jpg', frame, encode_param)
29 | return encoded_image.tostring()
30 |
--------------------------------------------------------------------------------
/chapter14/image_app_core.py:
--------------------------------------------------------------------------------
1 | import time
2 | from multiprocessing import Process, Queue
3 |
4 | from flask import Flask, render_template, Response, request
5 |
6 | app = Flask(__name__)
7 | control_queue = Queue()
8 | display_queue = Queue(maxsize=2)
9 | display_template = 'image_server.html'
10 |
11 | @app.route('/')
12 | def index():
13 | return render_template(display_template)
14 |
15 | def frame_generator():
16 | while True:
17 | time.sleep(0.05)
18 | encoded_bytes = display_queue.get()
19 | yield (b'--frame\r\n'
20 | b'Content-Type: image/jpeg\r\n\r\n' + encoded_bytes + b'\r\n')
21 |
22 | @app.route('/display')
23 | def display():
24 | return Response(frame_generator(),
25 | mimetype='multipart/x-mixed-replace; boundary=frame')
26 |
27 | @app.route('/control', methods=['POST'])
28 | def control():
29 | control_queue.put(request.form)
30 | return Response('queued')
31 |
32 | def start_server_process(template_name):
33 | """Start the process, call .terminate to close it"""
34 | global display_template
35 | display_template = template_name
36 | server = Process(target=app.run, kwargs={"host": "0.0.0.0", "port": 5001})
37 | server.start()
38 | return server
39 |
40 | def put_output_image(encoded_bytes):
41 | """Queue an output image"""
42 | if display_queue.empty():
43 | display_queue.put(encoded_bytes)
44 |
45 | def get_control_instruction():
46 | if control_queue.empty():
47 | return None
48 | else:
49 | return control_queue.get()
50 |
--------------------------------------------------------------------------------
/chapter14/pid_controller.py:
--------------------------------------------------------------------------------
1 | import logging
2 | logger = logging.getLogger("pid_controller")
3 |
4 | class PIController:
5 | def __init__(self, proportional_constant=0, integral_constant=0, windup_limit=None):
6 | self.proportional_constant = proportional_constant
7 | self.integral_constant = integral_constant
8 | self.windup_limit = windup_limit
9 | # Running sums
10 | self.integral_sum = 0
11 |
12 | def handle_proportional(self, error):
13 | return self.proportional_constant * error
14 |
15 | def handle_integral(self, error, delta_time):
16 | if self.windup_limit is None or \
17 | (abs(self.integral_sum) < self.windup_limit) or \
18 | ((error > 0) != (self.integral_sum > 0)):
19 | self.integral_sum += error * delta_time
20 | return self.integral_constant * self.integral_sum
21 |
22 | def get_value(self, error, delta_time=1):
23 | p = self.handle_proportional(error)
24 | i = self.handle_integral(error, delta_time)
25 | logger.debug(f"P: {p}, I: {i:.2f}")
26 | return p + i
27 |
28 | def reset(self):
29 | self.integral_sum = 0
30 |
--------------------------------------------------------------------------------
/chapter14/robot.py:
--------------------------------------------------------------------------------
1 | from Raspi_MotorHAT import Raspi_MotorHAT
2 | from gpiozero import DistanceSensor
3 |
4 | import atexit
5 | import leds_led_shim
6 | from servos import Servos
7 | from encoder_counter import EncoderCounter
8 |
9 |
10 | class Robot:
11 | wheel_diameter_mm = 70.0
12 | ticks_per_revolution = 40.0
13 | wheel_distance_mm = 132.0
14 | def __init__(self, motorhat_addr=0x6f):
15 | # Setup the motorhat with the passed in address
16 | self._mh = Raspi_MotorHAT(addr=motorhat_addr)
17 |
18 | self.left_motor = self._mh.getMotor(1)
19 | self.right_motor = self._mh.getMotor(2)
20 |
21 | # Setup the Leds
22 | self.leds = leds_led_shim.Leds()
23 | # Set up servo motors for pan and tilt.
24 | self.servos = Servos(addr=motorhat_addr, deflect_90_in_ms=1)
25 |
26 | # Setup The Distance Sensors
27 | self.left_distance_sensor = DistanceSensor(echo=17, trigger=27, queue_len=2)
28 | self.right_distance_sensor = DistanceSensor(echo=5, trigger=6, queue_len=2)
29 |
30 | # Setup the Encoders
31 | EncoderCounter.set_constants(self.wheel_diameter_mm, self.ticks_per_revolution)
32 | self.left_encoder = EncoderCounter(4)
33 | self.right_encoder = EncoderCounter(26)
34 |
35 | # ensure the motors get stopped when the code exits
36 | atexit.register(self.stop_all)
37 |
38 | def convert_speed(self, speed):
39 | # Choose the running mode
40 | mode = Raspi_MotorHAT.RELEASE
41 | if speed > 0:
42 | mode = Raspi_MotorHAT.FORWARD
43 | elif speed < 0:
44 | mode = Raspi_MotorHAT.BACKWARD
45 |
46 | # Scale the speed
47 | output_speed = (abs(speed) * 255) // 100
48 | return mode, int(output_speed)
49 |
50 | def set_left(self, speed):
51 | mode, output_speed = self.convert_speed(speed)
52 | self.left_motor.setSpeed(output_speed)
53 | self.left_motor.run(mode)
54 |
55 | def set_right(self, speed):
56 | mode, output_speed = self.convert_speed(speed)
57 | self.right_motor.setSpeed(output_speed)
58 | self.right_motor.run(mode)
59 |
60 | def stop_motors(self):
61 | self.left_motor.run(Raspi_MotorHAT.RELEASE)
62 | self.right_motor.run(Raspi_MotorHAT.RELEASE)
63 |
64 | def stop_all(self):
65 | self.stop_motors()
66 |
67 | # Clear the display
68 | self.leds.clear()
69 | self.leds.show()
70 |
71 | # Clear any sensor handlers
72 | self.left_encoder.stop()
73 | self.right_encoder.stop()
74 |
75 | # Reset the servos
76 | self.servos.stop_all()
77 |
78 | def set_pan(self, angle):
79 | self.servos.set_servo_angle(1, angle)
80 |
81 | def set_tilt(self, angle):
82 | self.servos.set_servo_angle(0, angle)
83 |
84 |
--------------------------------------------------------------------------------
/chapter14/servos.py:
--------------------------------------------------------------------------------
1 | from Raspi_MotorHAT.Raspi_PWM_Servo_Driver import PWM
2 |
3 | class Servos:
4 | def __init__(self, addr=0x6f, deflect_90_in_ms = 0.5):
5 | """addr: The i2c address of the PWM chip.
6 | deflect_90_in_ms: set this to calibrate the servo motors.
7 | it is what a deflection of 90 degrees is
8 | in terms of a pulse length in milliseconds."""
9 | self._pwm = PWM(addr)
10 | # This sets the timebase for it all
11 | pwm_frequency = 100
12 | self._pwm.setPWMFreq(pwm_frequency)
13 | # Mid-point of the servo pulse length in milliseconds.
14 | servo_mid_point_ms = 1.5
15 | # Frequency is 1/period, but working ms, we can use 1000
16 | period_in_ms = 1000 / pwm_frequency
17 | # The chip has 4096 steps in each period.
18 | pulse_steps = 4096
19 | # Steps for every millisecond.
20 | steps_per_ms = pulse_steps / period_in_ms
21 | # Steps for a degree
22 | self.steps_per_degree = (deflect_90_in_ms * steps_per_ms) / 90
23 | # Mid-point of the servo in steps
24 | self.servo_mid_point_steps = servo_mid_point_ms * steps_per_ms
25 |
26 | # Map for channels
27 | self.channels = [0, 1, 14, 15]
28 |
29 | def stop_all(self):
30 | # 0 in start is nothing, 4096 sets the OFF bit.
31 | self._pwm.setPWM(self.channels[0], 0, 4096)
32 | self._pwm.setPWM(self.channels[1], 0, 4096)
33 | self._pwm.setPWM(self.channels[2], 0, 4096)
34 | self._pwm.setPWM(self.channels[3], 0, 4096)
35 |
36 | def _convert_degrees_to_steps(self, position):
37 | return int(self.servo_mid_point_steps + (position * self.steps_per_degree))
38 |
39 | def set_servo_angle(self, channel, angle):
40 | """position: The position in degrees from the center. -90 to 90"""
41 | # Validate
42 | if angle > 90 or angle < -90:
43 | raise ValueError("Angle outside of range")
44 | # Then set the position
45 | off_step = self._convert_degrees_to_steps(angle)
46 | self._pwm.setPWM(self.channels[channel], 0, off_step)
47 |
48 |
--------------------------------------------------------------------------------
/chapter14/templates/color_track_behavior.html:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 | Robot Image Server
5 |
6 |
7 | Robot Image Server
8 |
9 | Start
10 | Stop
11 | Exit
12 |
13 |
14 |
--------------------------------------------------------------------------------
/chapter14/test_line_find.py:
--------------------------------------------------------------------------------
1 | import cv2
2 | import numpy as np
3 | from matplotlib import pyplot as plt
4 |
5 | image = cv2.imread("carpet_line1 2.jpg")
6 | assert image is not None, "Unable to read file"
7 |
8 | resized = cv2.resize(image, (320, 240))
9 |
10 | gray = cv2.cvtColor(resized, cv2.COLOR_BGR2GRAY)
11 | blur = cv2.blur(gray, (5, 80))
12 | row = blur[180].astype(np.int32)
13 | diff = np.diff(row)
14 |
15 | max_d = np.amax(diff, 0)
16 | min_d = np.amin(diff, 0)
17 |
18 | highest = np.where(diff == max_d)[0][0]
19 | lowest = np.where(diff == min_d)[0][0]
20 | middle = (highest + lowest) // 2
21 |
22 | x = np.arange(len(diff))
23 | plt.plot(x, diff)
24 | plt.plot([lowest, lowest], [max_d, min_d], "g--")
25 | plt.plot([middle, middle], [max_d, min_d], "r-")
26 | plt.plot([highest, highest], [max_d, min_d], "g--")
27 | plt.savefig("carpet_line1_2_blur580.png")
28 |
--------------------------------------------------------------------------------
/chapter15/control_server.py:
--------------------------------------------------------------------------------
1 | from flask import Flask
2 |
3 | from robot_modes import RobotModes
4 | # A Flask App contains all its routes.
5 | app = Flask(__name__)
6 | # Prepare our robot modes for use
7 | mode_manager = RobotModes()
8 |
9 |
10 | @app.route("/run/", methods=['POST'])
11 | def run(mode_name):
12 | # Use our robot app to run something with this mode_name
13 | mode_manager.run(mode_name)
14 | return "%s running"
15 |
16 |
17 | @app.route("/stop", methods=['POST'])
18 | def stop():
19 | # Tell our system to stop the mode it's in.
20 | mode_manager.stop()
21 | return "Stopped"
22 |
23 |
24 | app.run(host="0.0.0.0", debug=True)
25 |
--------------------------------------------------------------------------------
/chapter15/my-robot-skill/__init__.py:
--------------------------------------------------------------------------------
1 | from adapt.intent import IntentBuilder
2 | from mycroft import MycroftSkill, intent_handler
3 | from mycroft.util.log import LOG
4 |
5 | import requests
6 |
7 |
8 | class MyRobot(MycroftSkill):
9 | def __init__(self):
10 | super().__init__()
11 | self.base_url = self.settings.get("base_url")
12 |
13 | @intent_handler(IntentBuilder("")
14 | .require("Robot")
15 | .require("TestRainbow"))
16 | def handle_test_rainbow(self, message):
17 | self.handle_control('/run/test_rainbow', 'TestingRainbow')
18 |
19 | @intent_handler(IntentBuilder("")
20 | .require("Robot")
21 | .require("stop"))
22 | def handle_stop(self, message):
23 | self.handle_control('/stop', 'stopping')
24 |
25 | def handle_control(self, end_point, dialog_verb):
26 | try:
27 | requests.post(self.base_url + end_point)
28 | self.speak_dialog('Robot')
29 | self.speak_dialog(dialog_verb)
30 | except:
31 | self.speak_dialog("UnableToReach")
32 | LOG.exception("Unable to reach the robot")
33 |
34 |
35 | def create_skill():
36 | return MyRobot()
37 |
38 |
--------------------------------------------------------------------------------
/chapter15/my-robot-skill/dialog/en-us/Robot.dialog:
--------------------------------------------------------------------------------
1 | The Robot
2 | Robot
3 |
--------------------------------------------------------------------------------
/chapter15/my-robot-skill/dialog/en-us/TestingRainbow.dialog:
--------------------------------------------------------------------------------
1 | is testing rainbows.
2 | is deploying rainbows.
3 | is starting rainbows.
4 | is lighting up.
5 |
--------------------------------------------------------------------------------
/chapter15/my-robot-skill/dialog/en-us/UnableToReach.dialog:
--------------------------------------------------------------------------------
1 | Sorry I cannot reach the robot.
2 | The robot is unreachable.
3 | Have you turned the robot on?
4 | Is the control server running on the robot?
5 |
--------------------------------------------------------------------------------
/chapter15/my-robot-skill/dialog/en-us/stopping.dialog:
--------------------------------------------------------------------------------
1 | is stopping.
2 | will stop.
3 |
--------------------------------------------------------------------------------
/chapter15/my-robot-skill/requirements.txt:
--------------------------------------------------------------------------------
1 | requests
2 |
--------------------------------------------------------------------------------
/chapter15/my-robot-skill/settingsmeta.json:
--------------------------------------------------------------------------------
1 | {
2 | "skillMetadata": {
3 | "sections": [
4 | {
5 | "name": "Robot",
6 | "fields": [
7 | {
8 | "name": "base_url",
9 | "type": "text",
10 | "label": "Base URL for the robot control server",
11 | "value": "http://myrobot.local:5000"
12 | }
13 | ]
14 | }
15 | ]
16 | }
17 | }
18 |
--------------------------------------------------------------------------------
/chapter15/my-robot-skill/vocab/en-us/TestRainbow.voc:
--------------------------------------------------------------------------------
1 | test rainbow
2 | test the leds
3 | deploy rainbows
4 | turn on the lights
5 |
--------------------------------------------------------------------------------
/chapter15/my-robot-skill/vocab/en-us/robot.voc:
--------------------------------------------------------------------------------
1 | robot
2 | my robot
3 | ask robot to
4 | tell the robot to
5 |
--------------------------------------------------------------------------------
/chapter15/my-robot-skill/vocab/en-us/stop.voc:
--------------------------------------------------------------------------------
1 | stop
2 | cease
3 | turn off
4 | stand down
--------------------------------------------------------------------------------
/chapter15/robot_modes.py:
--------------------------------------------------------------------------------
1 | import subprocess
2 |
3 | class RobotModes(object):
4 | """Our robot behaviors and tests as running modes"""
5 |
6 | # Mode config goes from a "mode_name" to a script to run. Configured for look up.
7 | mode_config = {
8 | "avoid_behavior": "avoid_with_rainbows.py",
9 | "circle_head": "circle_pan_tilt_behavior.py",
10 | "test_leds": "leds_test.py",
11 | "test_rainbow": "test_rainbow.py",
12 | "straight_line": "straight_line_drive.py",
13 | "square": "drive_square.py",
14 | "line_following": "line_follow_behavior.py",
15 | "color_track": "color_track_behavior.py",
16 | "face_track": "face_track_behavior.py",
17 | }
18 |
19 | def __init__(self):
20 | self.current_process = None
21 |
22 | def is_running(self):
23 | """Check if there is a process running. Returncode is only set when a process finishes"""
24 | return self.current_process and self.current_process.returncode is None
25 |
26 | def run(self, mode_name):
27 | """Run the mode as a subprocess, but not if we still have one running"""
28 | if not self.is_running():
29 | script = self.mode_config[mode_name]
30 | self.current_process = subprocess.Popen(["python3", script])
31 | return True
32 | return False
33 |
34 | def stop(self):
35 | """Stop a process"""
36 | if self.is_running():
37 | # Sending the signal sigint is (on Linux) similar to pressing ctrl-c.
38 | # That causes the behavior to clean up and exit.
39 | self.current_process.send_signal(subprocess.signal.SIGINT)
40 | self.current_process = None
41 |
--------------------------------------------------------------------------------
/chapter16/calibrate_gyro.py:
--------------------------------------------------------------------------------
1 | from robot_imu import RobotImu
2 | import time
3 | import vpython as vp
4 |
5 | imu = RobotImu()
6 |
7 | gyro_min = vp.vector(0, 0, 0)
8 | gyro_max = vp.vector(0, 0, 0)
9 |
10 | for n in range(500):
11 | gyro = imu.read_gyroscope()
12 | gyro_min.x = min(gyro_min.x, gyro.x)
13 | gyro_min.y = min(gyro_min.y, gyro.y)
14 | gyro_min.z = min(gyro_min.z, gyro.z)
15 |
16 | gyro_max.x = max(gyro_max.x, gyro.x)
17 | gyro_max.y = max(gyro_max.y, gyro.y)
18 | gyro_max.z = max(gyro_max.z, gyro.z)
19 |
20 | offset = (gyro_min + gyro_max) / 2
21 |
22 | time.sleep(.01)
23 |
24 | print(f"Zero offset: {offset}.")
25 |
--------------------------------------------------------------------------------
/chapter16/delta_timer.py:
--------------------------------------------------------------------------------
1 | import time
2 |
3 |
4 | class DeltaTimer:
5 | def __init__(self):
6 | self.last = self.start = time.time()
7 |
8 | def update(self):
9 | current = time.time()
10 | dt = current - self.last
11 | elapsed = current - self.start
12 | self.last = current
13 | return dt, elapsed
--------------------------------------------------------------------------------
/chapter16/drive_north_behavior.py:
--------------------------------------------------------------------------------
1 | """This behavior will turn to seek north, and then drive that way"""
2 | from robot_imu import RobotImu, ImuFusion
3 | from delta_timer import DeltaTimer
4 | from pid_controller import PIController
5 | from robot import Robot
6 | import imu_settings
7 |
8 |
9 | imu = RobotImu(mag_offsets=imu_settings.mag_offsets,
10 | gyro_offsets=imu_settings.gyro_offsets)
11 | fusion = ImuFusion(imu)
12 | timer = DeltaTimer()
13 | pid = PIController(0.7, 0.01)
14 | robot = Robot()
15 | base_speed = 70
16 |
17 | # Lets head for this heading
18 | heading_set_point = 0
19 |
20 | while True:
21 | dt, elapsed = timer.update()
22 | fusion.update(dt)
23 | heading_error = fusion.yaw - heading_set_point
24 | steer_value = pid.get_value(heading_error, delta_time=dt)
25 | print(f"Error: {heading_error}, Value:{steer_value:2f}, t: {elapsed}")
26 | robot.set_left(base_speed + steer_value)
27 | robot.set_right(base_speed - steer_value)
28 |
--------------------------------------------------------------------------------
/chapter16/imu_settings.py:
--------------------------------------------------------------------------------
1 | from vpython import vector
2 | gyro_offsets = vector(-0.583969, 0.675573, -0.530534)
3 | mag_offsets = vector(7.725, 9.6, -25.275)
4 |
--------------------------------------------------------------------------------
/chapter16/magnetometer_calibration.py:
--------------------------------------------------------------------------------
1 | import vpython as vp
2 | from robot_imu import RobotImu
3 | from imu_settings import mag_offsets
4 |
5 | imu = RobotImu(mag_offsets=mag_offsets)
6 |
7 | mag_min = vp.vector(0, 0, 0)
8 | mag_max = vp.vector(0, 0, 0)
9 |
10 |
11 | scatter_xy = vp.gdots(color=vp.color.red)
12 | scatter_yz = vp.gdots(color=vp.color.green)
13 | scatter_zx = vp.gdots(color=vp.color.blue)
14 |
15 | while True:
16 | vp.rate(100)
17 | mag = imu.read_magnetometer()
18 |
19 | mag_min.x = min(mag_min.x, mag.x)
20 | mag_min.y = min(mag_min.y, mag.y)
21 | mag_min.z = min(mag_min.z, mag.z)
22 |
23 | mag_max.x = max(mag_max.x, mag.x)
24 | mag_max.y = max(mag_max.y, mag.y)
25 | mag_max.z = max(mag_max.z, mag.z)
26 | offset = (mag_max + mag_min) / 2
27 |
28 | print(f"Magnetometer: {mag}. Offsets: {offset}.")
29 | scatter_xy.plot(mag.x, mag.y)
30 | scatter_yz.plot(mag.y, mag.z)
31 | scatter_zx.plot(mag.z, mag.x)
32 |
--------------------------------------------------------------------------------
/chapter16/pid_controller.py:
--------------------------------------------------------------------------------
1 | import logging
2 | logger = logging.getLogger("pid_controller")
3 |
4 | class PIController:
5 | def __init__(self, proportional_constant=0, integral_constant=0, windup_limit=None):
6 | self.proportional_constant = proportional_constant
7 | self.integral_constant = integral_constant
8 | self.windup_limit = windup_limit
9 | # Running sums
10 | self.integral_sum = 0
11 |
12 | def handle_proportional(self, error):
13 | return self.proportional_constant * error
14 |
15 | def handle_integral(self, error, delta_time):
16 | if self.windup_limit is None or \
17 | (abs(self.integral_sum) < self.windup_limit) or \
18 | ((error > 0) != (self.integral_sum > 0)):
19 | self.integral_sum += error * delta_time
20 | return self.integral_constant * self.integral_sum
21 |
22 | def get_value(self, error, delta_time=1):
23 | p = self.handle_proportional(error)
24 | i = self.handle_integral(error, delta_time)
25 | logger.debug(f"P: {p}, I: {i:.2f}")
26 | return p + i
27 |
28 | def reset(self):
29 | self.integral_sum = 0
30 |
--------------------------------------------------------------------------------
/chapter16/plot_acc_gyro_fusion.py:
--------------------------------------------------------------------------------
1 | import vpython as vp
2 | from robot_imu import RobotImu, ImuFusion
3 | from delta_timer import DeltaTimer
4 | import imu_settings
5 |
6 | imu = RobotImu(gyro_offsets=imu_settings.gyro_offsets)
7 |
8 | fusion = ImuFusion(imu)
9 |
10 | vp.graph(xmin=0, xmax=60, scroll=True)
11 | graph_pitch = vp.gcurve(color=vp.color.red)
12 | graph_roll = vp.gcurve(color=vp.color.green)
13 |
14 |
15 | timer = DeltaTimer()
16 | while True:
17 | vp.rate(100)
18 | dt, elapsed = timer.update()
19 | fusion.update(dt)
20 | graph_pitch.plot(elapsed, fusion.pitch)
21 | graph_roll.plot(elapsed, fusion.roll)
22 |
--------------------------------------------------------------------------------
/chapter16/plot_accel_debug_pitch_and_roll.py:
--------------------------------------------------------------------------------
1 | import vpython as vp
2 | import logging
3 | import time
4 | from robot_imu import RobotImu
5 |
6 | logging.basicConfig(level=logging.INFO)
7 | imu = RobotImu()
8 |
9 |
10 | pr = vp.graph(xmin=0, xmax=60, scroll=True)
11 | graph_pitch = vp.gcurve(color=vp.color.red, graph=pr)
12 | graph_roll = vp.gcurve(color=vp.color.green, graph=pr)
13 |
14 | xyz = vp.graph(xmin=0, xmax=60, scroll=True)
15 | graph_x = vp.gcurve(color=vp.color.orange, graph=xyz)
16 | graph_y = vp.gcurve(color=vp.color.cyan, graph=xyz)
17 | graph_z = vp.gcurve(color=vp.color.purple, graph=xyz)
18 |
19 | start = time.time()
20 | while True:
21 | vp.rate(100)
22 | elapsed = time.time() - start
23 | pitch, roll = imu.read_accelerometer_pitch_and_roll()
24 | raw_accel = imu.read_accelerometer()
25 | graph_pitch.plot(elapsed, pitch)
26 | graph_roll.plot(elapsed, roll)
27 | print(f"Pitch: {pitch:.2f}, Roll: {roll:.2f}")
28 | graph_x.plot(elapsed, raw_accel.x)
29 | graph_y.plot(elapsed, raw_accel.y)
30 | graph_z.plot(elapsed, raw_accel.z)
31 |
--------------------------------------------------------------------------------
/chapter16/plot_accel_pitch_and_roll.py:
--------------------------------------------------------------------------------
1 | import vpython as vp
2 | import time
3 | from robot_imu import RobotImu
4 |
5 |
6 | imu = RobotImu()
7 |
8 | vp.graph(xmin=0, xmax=60, scroll=True)
9 | graph_pitch = vp.gcurve(color=vp.color.red)
10 | graph_roll = vp.gcurve(color=vp.color.green)
11 |
12 | start = time.time()
13 | while True:
14 | vp.rate(100)
15 | elapsed = time.time() - start
16 | pitch, roll = imu.read_accelerometer_pitch_and_roll()
17 | graph_pitch.plot(elapsed, pitch)
18 | graph_roll.plot(elapsed, roll)
19 |
--------------------------------------------------------------------------------
/chapter16/plot_calibrated_gyroscope.py:
--------------------------------------------------------------------------------
1 | import vpython as vp
2 | import time
3 | from robot_imu import RobotImu
4 | import imu_settings
5 |
6 | imu = RobotImu(gyro_offsets=imu_settings.gyro_offsets)
7 |
8 | vp.graph(xmin=0, xmax=60, ymax=360, ymin=-360, scroll=True)
9 | graph_x = vp.gcurve(color=vp.color.red)
10 | graph_y = vp.gcurve(color=vp.color.green)
11 | graph_z = vp.gcurve(color=vp.color.blue)
12 |
13 | start = time.time()
14 | while True:
15 | vp.rate(100)
16 | elapsed = time.time() - start
17 | gyro = imu.read_gyroscope()
18 | print(f"Gyro x: {gyro.x:.2f}, y: {gyro.y:.2f}, z: {gyro.z:.2f}")
19 | graph_x.plot(elapsed, gyro.x)
20 | graph_y.plot(elapsed, gyro.y)
21 | graph_z.plot(elapsed, gyro.z)
22 |
--------------------------------------------------------------------------------
/chapter16/plot_gyroscope.py:
--------------------------------------------------------------------------------
1 | import vpython as vp
2 | import time
3 | from robot_imu import RobotImu
4 |
5 | imu = RobotImu()
6 |
7 | vp.graph(xmin=0, xmax=60, scroll=True)
8 | graph_x = vp.gcurve(color=vp.color.red)
9 | graph_y = vp.gcurve(color=vp.color.green)
10 | graph_z = vp.gcurve(color=vp.color.blue)
11 |
12 | start = time.time()
13 | while True:
14 | vp.rate(100)
15 | elapsed = time.time() - start
16 | gyro = imu.read_gyroscope()
17 | print(f"Gyro x: {gyro.x:.2f}, y: {gyro.y:.2f}, z: {gyro.z:.2f}")
18 | graph_x.plot(elapsed, gyro.x)
19 | graph_y.plot(elapsed, gyro.y)
20 | graph_z.plot(elapsed, gyro.z)
21 |
--------------------------------------------------------------------------------
/chapter16/plot_integrated_gyro.py:
--------------------------------------------------------------------------------
1 | import vpython as vp
2 | import logging
3 | from robot_imu import RobotImu
4 | from delta_timer import DeltaTimer
5 | import imu_settings
6 |
7 |
8 | logging.basicConfig(level=logging.INFO)
9 | imu = RobotImu(gyro_offsets=imu_settings.gyro_offsets)
10 |
11 | vp.graph(xmin=0, xmax=60, scroll=True)
12 | graph_x = vp.gcurve(color=vp.color.red)
13 | graph_y = vp.gcurve(color=vp.color.green)
14 | graph_z = vp.gcurve(color=vp.color.blue)
15 |
16 | timer = DeltaTimer()
17 | pitch = 0
18 | roll = 0
19 | yaw = 0
20 |
21 | while True:
22 | vp.rate(100)
23 | dt, elapsed = timer.update()
24 | gyro = imu.read_gyroscope()
25 | roll += gyro.x * dt
26 | pitch += gyro.y * dt
27 | yaw += gyro.z * dt
28 | print(f"Elapsed: {elapsed:.2f}, Pitch: {pitch:.2f}, Roll: {roll:.2f}, Yaw: {yaw:.2f}")
29 | graph_x.plot(elapsed, pitch)
30 | graph_y.plot(elapsed, roll)
31 | graph_z.plot(elapsed, yaw)
32 |
--------------------------------------------------------------------------------
/chapter16/plot_mag_heading.py:
--------------------------------------------------------------------------------
1 | import vpython as vp
2 | from robot_imu import RobotImu
3 | from delta_timer import DeltaTimer
4 | import imu_settings
5 |
6 | imu = RobotImu(mag_offsets=imu_settings.mag_offsets)
7 |
8 | vp.cylinder(radius=1, axis=vp.vector(0, 0, 1),
9 | pos=vp.vector(0, 0, -1))
10 | needle = vp.arrow(axis=vp.vector(1, 0, 0),
11 | color=vp.color.red)
12 |
13 | vp.graph(xmin=0, xmax=60, scroll=True)
14 | graph_yaw = vp.gcurve(color=vp.color.blue)
15 | timer = DeltaTimer()
16 |
17 |
18 | while True:
19 | vp.rate(100)
20 | dt, elapsed = timer.update()
21 | mag = imu.read_magnetometer()
22 | yaw = -vp.atan2(mag.y, mag.x)
23 | graph_yaw.plot(elapsed, vp.degrees(yaw))
24 | needle.axis = vp.vector(vp.sin(yaw), vp.cos(yaw), 0)
25 |
--------------------------------------------------------------------------------
/chapter16/plot_pitch_and_roll.py:
--------------------------------------------------------------------------------
1 | import vpython as vp
2 | from robot_imu import RobotImu, ImuFusion
3 | from delta_timer import DeltaTimer
4 | import imu_settings
5 |
6 | imu = RobotImu(gyro_offsets=imu_settings.gyro_offsets)
7 | fusion = ImuFusion(imu)
8 |
9 | vp.graph(xmin=0, xmax=60, scroll=True)
10 | graph_pitch = vp.gcurve(color=vp.color.red)
11 | graph_roll = vp.gcurve(color=vp.color.green)
12 |
13 | timer = DeltaTimer()
14 | while True:
15 | vp.rate(100)
16 | dt, elapsed = timer.update()
17 | fusion.update(dt)
18 | graph_pitch.plot(elapsed, fusion.pitch)
19 | graph_roll.plot(elapsed, fusion.roll)
20 |
--------------------------------------------------------------------------------
/chapter16/robot.py:
--------------------------------------------------------------------------------
1 | from Raspi_MotorHAT import Raspi_MotorHAT
2 | from gpiozero import DistanceSensor
3 |
4 | import atexit
5 | import leds_led_shim
6 | from servos import Servos
7 | from encoder_counter import EncoderCounter
8 |
9 |
10 | class Robot:
11 | wheel_diameter_mm = 70.0
12 | ticks_per_revolution = 40.0
13 | wheel_distance_mm = 132.0
14 | def __init__(self, motorhat_addr=0x6f):
15 | # Setup the motorhat with the passed in address
16 | self._mh = Raspi_MotorHAT(addr=motorhat_addr)
17 |
18 | self.left_motor = self._mh.getMotor(1)
19 | self.right_motor = self._mh.getMotor(2)
20 |
21 | # Setup the Leds
22 | self.leds = leds_led_shim.Leds()
23 | # Set up servo motors for pan and tilt.
24 | self.servos = Servos(addr=motorhat_addr, deflect_90_in_ms=1)
25 |
26 | # Setup The Distance Sensors
27 | self.left_distance_sensor = DistanceSensor(echo=17, trigger=27, queue_len=2)
28 | self.right_distance_sensor = DistanceSensor(echo=5, trigger=6, queue_len=2)
29 |
30 | # Setup the Encoders
31 | EncoderCounter.set_constants(self.wheel_diameter_mm, self.ticks_per_revolution)
32 | self.left_encoder = EncoderCounter(4)
33 | self.right_encoder = EncoderCounter(26)
34 |
35 | # ensure the motors get stopped when the code exits
36 | atexit.register(self.stop_all)
37 |
38 | def convert_speed(self, speed):
39 | # Choose the running mode
40 | mode = Raspi_MotorHAT.RELEASE
41 | if speed > 0:
42 | mode = Raspi_MotorHAT.FORWARD
43 | elif speed < 0:
44 | mode = Raspi_MotorHAT.BACKWARD
45 |
46 | # Scale the speed
47 | output_speed = (abs(speed) * 255) // 100
48 | return mode, int(output_speed)
49 |
50 | def set_left(self, speed):
51 | mode, output_speed = self.convert_speed(speed)
52 | self.left_motor.setSpeed(output_speed)
53 | self.left_motor.run(mode)
54 |
55 | def set_right(self, speed):
56 | mode, output_speed = self.convert_speed(speed)
57 | self.right_motor.setSpeed(output_speed)
58 | self.right_motor.run(mode)
59 |
60 | def stop_motors(self):
61 | self.left_motor.run(Raspi_MotorHAT.RELEASE)
62 | self.right_motor.run(Raspi_MotorHAT.RELEASE)
63 |
64 | def stop_all(self):
65 | self.stop_motors()
66 |
67 | # Clear the display
68 | self.leds.clear()
69 | self.leds.show()
70 |
71 | # Clear any sensor handlers
72 | self.left_encoder.stop()
73 | self.right_encoder.stop()
74 |
75 | # Reset the servos
76 | self.servos.stop_all()
77 |
78 | def set_pan(self, angle):
79 | self.servos.set_servo_angle(1, angle)
80 |
81 | def set_tilt(self, angle):
82 | self.servos.set_servo_angle(0, angle)
83 |
84 |
--------------------------------------------------------------------------------
/chapter16/robot_pose.py:
--------------------------------------------------------------------------------
1 | import vpython as vp
2 |
3 |
4 | def robot_view():
5 | vp.scene.forward = vp.vector(-3, -1, -1)
6 | vp.scene.up = vp.vector(0, 0, 1)
7 |
--------------------------------------------------------------------------------
/chapter16/servos.py:
--------------------------------------------------------------------------------
1 | from Raspi_MotorHAT.Raspi_PWM_Servo_Driver import PWM
2 |
3 | class Servos:
4 | def __init__(self, addr=0x6f, deflect_90_in_ms = 0.5):
5 | """addr: The i2c address of the PWM chip.
6 | deflect_90_in_ms: set this to calibrate the servo motors.
7 | it is what a deflection of 90 degrees is
8 | in terms of a pulse length in milliseconds."""
9 | self._pwm = PWM(addr)
10 | # This sets the timebase for it all
11 | pwm_frequency = 100
12 | self._pwm.setPWMFreq(pwm_frequency)
13 | # Mid-point of the servo pulse length in milliseconds.
14 | servo_mid_point_ms = 1.5
15 | # Frequency is 1/period, but working ms, we can use 1000
16 | period_in_ms = 1000 / pwm_frequency
17 | # The chip has 4096 steps in each period.
18 | pulse_steps = 4096
19 | # Steps for every millisecond.
20 | steps_per_ms = pulse_steps / period_in_ms
21 | # Steps for a degree
22 | self.steps_per_degree = (deflect_90_in_ms * steps_per_ms) / 90
23 | # Mid-point of the servo in steps
24 | self.servo_mid_point_steps = servo_mid_point_ms * steps_per_ms
25 |
26 | # Map for channels
27 | self.channels = [0, 1, 14, 15]
28 |
29 | def stop_all(self):
30 | # 0 in start is nothing, 4096 sets the OFF bit.
31 | self._pwm.setPWM(self.channels[0], 0, 4096)
32 | self._pwm.setPWM(self.channels[1], 0, 4096)
33 | self._pwm.setPWM(self.channels[2], 0, 4096)
34 | self._pwm.setPWM(self.channels[3], 0, 4096)
35 |
36 | def _convert_degrees_to_steps(self, position):
37 | return int(self.servo_mid_point_steps + (position * self.steps_per_degree))
38 |
39 | def set_servo_angle(self, channel, angle):
40 | """position: The position in degrees from the center. -90 to 90"""
41 | # Validate
42 | if angle > 90 or angle < -90:
43 | raise ValueError("Angle outside of range")
44 | # Then set the position
45 | off_step = self._convert_degrees_to_steps(angle)
46 | self._pwm.setPWM(self.channels[channel], 0, off_step)
47 |
48 |
--------------------------------------------------------------------------------
/chapter16/virtual_robot.py:
--------------------------------------------------------------------------------
1 | import vpython as vp
2 | from robot_pose import robot_view
3 |
4 |
5 | def make_robot():
6 | chassis_width = 155 # left side to right
7 | chassis_thickness = 3 # plastic thickness
8 | chassis_length = 200 # front to back
9 | wheel_thickness = 26
10 | wheel_diameter = 70
11 | axle_x = 30 # wheel axle from
12 | axle_z = -20
13 | castor_position = vp.vector(-80, -6, -30)
14 | castor_radius = 14
15 | castor_thickness = 12
16 |
17 | base = vp.box(length=chassis_length,
18 | height=chassis_thickness,
19 | width=chassis_width)
20 | # rotate to match body - so Z is height and Y is width
21 | base.rotate(angle=vp.radians(90),
22 | axis=vp.vector(1, 0, 0))
23 | wheel_dist = chassis_width/2
24 | wheel_l = vp.cylinder(radius=wheel_diameter/2,
25 | length=wheel_thickness,
26 | pos=vp.vector(axle_x, -wheel_dist, axle_z),
27 | axis=vp.vector(0, -1, 0))
28 | wheel_r = vp.cylinder(radius=wheel_diameter/2,
29 | length=wheel_thickness,
30 | pos=vp.vector(axle_x, wheel_dist, axle_z),
31 | axis=vp.vector(0, 1, 0))
32 | castor = vp.cylinder(radius=castor_radius,
33 | length=castor_thickness,
34 | pos=castor_position,
35 | axis=vp.vector(0, 1, 0))
36 | return vp.compound([base, wheel_l, wheel_r, castor])
37 |
38 |
39 | if __name__ == "__main__":
40 | robot_view()
41 | x_arrow = vp.arrow(axis=vp.vector(200, 0, 0), color=vp.color.red)
42 | y_arrow = vp.arrow(axis=vp.vector(0, 200, 0), color=vp.color.green)
43 | z_arrow = vp.arrow(axis=vp.vector(0, 0, 200), color=vp.color.blue)
44 | robot=make_robot()
45 |
--------------------------------------------------------------------------------
/chapter16/visual_filtered_accelerometer.py:
--------------------------------------------------------------------------------
1 | import vpython as vp
2 | import logging
3 | from robot_imu import RobotImu, ComplementaryFilter
4 | import virtual_robot
5 |
6 | logging.basicConfig(level=logging.INFO)
7 | imu = RobotImu()
8 | model = virtual_robot.make_robot()
9 | virtual_robot.robot_view()
10 |
11 | filter = ComplementaryFilter(0.95).filter
12 |
13 | pitch = 0
14 | roll = 0
15 |
16 | while True:
17 | vp.rate(100)
18 |
19 | new_pitch, new_roll = imu.read_accelerometer_pitch_and_roll()
20 | pitch = filter(pitch, new_pitch)
21 | roll = filter(roll, new_roll)
22 |
23 | print(f"Pitch: {pitch:.2f}, Roll: {roll:.2f}")
24 | # reset the model
25 | model.up = vp.vector(0, 1, 0)
26 | model.axis = vp.vector(1, 0, 0)
27 | # Reposition it
28 | model.rotate(angle=vp.radians(roll), axis=vp.vector(1, 0, 0))
29 | model.rotate(angle=vp.radians(pitch), axis=vp.vector(0, 1, 0))
30 |
--------------------------------------------------------------------------------
/chapter16/visual_fusion.py:
--------------------------------------------------------------------------------
1 | import vpython as vp
2 | from robot_imu import RobotImu, ImuFusion
3 | from delta_timer import DeltaTimer
4 | import imu_settings
5 | import virtual_robot
6 |
7 | imu = RobotImu(gyro_offsets=imu_settings.gyro_offsets,
8 | mag_offsets=imu_settings.mag_offsets)
9 | fusion = ImuFusion(imu)
10 |
11 | robot_view = vp.canvas(align="left")
12 | model = virtual_robot.make_robot()
13 | virtual_robot.robot_view()
14 |
15 | compass = vp.canvas(width=400, height=400)
16 | vp.cylinder(radius=1, axis=vp.vector(0, 0, 1),
17 | pos=vp.vector(0, 0, -1))
18 | needle = vp.arrow(axis=vp.vector(1, 0, 0),
19 | color=vp.color.red)
20 |
21 |
22 | vp.graph(xmin=0, xmax=60, scroll=True)
23 | graph_roll = vp.gcurve(color=vp.color.red)
24 | graph_pitch = vp.gcurve(color=vp.color.green)
25 | graph_yaw = vp.gcurve(color=vp.color.blue)
26 |
27 | timer = DeltaTimer()
28 |
29 | while True:
30 | vp.rate(100)
31 | dt, elapsed = timer.update()
32 | fusion.update(dt)
33 | # reset the model
34 | model.up = vp.vector(0, 1, 0)
35 | model.axis = vp.vector(1, 0, 0)
36 | # Reposition it
37 | model.rotate(angle=vp.radians(fusion.roll), axis=vp.vector(1, 0, 0))
38 | model.rotate(angle=vp.radians(fusion.pitch), axis=vp.vector(0, 1, 0))
39 | model.rotate(angle=vp.radians(fusion.yaw), axis=vp.vector(0, 0, 1))
40 | needle.axis = vp.vector(
41 | vp.sin(vp.radians(fusion.yaw)),
42 | vp.cos(vp.radians(fusion.yaw)),
43 | 0)
44 | graph_roll.plot(elapsed, fusion.roll)
45 | graph_pitch.plot(elapsed, fusion.pitch)
46 | graph_yaw.plot(elapsed, fusion.yaw)
47 |
--------------------------------------------------------------------------------
/chapter16/visual_gyroscope.py:
--------------------------------------------------------------------------------
1 | import vpython as vp
2 | from robot_imu import RobotImu
3 | import time
4 | import imu_settings
5 | import virtual_robot
6 |
7 |
8 | imu = RobotImu(gyro_offsets=imu_settings.gyro_offsets)
9 |
10 | pitch = 0
11 | roll = 0
12 | yaw = 0
13 |
14 | model = virtual_robot.make_robot()
15 | virtual_robot.robot_view()
16 |
17 | latest = time.time()
18 |
19 | while True:
20 | vp.rate(1000)
21 | current = time.time()
22 | dt = current - latest
23 | latest = current
24 | gyro = imu.read_gyroscope()
25 | roll += gyro.x * dt
26 | pitch += gyro.y * dt
27 | yaw += gyro.z * dt
28 |
29 | # reset the model
30 | model.up = vp.vector(0, 1, 0)
31 | model.axis = vp.vector(1, 0, 0)
32 | # Reposition it
33 | model.rotate(angle=vp.radians(roll), axis=vp.vector(1, 0, 0))
34 | model.rotate(angle=vp.radians(pitch), axis=vp.vector(0, 1, 0))
35 | model.rotate(angle=vp.radians(yaw), axis=vp.vector(0, 0, 1))
36 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/accelerometer_vector.py:
--------------------------------------------------------------------------------
1 | import vpython as vp
2 | import logging
3 | from robot_imu import RobotImu
4 | from robot_pose import robot_view
5 |
6 | logging.basicConfig(level=logging.INFO)
7 | imu = RobotImu()
8 | robot_view()
9 |
10 | accel_arrow = vp.arrow(axis=vp.vector(0, 1, 0))
11 | x_arrow = vp.arrow(axis=vp.vector(1, 0, 0), color=vp.color.red)
12 | y_arrow = vp.arrow(axis=vp.vector(0, 1, 0), color=vp.color.green)
13 | z_arrow = vp.arrow(axis=vp.vector(0, 0, 1), color=vp.color.blue)
14 |
15 | while True:
16 | vp.rate(100)
17 | accel = imu.read_accelerometer()
18 | print(f"Accelerometer: {accel}")
19 | accel_arrow.axis = accel.norm()
20 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/avoid_with_rainbows.py:
--------------------------------------------------------------------------------
1 | from robot import Robot
2 | from time import sleep
3 | from led_rainbow import show_rainbow
4 |
5 |
6 | class ObstacleAvoidingBehavior:
7 | """Simple obstacle avoiding"""
8 | def __init__(self, the_robot):
9 | self.robot = the_robot
10 | self.speed = 60
11 | # Calculations for the LEDs
12 | self.led_half = int(self.robot.leds.count/2)
13 | self.sense_colour = 255, 0, 0
14 |
15 | def distance_to_led_bar(self, distance):
16 | # Invert so closer means more LED's.
17 | inverted = max(0, 1.0 - distance)
18 | led_bar = int(round(inverted * self.led_half)) + 1
19 | return led_bar
20 |
21 | def display_state(self, left_distance, right_distance):
22 | # Clear first
23 | self.robot.leds.clear()
24 | # Left side
25 | led_bar = self.distance_to_led_bar(left_distance)
26 | show_rainbow(self.robot.leds, range(led_bar))
27 | # Right side
28 | led_bar = self.distance_to_led_bar(right_distance)
29 | # Bit trickier - must go from below the leds count, to the leds count.
30 | start = (self.robot.leds.count - 1) - (led_bar)
31 | right_range = range(self.robot.leds.count - 1, start, -1)
32 | show_rainbow(self.robot.leds, right_range)
33 | # Now show this display
34 | self.robot.leds.show()
35 |
36 | def get_speeds(self, nearest_distance):
37 | if nearest_distance >= 1.0:
38 | nearest_speed = self.speed
39 | furthest_speed = self.speed
40 | delay = 100
41 | elif nearest_distance > 0.5:
42 | nearest_speed = self.speed
43 | furthest_speed = self.speed * 0.8
44 | delay = 100
45 | elif nearest_distance > 0.2:
46 | nearest_speed = self.speed
47 | furthest_speed = self.speed * 0.6
48 | delay = 100
49 | elif nearest_distance > 0.1:
50 | nearest_speed = -self.speed * 0.4
51 | furthest_speed = -self.speed
52 | delay = 100
53 | else: # collison
54 | nearest_speed = -self.speed
55 | furthest_speed = -self.speed
56 | delay = 250
57 | return nearest_speed, furthest_speed, delay
58 |
59 | def run(self):
60 | while True:
61 | # Get the sensor readings in meters
62 | left_distance = self.robot.left_distance_sensor.distance
63 | right_distance = self.robot.right_distance_sensor.distance
64 | # Display this
65 | self.display_state(left_distance, right_distance)
66 | # Get speeds for motors from distances
67 | nearest_speed, furthest_speed, delay = self.get_speeds(min(left_distance, right_distance))
68 | print(f"Distances: l {left_distance:.2f}, r {right_distance:.2f}. Speeds: n: {nearest_speed}, f: {furthest_speed}. Delay: {delay}")
69 | # and drive
70 | # Send this to the motors
71 | if left_distance < right_distance:
72 | self.robot.set_left(nearest_speed)
73 | self.robot.set_right(furthest_speed)
74 | else:
75 | self.robot.set_right(nearest_speed)
76 | self.robot.set_left(furthest_speed)
77 | # Wait our delay time
78 | sleep(delay * 0.001)
79 |
80 |
81 | bot = Robot()
82 | behavior = ObstacleAvoidingBehavior(bot)
83 | behavior.run()
84 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/behavior_line.py:
--------------------------------------------------------------------------------
1 | import robot
2 | from Raspi_MotorHAT import Raspi_MotorHAT
3 | from time import sleep
4 |
5 | r = robot.Robot()
6 | r.set_left(100)
7 | r.set_right(70)
8 | sleep(1)
9 |
10 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/behavior_path.py:
--------------------------------------------------------------------------------
1 | import robot
2 | from time import sleep
3 |
4 | def straight(bot, seconds):
5 | bot.set_left(100)
6 | bot.set_right(100)
7 | sleep(seconds)
8 |
9 | def turn_left(bot, seconds):
10 | bot.set_left(20)
11 | bot.set_right(80)
12 | sleep(seconds)
13 |
14 | def turn_right(bot, seconds):
15 | bot.set_left(80)
16 | bot.set_right(20)
17 | sleep(seconds)
18 |
19 | def spin_left(bot, seconds):
20 | bot.set_left(-100)
21 | bot.set_right(100)
22 | sleep(seconds)
23 |
24 | bot = robot.Robot()
25 | straight(bot, 1)
26 | turn_right(bot, 0.6)
27 | straight(bot, 0.6)
28 | turn_left(bot, 0.6)
29 | straight(bot, 0.6)
30 | turn_left(bot, 0.6)
31 | straight(bot, 0.3)
32 | spin_left(bot, 1)
33 |
34 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/calibrate_gyro.py:
--------------------------------------------------------------------------------
1 | from robot_imu import RobotImu
2 | import time
3 | import vpython as vp
4 |
5 | imu = RobotImu()
6 |
7 | gyro_min = vp.vector(0, 0, 0)
8 | gyro_max = vp.vector(0, 0, 0)
9 |
10 | for n in range(500):
11 | gyro = imu.read_gyroscope()
12 | gyro_min.x = min(gyro_min.x, gyro.x)
13 | gyro_min.y = min(gyro_min.y, gyro.y)
14 | gyro_min.z = min(gyro_min.z, gyro.z)
15 |
16 | gyro_max.x = max(gyro_max.x, gyro.x)
17 | gyro_max.y = max(gyro_max.y, gyro.y)
18 | gyro_max.z = max(gyro_max.z, gyro.z)
19 |
20 | offset = (gyro_min + gyro_max) / 2
21 |
22 | time.sleep(.01)
23 |
24 | print(f"Zero offset: {offset}.")
25 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/calibrate_magnetometer.py:
--------------------------------------------------------------------------------
1 | import vpython as vp
2 | from robot_imu import RobotImu
3 | from imu_settings import mag_offsets
4 |
5 | imu = RobotImu(mag_offsets=mag_offsets)
6 |
7 | mag_min = vp.vector(0, 0, 0)
8 | mag_max = vp.vector(0, 0, 0)
9 |
10 |
11 | scatter_xy = vp.gdots(color=vp.color.red)
12 | scatter_yz = vp.gdots(color=vp.color.green)
13 | scatter_zx = vp.gdots(color=vp.color.blue)
14 |
15 | while True:
16 | vp.rate(100)
17 | mag = imu.read_magnetometer()
18 |
19 | mag_min.x = min(mag_min.x, mag.x)
20 | mag_min.y = min(mag_min.y, mag.y)
21 | mag_min.z = min(mag_min.z, mag.z)
22 |
23 | mag_max.x = max(mag_max.x, mag.x)
24 | mag_max.y = max(mag_max.y, mag.y)
25 | mag_max.z = max(mag_max.z, mag.z)
26 | offset = (mag_max + mag_min) / 2
27 |
28 | print(f"Magnetometer: {mag}. Offsets: {offset}.")
29 | scatter_xy.plot(mag.x, mag.y)
30 | scatter_yz.plot(mag.y, mag.z)
31 | scatter_zx.plot(mag.z, mag.x)
32 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/camera_stream.py:
--------------------------------------------------------------------------------
1 | from picamera.array import PiRGBArray
2 | from picamera import PiCamera
3 | import numpy as np
4 |
5 | import cv2
6 |
7 | size = (320, 240)
8 | encode_param = [int(cv2.IMWRITE_JPEG_QUALITY), 90]
9 |
10 |
11 | def setup_camera():
12 | camera = PiCamera()
13 | camera.resolution = size
14 | camera.rotation = 180
15 | return camera
16 |
17 |
18 | def start_stream(camera):
19 | image_storage = PiRGBArray(camera, size=size)
20 |
21 | cam_stream = camera.capture_continuous(image_storage, format="bgr", use_video_port=True)
22 | for raw_frame in cam_stream:
23 | yield raw_frame.array
24 | image_storage.truncate(0)
25 |
26 |
27 | def get_encoded_bytes_for_frame(frame):
28 | result, encoded_image = cv2.imencode('.jpg', frame, encode_param)
29 | return encoded_image.tostring()
30 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/circle_pan_tilt_behavior.py:
--------------------------------------------------------------------------------
1 | from time import sleep
2 | import math
3 |
4 | from robot import Robot
5 | class CirclePanTiltBehavior:
6 | def __init__(self, the_robot):
7 | self.robot = the_robot
8 | self.current_time = 0
9 | self.frames_per_circle = 50
10 | self.radians_per_frame = (2 * math.pi) / self.frames_per_circle
11 | self.radius = 30
12 |
13 | def run(self):
14 | while True:
15 | frame_number = self.current_time % self.frames_per_circle
16 | frame_in_radians = frame_number * self.radians_per_frame
17 | self.robot.set_pan(self.radius * math.cos(frame_in_radians))
18 | self.robot.set_tilt(self.radius * math.sin(frame_in_radians))
19 | sleep(0.05)
20 | self.current_time += 1
21 |
22 | bot = Robot()
23 | behavior = CirclePanTiltBehavior(bot)
24 | behavior.run()
25 |
26 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/control_image_behavior.py:
--------------------------------------------------------------------------------
1 | import time
2 |
3 | from image_app_core import start_server_process, get_control_instruction, put_output_image
4 | import camera_stream
5 |
6 | def controlled_image_server_behavior():
7 | camera = camera_stream.setup_camera()
8 | time.sleep(0.1)
9 |
10 | for frame in camera_stream.start_stream(camera):
11 | encoded_bytes = camera_stream.get_encoded_bytes_for_frame(frame)
12 | put_output_image(encoded_bytes)
13 |
14 | instruction = get_control_instruction()
15 | if instruction and instruction['command'] == "exit":
16 | print("Stopping")
17 | return
18 |
19 | process = start_server_process('control_image_behavior.html')
20 |
21 | try:
22 | controlled_image_server_behavior()
23 | finally:
24 | process.terminate()
25 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/control_server.py:
--------------------------------------------------------------------------------
1 | from flask import Flask
2 |
3 | from robot_modes import RobotModes
4 | # A Flask App contains all its routes.
5 | app = Flask(__name__)
6 | # Prepare our robot modes for use
7 | mode_manager = RobotModes()
8 |
9 |
10 | @app.route("/run/", methods=['POST'])
11 | def run(mode_name):
12 | # Use our robot app to run something with this mode_name
13 | mode_manager.run(mode_name)
14 | return "%s running"
15 |
16 |
17 | @app.route("/stop", methods=['POST'])
18 | def stop():
19 | # Tell our system to stop the mode it's in.
20 | mode_manager.stop()
21 | return "Stopped"
22 |
23 |
24 | app.run(host="0.0.0.0", debug=True)
25 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/delta_timer.py:
--------------------------------------------------------------------------------
1 | import time
2 |
3 |
4 | class DeltaTimer:
5 | def __init__(self):
6 | self.last = self.start = time.time()
7 |
8 | def update(self):
9 | current = time.time()
10 | dt = current - self.last
11 | elapsed = current - self.start
12 | self.last = current
13 | return dt, elapsed
--------------------------------------------------------------------------------
/chapter17/0_starting_point/drive_distance.py:
--------------------------------------------------------------------------------
1 | from robot import Robot, EncoderCounter
2 | from pid_controller import PIController
3 | import time
4 | import logging
5 | logger = logging.getLogger("drive_distance")
6 |
7 | def drive_distance(bot, distance, speed=80):
8 | # Use left as "primary" motor, the right is keeping up
9 | set_primary = bot.set_left
10 | primary_encoder = bot.left_encoder
11 | set_secondary = bot.set_right
12 | secondary_encoder = bot.right_encoder
13 | controller = PIController(proportional_constant=5, integral_constant=0.3)
14 |
15 | # start the motors, and start the loop
16 | set_primary(speed)
17 | set_secondary(speed)
18 | while primary_encoder.pulse_count < distance or secondary_encoder.pulse_count < distance:
19 | time.sleep(0.01)
20 | # How far off are we?
21 | error = primary_encoder.pulse_count - secondary_encoder.pulse_count
22 | adjustment = controller.get_value(error)
23 | # How fast should the motor move to get there?
24 | set_primary(int(speed - adjustment))
25 | set_secondary(int(speed + adjustment))
26 | # Some debug
27 | logger.debug(f"Encoders: primary: {primary_encoder.pulse_count}, secondary: {secondary_encoder.pulse_count},"
28 | f"e:{error} adjustment: {adjustment:.2f}")
29 | logger.info(f"Distances: primary: {primary_encoder.distance_in_mm()} mm, secondary: {secondary_encoder.distance_in_mm()} mm")
30 |
31 | logging.basicConfig(level=logging.DEBUG)
32 | bot = Robot()
33 | distance_to_drive = 1000 # in mm - this is a meter
34 | distance_in_ticks = EncoderCounter.mm_to_ticks(distance_to_drive)
35 | drive_distance(bot, distance_in_ticks)
36 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/drive_north_behavior.py:
--------------------------------------------------------------------------------
1 | """This behavior will turn to seek north, and then drive that way"""
2 | from robot_imu import RobotImu, ImuFusion
3 | from delta_timer import DeltaTimer
4 | from pid_controller import PIController
5 | from robot import Robot
6 | import imu_settings
7 |
8 |
9 | imu = RobotImu(mag_offsets=imu_settings.mag_offsets,
10 | gyro_offsets=imu_settings.gyro_offsets)
11 | fusion = ImuFusion(imu)
12 | timer = DeltaTimer()
13 | pid = PIController(0.7, 0.01)
14 | robot = Robot()
15 | base_speed = 70
16 |
17 | # Lets head for this heading
18 | heading_set_point = 0
19 |
20 | while True:
21 | dt, elapsed = timer.update()
22 | fusion.update(dt)
23 | heading_error = fusion.yaw - heading_set_point
24 | steer_value = pid.get_value(heading_error, delta_time=dt)
25 | print(f"Error: {heading_error}, Value:{steer_value:2f}, t: {elapsed}")
26 | robot.set_left(base_speed + steer_value)
27 | robot.set_right(base_speed - steer_value)
28 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/encoder_counter.py:
--------------------------------------------------------------------------------
1 | from gpiozero import DigitalInputDevice
2 | import math
3 | class EncoderCounter:
4 | ticks_to_mm_const = None # you must set this up before using distance methods
5 | def __init__(self, pin_number):
6 | self.pulse_count = 0
7 | self.direction = 1
8 | self.device = DigitalInputDevice(pin=pin_number)
9 | self.device.pin.when_changed = self.when_changed
10 |
11 | def when_changed(self, time_ticks, state):
12 | self.pulse_count += self.direction
13 |
14 | def set_direction(self, direction):
15 | """This should be -1 or 1. """
16 | assert abs(direction)==1, "Direction %s should be 1 or -1" % direction
17 | self.direction = direction
18 |
19 | def reset(self):
20 | self.pulse_count = 0
21 |
22 | def stop(self):
23 | self.device.close()
24 |
25 | def distance_in_mm(self):
26 | return int(self.pulse_count * EncoderCounter.ticks_to_mm_const)
27 |
28 | @staticmethod
29 | def mm_to_ticks(mm):
30 | return mm / EncoderCounter.ticks_to_mm_const
31 |
32 | @staticmethod
33 | def set_constants(wheel_diameter_mm, ticks_per_revolution):
34 | EncoderCounter.ticks_to_mm_const = (math.pi / ticks_per_revolution) * wheel_diameter_mm
35 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/image_app_core.py:
--------------------------------------------------------------------------------
1 | import time
2 | from multiprocessing import Process, Queue
3 |
4 | from flask import Flask, render_template, Response, request
5 |
6 | app = Flask(__name__)
7 | control_queue = Queue()
8 | display_queue = Queue(maxsize=2)
9 | display_template = 'image_server.html'
10 |
11 | @app.route('/')
12 | def index():
13 | return render_template(display_template)
14 |
15 | def frame_generator():
16 | while True:
17 | time.sleep(0.05)
18 | encoded_bytes = display_queue.get()
19 | yield (b'--frame\r\n'
20 | b'Content-Type: image/jpeg\r\n\r\n' + encoded_bytes + b'\r\n')
21 |
22 | @app.route('/display')
23 | def display():
24 | return Response(frame_generator(),
25 | mimetype='multipart/x-mixed-replace; boundary=frame')
26 |
27 | @app.route('/control', methods=['POST'])
28 | def control():
29 | control_queue.put(request.form)
30 | return Response('queued')
31 |
32 | def start_server_process(template_name):
33 | """Start the process, call .terminate to close it"""
34 | global display_template
35 | display_template = template_name
36 | server = Process(target=app.run, kwargs={"host": "0.0.0.0", "port": 5001})
37 | server.start()
38 | return server
39 |
40 | def put_output_image(encoded_bytes):
41 | """Queue an output image"""
42 | if display_queue.empty():
43 | display_queue.put(encoded_bytes)
44 |
45 | def get_control_instruction():
46 | if control_queue.empty():
47 | return None
48 | else:
49 | return control_queue.get()
50 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/image_server.py:
--------------------------------------------------------------------------------
1 | from flask import Flask, render_template, Response
2 | import camera_stream
3 | import time
4 |
5 | app = Flask(__name__)
6 |
7 | @app.route('/')
8 | def index():
9 | return render_template('image_server.html')
10 |
11 | def frame_generator():
12 | """This is our main video feed"""
13 | camera = camera_stream.setup_camera()
14 |
15 | # allow the camera to warm up
16 | time.sleep(0.1)
17 | for frame in camera_stream.start_stream(camera):
18 | encoded_bytes = camera_stream.get_encoded_bytes_for_frame(frame)
19 | yield (b'--frame\r\n'
20 | b'Content-Type: image/jpeg\r\n\r\n' + encoded_bytes + b'\r\n')
21 |
22 | @app.route('/display')
23 | def display():
24 | return Response(frame_generator(),
25 | mimetype='multipart/x-mixed-replace; boundary=frame')
26 |
27 | app.run(host="0.0.0.0", debug=True, port=5001)
28 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/imu_settings.py:
--------------------------------------------------------------------------------
1 | from vpython import vector
2 | gyro_offsets = vector(-0.583969, 0.675573, -0.530534)
3 | mag_offsets = vector(7.725, 9.6, -25.275)
4 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/led_rainbow.py:
--------------------------------------------------------------------------------
1 |
2 | import colorsys
3 |
4 |
5 | def show_rainbow(leds, led_range):
6 | led_range = list(led_range)
7 | hue_step = 1.0 / len(led_range)
8 | for index, led_address in enumerate(led_range):
9 | hue = hue_step * index
10 | rgb = colorsys.hsv_to_rgb(hue, 1.0, 0.6)
11 | rgb = [int(c*255) for c in rgb]
12 | leds.set_one(led_address, rgb)
13 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/leds_led_shim.py:
--------------------------------------------------------------------------------
1 | import ledshim
2 |
3 |
4 | class Leds:
5 | @property
6 | def count(self):
7 | return ledshim.width
8 |
9 | def set_one(self, led_number, color):
10 | ledshim.set_pixel(led_number, *color)
11 |
12 | def set_range(self, led_range, color):
13 | for pixel in led_range:
14 | ledshim.set_pixel(pixel, *color)
15 |
16 | def set_all(self, color):
17 | ledshim.set_all(*color)
18 |
19 | def clear(self):
20 | ledshim.clear()
21 |
22 | def show(self):
23 | ledshim.show()
24 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/leds_test.py:
--------------------------------------------------------------------------------
1 | from robot import Robot
2 | from time import sleep
3 |
4 | bot = Robot()
5 | red = (255, 0, 0)
6 | blue = (0, 0, 255)
7 |
8 | while True:
9 | print("red")
10 | bot.leds.set_all(red)
11 | bot.leds.show()
12 | sleep(0.5)
13 | print("blue")
14 | bot.leds.set_all(blue)
15 | bot.leds.show()
16 | sleep(0.5)
17 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/magnetometer_vector.py:
--------------------------------------------------------------------------------
1 | import vpython as vp
2 | import logging
3 | from robot_imu import RobotImu
4 | from robot_pose import robot_view
5 |
6 | logging.basicConfig(level=logging.INFO)
7 | imu = RobotImu()
8 | robot_view()
9 |
10 | mag_arrow = vp.arrow(pos=vp.vector(0, 0, 0))
11 | x_arrow = vp.arrow(axis=vp.vector(1, 0, 0), color=vp.color.red)
12 | y_arrow = vp.arrow(axis=vp.vector(0, 1, 0), color=vp.color.green)
13 | z_arrow = vp.arrow(axis=vp.vector(0, 0, 1), color=vp.color.blue)
14 |
15 | while True:
16 | vp.rate(100)
17 |
18 | mag = imu.read_magnetometer()
19 | mag_arrow.axis = mag.norm()
20 | print(f"Magnetometer: {mag}")
21 |
22 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/my-robot-skill/__init__.py:
--------------------------------------------------------------------------------
1 | from adapt.intent import IntentBuilder
2 | from mycroft import MycroftSkill, intent_handler
3 | from mycroft.util.log import LOG
4 |
5 | import requests
6 |
7 |
8 | class MyRobot(MycroftSkill):
9 | def __init__(self):
10 | super().__init__()
11 | self.base_url = self.settings.get("base_url")
12 |
13 | @intent_handler(IntentBuilder("")
14 | .require("Robot")
15 | .require("TestRainbow"))
16 | def handle_test_rainbow(self, message):
17 | self.handle_control('/run/test_rainbow', 'TestingRainbow')
18 |
19 | @intent_handler(IntentBuilder("")
20 | .require("Robot")
21 | .require("stop"))
22 | def handle_stop(self, message):
23 | self.handle_control('/stop', 'stopping')
24 |
25 | def handle_control(self, end_point, dialog_verb):
26 | try:
27 | requests.post(self.base_url + end_point)
28 | self.speak_dialog('Robot')
29 | self.speak_dialog(dialog_verb)
30 | except:
31 | self.speak_dialog("UnableToReach")
32 | LOG.exception("Unable to reach the robot")
33 |
34 |
35 | def create_skill():
36 | return MyRobot()
37 |
38 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/my-robot-skill/dialog/en-us/Robot.dialog:
--------------------------------------------------------------------------------
1 | The Robot
2 | Robot
3 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/my-robot-skill/dialog/en-us/TestingRainbow.dialog:
--------------------------------------------------------------------------------
1 | is testing rainbows.
2 | is deploying rainbows.
3 | is starting rainbows.
4 | is lighting up.
5 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/my-robot-skill/dialog/en-us/UnableToReach.dialog:
--------------------------------------------------------------------------------
1 | Sorry I cannot reach the robot.
2 | The robot is unreachable.
3 | Have you turned the robot on?
4 | Is the control server running on the robot?
5 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/my-robot-skill/dialog/en-us/stopping.dialog:
--------------------------------------------------------------------------------
1 | is stopping.
2 | will stop.
3 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/my-robot-skill/requirements.txt:
--------------------------------------------------------------------------------
1 | requests
2 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/my-robot-skill/settingsmeta.json:
--------------------------------------------------------------------------------
1 | {
2 | "base_url": "http://myrobot.local:5000",
3 | "skillMetadata": {
4 | "sections": [
5 | {
6 | "name": "Robot",
7 | "fields": [
8 | {
9 | "name": "base_url",
10 | "type": "text",
11 | "label": "Base URL for the robot control server",
12 | "value": "https://myrobots.local:5000"
13 | }
14 | ]
15 | }
16 | ]
17 | }
18 | }
19 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/my-robot-skill/vocab/en-us/TestRainbow.voc:
--------------------------------------------------------------------------------
1 | test rainbow
2 | test the leds
3 | deploy rainbows
4 | turn on the lights
5 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/my-robot-skill/vocab/en-us/robot.voc:
--------------------------------------------------------------------------------
1 | robot
2 | my robot
3 | ask robot to
4 | tell the robot to
5 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/my-robot-skill/vocab/en-us/stop.voc:
--------------------------------------------------------------------------------
1 | stop
2 | cease
3 | turn off
4 | stand down
--------------------------------------------------------------------------------
/chapter17/0_starting_point/pid_controller.py:
--------------------------------------------------------------------------------
1 | import logging
2 | logger = logging.getLogger("pid_controller")
3 |
4 | class PIController:
5 | def __init__(self, proportional_constant=0, integral_constant=0, windup_limit=None):
6 | self.proportional_constant = proportional_constant
7 | self.integral_constant = integral_constant
8 | self.windup_limit = windup_limit
9 | # Running sums
10 | self.integral_sum = 0
11 |
12 | def handle_proportional(self, error):
13 | return self.proportional_constant * error
14 |
15 | def handle_integral(self, error, delta_time):
16 | if self.windup_limit is None or \
17 | (abs(self.integral_sum) < self.windup_limit) or \
18 | ((error > 0) != (self.integral_sum > 0)):
19 | self.integral_sum += error * delta_time
20 | return self.integral_constant * self.integral_sum
21 |
22 | def get_value(self, error, delta_time=1):
23 | p = self.handle_proportional(error)
24 | i = self.handle_integral(error, delta_time)
25 | logger.debug(f"P: {p}, I: {i:.2f}")
26 | return p + i
27 |
28 | def reset(self):
29 | self.integral_sum = 0
30 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/plot_acc_gyro_fusion.py:
--------------------------------------------------------------------------------
1 | import vpython as vp
2 | from robot_imu import RobotImu, ImuFusion
3 | from delta_timer import DeltaTimer
4 | import imu_settings
5 |
6 | imu = RobotImu(gyro_offsets=imu_settings.gyro_offsets)
7 |
8 | fusion = ImuFusion(imu)
9 |
10 | vp.graph(xmin=0, xmax=60, scroll=True)
11 | graph_pitch = vp.gcurve(color=vp.color.red)
12 | graph_roll = vp.gcurve(color=vp.color.green)
13 |
14 |
15 | timer = DeltaTimer()
16 | while True:
17 | vp.rate(100)
18 | dt, elapsed = timer.update()
19 | fusion.update(dt)
20 | graph_pitch.plot(elapsed, fusion.pitch)
21 | graph_roll.plot(elapsed, fusion.roll)
22 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/plot_accel_debug_pitch_and_roll.py:
--------------------------------------------------------------------------------
1 | import vpython as vp
2 | import logging
3 | import time
4 | from robot_imu import RobotImu
5 |
6 | logging.basicConfig(level=logging.INFO)
7 | imu = RobotImu()
8 |
9 |
10 | pr = vp.graph(xmin=0, xmax=60, scroll=True)
11 | graph_pitch = vp.gcurve(color=vp.color.red, graph=pr)
12 | graph_roll = vp.gcurve(color=vp.color.green, graph=pr)
13 |
14 | xyz = vp.graph(xmin=0, xmax=60, scroll=True)
15 | graph_x = vp.gcurve(color=vp.color.orange, graph=xyz)
16 | graph_y = vp.gcurve(color=vp.color.cyan, graph=xyz)
17 | graph_z = vp.gcurve(color=vp.color.purple, graph=xyz)
18 |
19 | start = time.time()
20 | while True:
21 | vp.rate(100)
22 | elapsed = time.time() - start
23 | pitch, roll = imu.read_accelerometer_pitch_and_roll()
24 | raw_accel = imu.read_accelerometer()
25 | graph_pitch.plot(elapsed, pitch)
26 | graph_roll.plot(elapsed, roll)
27 | print(f"Pitch: {pitch:.2f}, Roll: {roll:.2f}")
28 | graph_x.plot(elapsed, raw_accel.x)
29 | graph_y.plot(elapsed, raw_accel.y)
30 | graph_z.plot(elapsed, raw_accel.z)
31 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/plot_accel_pitch_and_roll.py:
--------------------------------------------------------------------------------
1 | import vpython as vp
2 | import time
3 | from robot_imu import RobotImu
4 |
5 |
6 | imu = RobotImu()
7 |
8 | vp.graph(xmin=0, xmax=60, scroll=True)
9 | graph_pitch = vp.gcurve(color=vp.color.red)
10 | graph_roll = vp.gcurve(color=vp.color.green)
11 |
12 | start = time.time()
13 | while True:
14 | vp.rate(100)
15 | elapsed = time.time() - start
16 | pitch, roll = imu.read_accelerometer_pitch_and_roll()
17 | graph_pitch.plot(elapsed, pitch)
18 | graph_roll.plot(elapsed, roll)
19 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/plot_calibrated_gyroscope.py:
--------------------------------------------------------------------------------
1 | import vpython as vp
2 | import time
3 | from robot_imu import RobotImu
4 | import imu_settings
5 |
6 | imu = RobotImu(gyro_offsets=imu_settings.gyro_offsets)
7 |
8 | vp.graph(xmin=0, xmax=60, ymax=360, ymin=-360, scroll=True)
9 | graph_x = vp.gcurve(color=vp.color.red)
10 | graph_y = vp.gcurve(color=vp.color.green)
11 | graph_z = vp.gcurve(color=vp.color.blue)
12 |
13 | start = time.time()
14 | while True:
15 | vp.rate(100)
16 | elapsed = time.time() - start
17 | gyro = imu.read_gyroscope()
18 | print(f"Gyro x: {gyro.x:.2f}, y: {gyro.y:.2f}, z: {gyro.z:.2f}")
19 | graph_x.plot(elapsed, gyro.x)
20 | graph_y.plot(elapsed, gyro.y)
21 | graph_z.plot(elapsed, gyro.z)
22 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/plot_gyroscope.py:
--------------------------------------------------------------------------------
1 | import vpython as vp
2 | import time
3 | from robot_imu import RobotImu
4 |
5 | imu = RobotImu()
6 |
7 | vp.graph(xmin=0, xmax=60, scroll=True)
8 | graph_x = vp.gcurve(color=vp.color.red)
9 | graph_y = vp.gcurve(color=vp.color.green)
10 | graph_z = vp.gcurve(color=vp.color.blue)
11 |
12 | start = time.time()
13 | while True:
14 | vp.rate(100)
15 | elapsed = time.time() - start
16 | gyro = imu.read_gyroscope()
17 | print(f"Gyro x: {gyro.x:.2f}, y: {gyro.y:.2f}, z: {gyro.z:.2f}")
18 | graph_x.plot(elapsed, gyro.x)
19 | graph_y.plot(elapsed, gyro.y)
20 | graph_z.plot(elapsed, gyro.z)
21 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/plot_imu_fusion.py:
--------------------------------------------------------------------------------
1 | import vpython as vp
2 | from robot_imu import RobotImu, ImuFusion
3 | from delta_timer import DeltaTimer
4 | import imu_settings
5 |
6 | imu = RobotImu(gyro_offsets=imu_settings.gyro_offsets,
7 | mag_offsets=imu_settings.mag_offsets)
8 | fusion = ImuFusion(imu)
9 |
10 | vp.graph(xmin=0, xmax=60, scroll=True)
11 | graph_pitch = vp.gcurve(color=vp.color.red)
12 | graph_roll = vp.gcurve(color=vp.color.green)
13 | graph_yaw = vp.gcurve(color=vp.color.blue)
14 |
15 | timer = DeltaTimer()
16 | while True:
17 | vp.rate(100)
18 | dt, elapsed = timer.update()
19 | fusion.update(dt)
20 | graph_pitch.plot(elapsed, fusion.pitch)
21 | graph_roll.plot(elapsed, fusion.roll)
22 | graph_yaw.plot(elapsed, fusion.yaw)
23 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/plot_integrated_gyro.py:
--------------------------------------------------------------------------------
1 | import vpython as vp
2 | import logging
3 | from robot_imu import RobotImu
4 | from delta_timer import DeltaTimer
5 | import imu_settings
6 |
7 |
8 | logging.basicConfig(level=logging.INFO)
9 | imu = RobotImu(gyro_offsets=imu_settings.gyro_offsets)
10 |
11 | vp.graph(xmin=0, xmax=60, scroll=True)
12 | graph_x = vp.gcurve(color=vp.color.red)
13 | graph_y = vp.gcurve(color=vp.color.green)
14 | graph_z = vp.gcurve(color=vp.color.blue)
15 |
16 | timer = DeltaTimer()
17 | pitch = 0
18 | roll = 0
19 | yaw = 0
20 |
21 | while True:
22 | vp.rate(100)
23 | dt, elapsed = timer.update()
24 | gyro = imu.read_gyroscope()
25 | roll += gyro.x * dt
26 | pitch += gyro.y * dt
27 | yaw += gyro.z * dt
28 | print(f"Elapsed: {elapsed:.2f}, Pitch: {pitch:.2f}, Roll: {roll:.2f}, Yaw: {yaw:.2f}")
29 | graph_x.plot(elapsed, pitch)
30 | graph_y.plot(elapsed, roll)
31 | graph_z.plot(elapsed, yaw)
32 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/plot_mag_heading.py:
--------------------------------------------------------------------------------
1 | import vpython as vp
2 | from robot_imu import RobotImu
3 | from delta_timer import DeltaTimer
4 | import imu_settings
5 |
6 | imu = RobotImu(mag_offsets=imu_settings.mag_offsets)
7 |
8 | vp.graph(xmin=0, xmax=60, scroll=True)
9 | graph_yaw = vp.gcurve(color=vp.color.blue)
10 | timer = DeltaTimer()
11 |
12 | while True:
13 | vp.rate(100)
14 | dt, elapsed = timer.update()
15 | mag = imu.read_magnetometer()
16 | yaw = -vp.degrees(vp.atan2(mag.y, mag.x))
17 | graph_yaw.plot(elapsed, yaw)
18 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/plot_temperature.py:
--------------------------------------------------------------------------------
1 | """When running,
2 | use VPYTHON_PORT=9020 VPYTHON_NOBROWSER=true"""
3 | import vpython as vp
4 | import time
5 | import logging
6 | from robot_imu import RobotImu
7 |
8 |
9 | logging.basicConfig(level=logging.INFO)
10 | imu = RobotImu()
11 | vp.graph(xmin=0, xmax=60, scroll=True)
12 | temp_graph = vp.gcurve()
13 | start = time.time()
14 | while True:
15 | vp.rate(100)
16 | temperature = imu.read_temperature()
17 | logging.info("Temperature {}".format(temperature))
18 | elapsed = time.time() - start
19 | temp_graph.plot(elapsed, temperature)
20 |
21 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/robot.py:
--------------------------------------------------------------------------------
1 | from Raspi_MotorHAT import Raspi_MotorHAT
2 | from gpiozero import DistanceSensor
3 |
4 | import atexit
5 | import leds_led_shim
6 | from servos import Servos
7 | from encoder_counter import EncoderCounter
8 |
9 |
10 | class Robot:
11 | wheel_diameter_mm = 70.0
12 | ticks_per_revolution = 40.0
13 | wheel_distance_mm = 132.0
14 | def __init__(self, motorhat_addr=0x6f):
15 | # Setup the motorhat with the passed in address
16 | self._mh = Raspi_MotorHAT(addr=motorhat_addr)
17 |
18 | self.left_motor = self._mh.getMotor(1)
19 | self.right_motor = self._mh.getMotor(2)
20 |
21 | # Setup the Leds
22 | self.leds = leds_led_shim.Leds()
23 | # Set up servo motors for pan and tilt.
24 | self.servos = Servos(addr=motorhat_addr, deflect_90_in_ms=1)
25 |
26 | # Setup The Distance Sensors
27 | self.left_distance_sensor = DistanceSensor(echo=17, trigger=27, queue_len=2)
28 | self.right_distance_sensor = DistanceSensor(echo=5, trigger=6, queue_len=2)
29 |
30 | # Setup the Encoders
31 | EncoderCounter.set_constants(self.wheel_diameter_mm, self.ticks_per_revolution)
32 | self.left_encoder = EncoderCounter(4)
33 | self.right_encoder = EncoderCounter(26)
34 |
35 | # ensure the motors get stopped when the code exits
36 | atexit.register(self.stop_all)
37 |
38 | def convert_speed(self, speed):
39 | # Choose the running mode
40 | mode = Raspi_MotorHAT.RELEASE
41 | if speed > 0:
42 | mode = Raspi_MotorHAT.FORWARD
43 | elif speed < 0:
44 | mode = Raspi_MotorHAT.BACKWARD
45 |
46 | # Scale the speed
47 | output_speed = (abs(speed) * 255) // 100
48 | return mode, int(output_speed)
49 |
50 | def set_left(self, speed):
51 | mode, output_speed = self.convert_speed(speed)
52 | self.left_motor.setSpeed(output_speed)
53 | self.left_motor.run(mode)
54 |
55 | def set_right(self, speed):
56 | mode, output_speed = self.convert_speed(speed)
57 | self.right_motor.setSpeed(output_speed)
58 | self.right_motor.run(mode)
59 |
60 | def stop_motors(self):
61 | self.left_motor.run(Raspi_MotorHAT.RELEASE)
62 | self.right_motor.run(Raspi_MotorHAT.RELEASE)
63 |
64 | def stop_all(self):
65 | self.stop_motors()
66 |
67 | # Clear the display
68 | self.leds.clear()
69 | self.leds.show()
70 |
71 | # Clear any sensor handlers
72 | self.left_encoder.stop()
73 | self.right_encoder.stop()
74 |
75 | # Reset the servos
76 | self.servos.stop_all()
77 |
78 | def set_pan(self, angle):
79 | self.servos.set_servo_angle(1, angle)
80 |
81 | def set_tilt(self, angle):
82 | self.servos.set_servo_angle(0, angle)
83 |
84 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/robot_modes.py:
--------------------------------------------------------------------------------
1 | import subprocess
2 |
3 | class RobotModes(object):
4 | """Our robot behaviors and tests as running modes"""
5 |
6 | # Mode config goes from a "mode_name" to a script to run. Configured for look up.
7 | mode_config = {
8 | "avoid_behavior": "avoid_with_rainbows.py",
9 | "circle_head": "circle_pan_tilt_behavior.py",
10 | "test_leds": "leds_test.py",
11 | "test_rainbow": "test_rainbow.py",
12 | "straight_line": "straight_line_drive.py",
13 | "square": "drive_square.py",
14 | "line_following": "line_follow_behavior.py",
15 | "color_track": "color_track_behavior.py",
16 | "face_track": "face_track_behavior.py",
17 | }
18 |
19 | def __init__(self):
20 | self.current_process = None
21 |
22 | def is_running(self):
23 | """Check if there is a process running. Returncode is only set when a process finishes"""
24 | return self.current_process and self.current_process.returncode is None
25 |
26 | def run(self, mode_name):
27 | """Run the mode as a subprocess, but not if we still have one running"""
28 | if not self.is_running():
29 | script = self.mode_config[mode_name]
30 | self.current_process = subprocess.Popen(["python3", script])
31 | return True
32 | return False
33 |
34 | def stop(self):
35 | """Stop a process"""
36 | if self.is_running():
37 | # Sending the signal sigint is (on Linux) similar to pressing ctrl-c.
38 | # That causes the behavior to clean up and exit.
39 | self.current_process.send_signal(subprocess.signal.SIGINT)
40 | self.current_process = None
41 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/robot_pose.py:
--------------------------------------------------------------------------------
1 | import vpython as vp
2 |
3 |
4 | def robot_view():
5 | vp.scene.forward = vp.vector(-3, -1, -1)
6 | vp.scene.up = vp.vector(0, 0, 1)
7 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/servo_type_position.py:
--------------------------------------------------------------------------------
1 | from Raspi_MotorHAT.Raspi_PWM_Servo_Driver import PWM
2 | import atexit
3 |
4 | pwm = PWM(0x6f)
5 | # This sets the timebase for it all
6 | pwm_frequency = 100
7 | pwm.setPWMFreq(pwm_frequency)
8 | # Mid-point of the servo pulse length in milliseconds.
9 | servo_mid_point_ms = 1.5
10 | # What a deflection of 90 degrees is in pulse length in milliseconds
11 | deflect_90_in_ms = 0.5
12 | # Frequency is 1 divided by period, but working ms, we can use 1000
13 | period_in_ms = 1000 / pwm_frequency
14 | # The chip has 4096 steps in each period.
15 | pulse_steps = 4096
16 | # Steps for every millisecond.
17 | steps_per_ms = pulse_steps / period_in_ms
18 | # Steps for a degree.
19 | steps_per_degree = (deflect_90_in_ms * steps_per_ms) / 90
20 | # Mid-point of the servo in steps
21 | servo_mid_point_steps = servo_mid_point_ms * steps_per_ms
22 |
23 | def convert_degrees_to_steps(position):
24 | return int(servo_mid_point_steps + (position * steps_per_degree))
25 |
26 | atexit.register(pwm.setPWM, 0, 0, 4096)
27 |
28 | while True:
29 | position = int(input("Type your position in degrees (90 to -90, 0 is middle): "))
30 | end_step = convert_degrees_to_steps(position)
31 | pwm.setPWM(0, 0, end_step)
32 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/servos.py:
--------------------------------------------------------------------------------
1 | from Raspi_MotorHAT.Raspi_PWM_Servo_Driver import PWM
2 |
3 | class Servos:
4 | def __init__(self, addr=0x6f, deflect_90_in_ms = 0.5):
5 | """addr: The i2c address of the PWM chip.
6 | deflect_90_in_ms: set this to calibrate the servo motors.
7 | it is what a deflection of 90 degrees is
8 | in terms of a pulse length in milliseconds."""
9 | self._pwm = PWM(addr)
10 | # This sets the timebase for it all
11 | pwm_frequency = 100
12 | self._pwm.setPWMFreq(pwm_frequency)
13 | # Mid-point of the servo pulse length in milliseconds.
14 | servo_mid_point_ms = 1.5
15 | # Frequency is 1/period, but working ms, we can use 1000
16 | period_in_ms = 1000 / pwm_frequency
17 | # The chip has 4096 steps in each period.
18 | pulse_steps = 4096
19 | # Steps for every millisecond.
20 | steps_per_ms = pulse_steps / period_in_ms
21 | # Steps for a degree
22 | self.steps_per_degree = (deflect_90_in_ms * steps_per_ms) / 90
23 | # Mid-point of the servo in steps
24 | self.servo_mid_point_steps = servo_mid_point_ms * steps_per_ms
25 |
26 | # Map for channels
27 | self.channels = [0, 1, 14, 15]
28 |
29 | def stop_all(self):
30 | # 0 in start is nothing, 4096 sets the OFF bit.
31 | self._pwm.setPWM(self.channels[0], 0, 4096)
32 | self._pwm.setPWM(self.channels[1], 0, 4096)
33 | self._pwm.setPWM(self.channels[2], 0, 4096)
34 | self._pwm.setPWM(self.channels[3], 0, 4096)
35 |
36 | def _convert_degrees_to_steps(self, position):
37 | return int(self.servo_mid_point_steps + (position * self.steps_per_degree))
38 |
39 | def set_servo_angle(self, channel, angle):
40 | """position: The position in degrees from the center. -90 to 90"""
41 | # Validate
42 | if angle > 90 or angle < -90:
43 | raise ValueError("Angle outside of range")
44 | # Then set the position
45 | off_step = self._convert_degrees_to_steps(angle)
46 | self._pwm.setPWM(self.channels[channel], 0, off_step)
47 |
48 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/simple_avoid_behavior.py:
--------------------------------------------------------------------------------
1 | from robot import Robot
2 | from time import sleep
3 |
4 | class ObstacleAvoidingBehavior:
5 | """Simple obstacle avoiding"""
6 | def __init__(self, the_robot):
7 | self.robot = the_robot
8 | self.speed = 60
9 |
10 | def get_motor_speed(self, distance):
11 | """This method chooses a speed for a motor based on the distance from a sensor"""
12 | if distance < 0.2:
13 | return -self.speed
14 | else:
15 | return self.speed
16 |
17 | def run(self):
18 | while True:
19 | # Get the sensor readings in meters
20 | left_distance = self.robot.left_distance_sensor.distance
21 | right_distance = self.robot.right_distance_sensor.distance
22 | print("Left: {l:.2f}, Right: {r:.2f}".format(l=left_distance, r=right_distance))
23 | # and drive
24 | left_speed = self.get_motor_speed(left_distance)
25 | self.robot.set_right(left_speed)
26 | right_speed = self.get_motor_speed(right_distance)
27 | self.robot.set_left(right_speed)
28 | # Wait a little
29 | sleep(0.05)
30 |
31 | bot = Robot()
32 | behavior = ObstacleAvoidingBehavior(bot)
33 | behavior.run()
34 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/sonar_scan.py:
--------------------------------------------------------------------------------
1 | import time
2 | import math
3 |
4 | import matplotlib.pyplot as plt
5 |
6 | from robot import Robot
7 |
8 | start_scan =0
9 | lower_bound = -90
10 | upper_bound = 90
11 | scan_step = 5
12 |
13 | the_robot = Robot()
14 | the_robot.set_tilt(0)
15 |
16 | scan_data = {}
17 | # Make the sensor scan
18 | for facing in range(lower_bound, upper_bound, scan_step):
19 | the_robot.set_pan(-facing)
20 | time.sleep(0.1)
21 | scan_data[facing] = the_robot.left_distance_sensor.distance * 100
22 | # make plot
23 | axis = [math.radians(facing) for facing in scan_data.keys()]
24 | print(axis)
25 | print(scan_data.values())
26 | plt.polar(axis, list(scan_data.values()), 'g-')
27 | # dump to png
28 | plt.savefig("scan.png")
29 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/straight_line_drive.py:
--------------------------------------------------------------------------------
1 | from robot import Robot
2 | from pid_controller import PIController
3 | import time
4 | import logging
5 |
6 | logger = logging.getLogger("straight_line")
7 | logging.basicConfig(level=logging.INFO)
8 | logging.getLogger("pid_controller").setLevel(logging.DEBUG)
9 |
10 | bot = Robot()
11 | stop_at_time = time.time() + 15
12 |
13 | speed = 80
14 | bot.set_left(speed)
15 | bot.set_right(speed)
16 |
17 | pid = PIController(proportional_constant=4, integral_constant=0.3)
18 |
19 | while time.time() < stop_at_time:
20 | time.sleep(0.01)
21 | # Calculate the error
22 | left = bot.left_encoder.pulse_count
23 | right = bot.right_encoder.pulse_count
24 | error = left - right
25 |
26 | # Get the speed
27 | adjustment = pid.get_value(error)
28 | right_speed = int(speed + adjustment)
29 | left_speed = int(speed - adjustment)
30 |
31 | logger.debug(f"error: {error} adjustment: {adjustment:.2f}")
32 | logger.info(f"left: {left} right: {right}, left_speed: {left_speed} right_speed: {right_speed}")
33 | bot.set_left(left_speed)
34 | bot.set_right(right_speed)
35 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/templates/color_track_behavior.html:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 | Robot Image Server
5 |
6 |
7 | Robot Image Server
8 |
9 | Start
10 | Stop
11 | Exit
12 |
13 |
14 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/templates/control_image_behavior.html:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 | Robot Image Server
5 |
6 |
7 | Robot Image Server
8 |
9 | Exit
10 |
11 |
12 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/templates/image_server.html:
--------------------------------------------------------------------------------
1 |
2 |
3 | Robot Image Server
4 |
5 |
6 | Robot Image Server
7 |
8 |
9 |
10 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/test_distance_sensors.py:
--------------------------------------------------------------------------------
1 | import time
2 | from gpiozero import DistanceSensor
3 |
4 | print("Prepare GPIO Pins")
5 | sensor_l = DistanceSensor(echo=17, trigger=27, queue_len=2)
6 | sensor_r = DistanceSensor(echo=5, trigger=6, queue_len=2)
7 |
8 | while True:
9 | print("Left: {l:.2f}, Right: {r:.2f}".format(
10 | l=sensor_l.distance * 100,
11 | r=sensor_r.distance * 100))
12 | time.sleep(0.1)
13 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/test_distance_travelled.py:
--------------------------------------------------------------------------------
1 | from robot import Robot
2 | import time
3 | import math
4 | import logging
5 | logger = logging.getLogger("test_distance_travelled")
6 |
7 | wheel_diameter_mm = 70.0
8 | ticks_per_revolution = 40.0
9 | ticks_to_mm_const = (math.pi / ticks_per_revolution) * wheel_diameter_mm
10 |
11 | def ticks_to_mm(ticks):
12 | return int(ticks_to_mm_const * ticks)
13 |
14 | bot = Robot()
15 | stop_at_time = time.time() + 1
16 |
17 | logging.basicConfig(level=logging.INFO)
18 | bot.set_left(90)
19 | bot.set_right(90)
20 |
21 | while time.time() < stop_at_time:
22 | logger.info("Left: {} Right: {}".format(
23 | ticks_to_mm(bot.left_encoder.pulse_count),
24 | ticks_to_mm(bot.right_encoder.pulse_count)))
25 | time.sleep(0.05)
26 |
27 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/test_encoders.py:
--------------------------------------------------------------------------------
1 | from robot import Robot
2 | import time
3 | import logging
4 |
5 | from gpiozero import DigitalInputDevice
6 |
7 | logger = logging.getLogger("test_encoders")
8 |
9 | class EncoderCounter(object):
10 | def __init__(self, pin_number):
11 | self.pulse_count = 0
12 | self.device = DigitalInputDevice(pin=pin_number)
13 | self.device.pin.when_changed = self.when_changed
14 |
15 | def when_changed(self, time_ticks, state):
16 | self.pulse_count += 1
17 |
18 | bot = Robot()
19 | left_encoder = EncoderCounter(4)
20 | right_encoder = EncoderCounter(26)
21 |
22 | stop_at_time = time.time() + 1
23 |
24 | logging.basicConfig(level=logging.INFO)
25 | bot.set_left(90)
26 | bot.set_right(90)
27 | while time.time() < stop_at_time:
28 | logger.info(f"Left: {left_encoder.pulse_count} Right: {right_encoder.pulse_count}")
29 | time.sleep(0.05)
30 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/test_line_find.py:
--------------------------------------------------------------------------------
1 | import cv2
2 | import numpy as np
3 | from matplotlib import pyplot as plt
4 |
5 | image = cv2.imread("carpet_line1 2.jpg")
6 | assert image is not None, "Unable to read file"
7 |
8 | resized = cv2.resize(image, (320, 240))
9 |
10 | gray = cv2.cvtColor(resized, cv2.COLOR_BGR2GRAY)
11 | blur = cv2.blur(gray, (5, 80))
12 | row = blur[180].astype(np.int32)
13 | diff = np.diff(row)
14 |
15 | max_d = np.amax(diff, 0)
16 | min_d = np.amin(diff, 0)
17 |
18 | highest = np.where(diff == max_d)[0][0]
19 | lowest = np.where(diff == min_d)[0][0]
20 | middle = (highest + lowest) // 2
21 |
22 | x = np.arange(len(diff))
23 | plt.plot(x, diff)
24 | plt.plot([lowest, lowest], [max_d, min_d], "g--")
25 | plt.plot([middle, middle], [max_d, min_d], "r-")
26 | plt.plot([highest, highest], [max_d, min_d], "g--")
27 | plt.savefig("carpet_line1_2_blur580.png")
28 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/test_motors.py:
--------------------------------------------------------------------------------
1 | from Raspi_MotorHAT import Raspi_MotorHAT
2 |
3 | import time
4 | import atexit
5 |
6 | mh = Raspi_MotorHAT(addr=0x6f)
7 | lm = mh.getMotor(1)
8 | rm = mh.getMotor(2)
9 |
10 | def turn_off_motors():
11 | lm.run(Raspi_MotorHAT.RELEASE)
12 | rm.run(Raspi_MotorHAT.RELEASE)
13 |
14 | atexit.register(turn_off_motors)
15 |
16 | lm.setSpeed(255)
17 | rm.setSpeed(255)
18 |
19 | lm.run(Raspi_MotorHAT.FORWARD)
20 | rm.run(Raspi_MotorHAT.FORWARD)
21 | time.sleep(1)
22 |
23 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/test_rainbow.py:
--------------------------------------------------------------------------------
1 | from time import sleep
2 | from robot import Robot
3 | from led_rainbow import show_rainbow
4 |
5 | bot = Robot()
6 |
7 | while True:
8 | print("on")
9 | show_rainbow(bot.leds, range(bot.leds.count))
10 | bot.leds.show()
11 | sleep(0.5)
12 | print("off")
13 | bot.leds.clear()
14 | bot.leds.show()
15 | sleep(0.5)
16 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/virtual_robot.py:
--------------------------------------------------------------------------------
1 | import vpython as vp
2 | from robot_pose import robot_view
3 |
4 |
5 | def make_robot():
6 | chassis_width = 155 # left side to right
7 | chassis_thickness = 3 # plastic thickness
8 | chassis_length = 200 # front to back
9 | wheel_thickness = 26
10 | wheel_diameter = 70
11 | axle_x = 30 # wheel axle from
12 | axle_z = -20
13 | rear_castor_position = vp.vector(-80, -6, -30)
14 | rear_castor_radius = 14
15 | rear_caster_thickness = 12
16 |
17 | base = vp.box(length=chassis_length,
18 | height=chassis_thickness,
19 | width=chassis_width)
20 | # rotate to match body - so Z is height and Y is width
21 | base.rotate(angle=vp.radians(90),
22 | axis=vp.vector(1, 0, 0))
23 | wheel_dist = chassis_width/2
24 | wheel_l = vp.cylinder(radius=wheel_diameter/2,
25 | length=wheel_thickness,
26 | pos=vp.vector(axle_x, -wheel_dist, axle_z),
27 | axis=vp.vector(0, -1, 0))
28 | wheel_r = vp.cylinder(radius=wheel_diameter/2,
29 | length=wheel_thickness,
30 | pos=vp.vector(axle_x, wheel_dist, axle_z),
31 | axis=vp.vector(0, 1, 0))
32 | rear_castor = vp.cylinder(radius=rear_castor_radius,
33 | length=rear_caster_thickness,
34 | pos=rear_castor_position,
35 | axis=vp.vector(0, 1, 0))
36 | return vp.compound([base, wheel_l, wheel_r, rear_castor])
37 |
38 |
39 | if __name__ == "__main__":
40 | robot_view()
41 | x_arrow = vp.arrow(axis=vp.vector(200, 0, 0), color=vp.color.red)
42 | y_arrow = vp.arrow(axis=vp.vector(0, 200, 0), color=vp.color.green)
43 | z_arrow = vp.arrow(axis=vp.vector(0, 0, 200), color=vp.color.blue)
44 | robot=make_robot()
45 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/visual_filtered_accelerometer.py:
--------------------------------------------------------------------------------
1 | import vpython as vp
2 | import logging
3 | from robot_imu import RobotImu, ComplementaryFilter
4 | import virtual_robot
5 |
6 | logging.basicConfig(level=logging.INFO)
7 | imu = RobotImu()
8 | model = virtual_robot.make_robot()
9 | virtual_robot.robot_view()
10 |
11 | filter = ComplementaryFilter(0.95).filter
12 |
13 | pitch = 0
14 | roll = 0
15 |
16 | while True:
17 | vp.rate(100)
18 |
19 | new_pitch, new_roll = imu.read_accelerometer_pitch_and_roll()
20 | pitch = filter(pitch, new_pitch)
21 | roll = filter(roll, new_roll)
22 |
23 | print(f"Pitch: {pitch:.2f}, Roll: {roll:.2f}")
24 | # reset the model
25 | model.up = vp.vector(0, 1, 0)
26 | model.axis = vp.vector(1, 0, 0)
27 | # Reposition it
28 | model.rotate(angle=vp.radians(roll), axis=vp.vector(1, 0, 0))
29 | model.rotate(angle=vp.radians(pitch), axis=vp.vector(0, 1, 0))
30 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/visual_fusion.py:
--------------------------------------------------------------------------------
1 | import vpython as vp
2 | from robot_imu import RobotImu, ImuFusion
3 | from delta_timer import DeltaTimer
4 | import imu_settings
5 | import virtual_robot
6 |
7 | imu = RobotImu(gyro_offsets=imu_settings.gyro_offsets,
8 | mag_offsets=imu_settings.mag_offsets)
9 | fusion = ImuFusion(imu)
10 |
11 | model = virtual_robot.make_robot()
12 | virtual_robot.robot_view()
13 |
14 | timer = DeltaTimer()
15 |
16 | while True:
17 | vp.rate(100)
18 | dt, elapsed = timer.update()
19 | fusion.update(dt)
20 | # reset the model
21 | model.up = vp.vector(0, 1, 0)
22 | model.axis = vp.vector(1, 0, 0)
23 | # Reposition it
24 | model.rotate(angle=vp.radians(fusion.roll), axis=vp.vector(1, 0, 0))
25 | model.rotate(angle=vp.radians(fusion.pitch), axis=vp.vector(0, 1, 0))
26 | model.rotate(angle=vp.radians(fusion.yaw), axis=vp.vector(0, 0, 1))
27 |
--------------------------------------------------------------------------------
/chapter17/0_starting_point/visual_gyroscope.py:
--------------------------------------------------------------------------------
1 | import vpython as vp
2 | from robot_imu import RobotImu
3 | import time
4 | import imu_settings
5 | import virtual_robot
6 |
7 |
8 | imu = RobotImu(gyro_offsets=imu_settings.gyro_offsets)
9 |
10 | pitch = 0
11 | roll = 0
12 | yaw = 0
13 |
14 | model = virtual_robot.make_robot()
15 | virtual_robot.robot_view()
16 |
17 | latest = time.time()
18 |
19 | while True:
20 | vp.rate(1000)
21 | current = time.time()
22 | dt = latest - current
23 | latest = current
24 | gyro = imu.read_gyroscope()
25 | pitch += gyro.x * dt
26 | roll += gyro.y * dt
27 | yaw += gyro.z * dt
28 |
29 | # reset the model
30 | model.up = vp.vector(0, 1, 0)
31 | model.axis = vp.vector(1, 0, 0)
32 | # Reposition it
33 | model.rotate(angle=vp.radians(pitch), axis=vp.vector(1, 0, 0))
34 | model.rotate(angle=vp.radians(roll), axis=vp.vector(0, 1, 0))
35 | model.rotate(angle=vp.radians(yaw), axis=vp.vector(0, 0, 1))
36 |
--------------------------------------------------------------------------------
/chapter17/full_system/accelerometer_vector.py:
--------------------------------------------------------------------------------
1 | import vpython as vp
2 | import logging
3 | from robot_imu import RobotImu
4 | from robot_pose import robot_view
5 |
6 | logging.basicConfig(level=logging.INFO)
7 | imu = RobotImu()
8 | robot_view()
9 |
10 | accel_arrow = vp.arrow(axis=vp.vector(0, 1, 0))
11 | x_arrow = vp.arrow(axis=vp.vector(1, 0, 0), color=vp.color.red)
12 | y_arrow = vp.arrow(axis=vp.vector(0, 1, 0), color=vp.color.green)
13 | z_arrow = vp.arrow(axis=vp.vector(0, 0, 1), color=vp.color.blue)
14 |
15 | while True:
16 | vp.rate(100)
17 | accel = imu.read_accelerometer()
18 | print(f"Accelerometer: {accel}")
19 | accel_arrow.axis = accel.norm()
20 |
--------------------------------------------------------------------------------
/chapter17/full_system/avoid_with_rainbows.py:
--------------------------------------------------------------------------------
1 | from robot import Robot
2 | from time import sleep
3 | from led_rainbow import show_rainbow
4 |
5 |
6 | class ObstacleAvoidingBehavior:
7 | """Simple obstacle avoiding"""
8 | def __init__(self, the_robot):
9 | self.robot = the_robot
10 | self.speed = 60
11 | # Calculations for the LEDs
12 | self.led_half = int(self.robot.leds.count/2)
13 | self.sense_colour = 255, 0, 0
14 |
15 | def distance_to_led_bar(self, distance):
16 | # Invert so closer means more LED's.
17 | inverted = max(0, 1.0 - distance)
18 | led_bar = int(round(inverted * self.led_half)) + 1
19 | return led_bar
20 |
21 | def display_state(self, left_distance, right_distance):
22 | # Clear first
23 | self.robot.leds.clear()
24 | # Left side
25 | led_bar = self.distance_to_led_bar(left_distance)
26 | show_rainbow(self.robot.leds, range(led_bar))
27 | # Right side
28 | led_bar = self.distance_to_led_bar(right_distance)
29 | # Bit trickier - must go from below the leds count, to the leds count.
30 | start = (self.robot.leds.count - 1) - (led_bar)
31 | right_range = range(self.robot.leds.count - 1, start, -1)
32 | show_rainbow(self.robot.leds, right_range)
33 | # Now show this display
34 | self.robot.leds.show()
35 |
36 | def get_speeds(self, nearest_distance):
37 | if nearest_distance >= 1.0:
38 | nearest_speed = self.speed
39 | furthest_speed = self.speed
40 | delay = 100
41 | elif nearest_distance > 0.5:
42 | nearest_speed = self.speed
43 | furthest_speed = self.speed * 0.8
44 | delay = 100
45 | elif nearest_distance > 0.2:
46 | nearest_speed = self.speed
47 | furthest_speed = self.speed * 0.6
48 | delay = 100
49 | elif nearest_distance > 0.1:
50 | nearest_speed = -self.speed * 0.4
51 | furthest_speed = -self.speed
52 | delay = 100
53 | else: # collison
54 | nearest_speed = -self.speed
55 | furthest_speed = -self.speed
56 | delay = 250
57 | return nearest_speed, furthest_speed, delay
58 |
59 | def run(self):
60 | while True:
61 | # Get the sensor readings in meters
62 | left_distance = self.robot.left_distance_sensor.distance
63 | right_distance = self.robot.right_distance_sensor.distance
64 | # Display this
65 | self.display_state(left_distance, right_distance)
66 | # Get speeds for motors from distances
67 | nearest_speed, furthest_speed, delay = self.get_speeds(min(left_distance, right_distance))
68 | print(f"Distances: l {left_distance:.2f}, r {right_distance:.2f}. Speeds: n: {nearest_speed}, f: {furthest_speed}. Delay: {delay}")
69 | # and drive
70 | # Send this to the motors
71 | if left_distance < right_distance:
72 | self.robot.set_left(nearest_speed)
73 | self.robot.set_right(furthest_speed)
74 | else:
75 | self.robot.set_right(nearest_speed)
76 | self.robot.set_left(furthest_speed)
77 | # Wait our delay time
78 | sleep(delay * 0.001)
79 |
80 |
81 | bot = Robot()
82 | behavior = ObstacleAvoidingBehavior(bot)
83 | behavior.run()
84 |
--------------------------------------------------------------------------------
/chapter17/full_system/behavior_line.py:
--------------------------------------------------------------------------------
1 | import robot
2 | from Raspi_MotorHAT import Raspi_MotorHAT
3 | from time import sleep
4 |
5 | r = robot.Robot()
6 | r.set_left(100)
7 | r.set_right(70)
8 | sleep(1)
9 |
10 |
--------------------------------------------------------------------------------
/chapter17/full_system/behavior_path.py:
--------------------------------------------------------------------------------
1 | import robot
2 | from time import sleep
3 |
4 | def straight(bot, seconds):
5 | bot.set_left(100)
6 | bot.set_right(100)
7 | sleep(seconds)
8 |
9 | def turn_left(bot, seconds):
10 | bot.set_left(20)
11 | bot.set_right(80)
12 | sleep(seconds)
13 |
14 | def turn_right(bot, seconds):
15 | bot.set_left(80)
16 | bot.set_right(20)
17 | sleep(seconds)
18 |
19 | def spin_left(bot, seconds):
20 | bot.set_left(-100)
21 | bot.set_right(100)
22 | sleep(seconds)
23 |
24 | bot = robot.Robot()
25 | straight(bot, 1)
26 | turn_right(bot, 0.6)
27 | straight(bot, 0.6)
28 | turn_left(bot, 0.6)
29 | straight(bot, 0.6)
30 | turn_left(bot, 0.6)
31 | straight(bot, 0.3)
32 | spin_left(bot, 1)
33 |
34 |
--------------------------------------------------------------------------------
/chapter17/full_system/calibrate_gyro.py:
--------------------------------------------------------------------------------
1 | from robot_imu import RobotImu
2 | import time
3 | import vpython as vp
4 |
5 | imu = RobotImu()
6 |
7 | gyro_min = vp.vector(0, 0, 0)
8 | gyro_max = vp.vector(0, 0, 0)
9 |
10 | for n in range(500):
11 | gyro = imu.read_gyroscope()
12 | gyro_min.x = min(gyro_min.x, gyro.x)
13 | gyro_min.y = min(gyro_min.y, gyro.y)
14 | gyro_min.z = min(gyro_min.z, gyro.z)
15 |
16 | gyro_max.x = max(gyro_max.x, gyro.x)
17 | gyro_max.y = max(gyro_max.y, gyro.y)
18 | gyro_max.z = max(gyro_max.z, gyro.z)
19 |
20 | offset = (gyro_min + gyro_max) / 2
21 |
22 | time.sleep(.01)
23 |
24 | print(f"Zero offset: {offset}.")
25 |
--------------------------------------------------------------------------------
/chapter17/full_system/calibrate_magnetometer.py:
--------------------------------------------------------------------------------
1 | import vpython as vp
2 | from robot_imu import RobotImu
3 | from imu_settings import mag_offsets
4 |
5 | imu = RobotImu(mag_offsets=mag_offsets)
6 |
7 | mag_min = vp.vector(0, 0, 0)
8 | mag_max = vp.vector(0, 0, 0)
9 |
10 |
11 | scatter_xy = vp.gdots(color=vp.color.red)
12 | scatter_yz = vp.gdots(color=vp.color.green)
13 | scatter_zx = vp.gdots(color=vp.color.blue)
14 |
15 | while True:
16 | vp.rate(100)
17 | mag = imu.read_magnetometer()
18 |
19 | mag_min.x = min(mag_min.x, mag.x)
20 | mag_min.y = min(mag_min.y, mag.y)
21 | mag_min.z = min(mag_min.z, mag.z)
22 |
23 | mag_max.x = max(mag_max.x, mag.x)
24 | mag_max.y = max(mag_max.y, mag.y)
25 | mag_max.z = max(mag_max.z, mag.z)
26 | offset = (mag_max + mag_min) / 2
27 |
28 | print(f"Magnetometer: {mag}. Offsets: {offset}.")
29 | scatter_xy.plot(mag.x, mag.y)
30 | scatter_yz.plot(mag.y, mag.z)
31 | scatter_zx.plot(mag.z, mag.x)
32 |
--------------------------------------------------------------------------------
/chapter17/full_system/camera_stream.py:
--------------------------------------------------------------------------------
1 | from picamera.array import PiRGBArray
2 | from picamera import PiCamera
3 | import numpy as np
4 |
5 | import cv2
6 |
7 | size = (320, 240)
8 | encode_param = [int(cv2.IMWRITE_JPEG_QUALITY), 90]
9 |
10 |
11 | def setup_camera():
12 | camera = PiCamera()
13 | camera.resolution = size
14 | camera.rotation = 180
15 | return camera
16 |
17 |
18 | def start_stream(camera):
19 | image_storage = PiRGBArray(camera, size=size)
20 |
21 | cam_stream = camera.capture_continuous(image_storage, format="bgr", use_video_port=True)
22 | for raw_frame in cam_stream:
23 | yield raw_frame.array
24 | image_storage.truncate(0)
25 |
26 |
27 | def get_encoded_bytes_for_frame(frame):
28 | result, encoded_image = cv2.imencode('.jpg', frame, encode_param)
29 | return encoded_image.tostring()
30 |
--------------------------------------------------------------------------------
/chapter17/full_system/circle_pan_tilt_behavior.py:
--------------------------------------------------------------------------------
1 | from time import sleep
2 | import math
3 |
4 | from robot import Robot
5 | class CirclePanTiltBehavior:
6 | def __init__(self, the_robot):
7 | self.robot = the_robot
8 | self.current_time = 0
9 | self.frames_per_circle = 50
10 | self.radians_per_frame = (2 * math.pi) / self.frames_per_circle
11 | self.radius = 30
12 |
13 | def run(self):
14 | while True:
15 | frame_number = self.current_time % self.frames_per_circle
16 | frame_in_radians = frame_number * self.radians_per_frame
17 | self.robot.set_pan(self.radius * math.cos(frame_in_radians))
18 | self.robot.set_tilt(self.radius * math.sin(frame_in_radians))
19 | sleep(0.05)
20 | self.current_time += 1
21 |
22 | bot = Robot()
23 | behavior = CirclePanTiltBehavior(bot)
24 | behavior.run()
25 |
26 |
--------------------------------------------------------------------------------
/chapter17/full_system/control_image_behavior.py:
--------------------------------------------------------------------------------
1 | import time
2 |
3 | from image_app_core import start_server_process, get_control_instruction, put_output_image
4 | import camera_stream
5 |
6 | def controlled_image_server_behavior():
7 | camera = camera_stream.setup_camera()
8 | time.sleep(0.1)
9 |
10 | for frame in camera_stream.start_stream(camera):
11 | encoded_bytes = camera_stream.get_encoded_bytes_for_frame(frame)
12 | put_output_image(encoded_bytes)
13 |
14 | instruction = get_control_instruction()
15 | if instruction and instruction['command'] == "exit":
16 | print("Stopping")
17 | return
18 |
19 | process = start_server_process('control_image_behavior.html')
20 |
21 | try:
22 | controlled_image_server_behavior()
23 | finally:
24 | process.terminate()
25 |
--------------------------------------------------------------------------------
/chapter17/full_system/control_server.py:
--------------------------------------------------------------------------------
1 | from flask import Flask, render_template, jsonify
2 | from robot_modes import RobotModes
3 | from leds_led_shim import Leds
4 |
5 | # A Flask App contains all its routes.
6 | app = Flask(__name__)
7 | # Prepare our robot modes for use
8 | mode_manager = RobotModes()
9 | leds = Leds()
10 | leds.set_one(1, [0, 255, 0])
11 | leds.show()
12 |
13 |
14 | @app.after_request
15 | def add_header(response):
16 | response.headers['Cache-Control'] = "no-cache, no-store, must-revalidate"
17 | return response
18 |
19 |
20 | @app.route("/")
21 | def index():
22 | return render_template('menu.html', menu=mode_manager.menu_config)
23 |
24 |
25 | @app.route("/run/", methods=['POST'])
26 | def run(mode_name):
27 | global leds
28 | if leds:
29 | leds.clear()
30 | leds.show()
31 | leds = None
32 |
33 | # Use our robot app to run something with this mode_name
34 | mode_manager.run(mode_name)
35 | response = {'message': f'{mode_name} running'}
36 | if mode_manager.should_redirect(mode_name):
37 | response['redirect'] = True
38 | return jsonify(response)
39 |
40 |
41 | @app.route("/stop", methods=['POST'])
42 | def stop():
43 | # Tell our system to stop the mode it's in.
44 | mode_manager.stop()
45 | return jsonify({'message': "Stopped"})
46 |
47 |
48 | app.run(host="0.0.0.0")
49 |
--------------------------------------------------------------------------------
/chapter17/full_system/delta_timer.py:
--------------------------------------------------------------------------------
1 | import time
2 |
3 |
4 | class DeltaTimer:
5 | def __init__(self):
6 | self.last = self.start = time.time()
7 |
8 | def update(self):
9 | current = time.time()
10 | dt = current - self.last
11 | elapsed = current - self.start
12 | self.last = current
13 | return dt, elapsed
--------------------------------------------------------------------------------
/chapter17/full_system/drive_distance.py:
--------------------------------------------------------------------------------
1 | from robot import Robot, EncoderCounter
2 | from pid_controller import PIController
3 | import time
4 | import logging
5 | logger = logging.getLogger("drive_distance")
6 |
7 | def drive_distance(bot, distance, speed=80):
8 | # Use left as "primary" motor, the right is keeping up
9 | set_primary = bot.set_left
10 | primary_encoder = bot.left_encoder
11 | set_secondary = bot.set_right
12 | secondary_encoder = bot.right_encoder
13 | controller = PIController(proportional_constant=5, integral_constant=0.3)
14 |
15 | # start the motors, and start the loop
16 | set_primary(speed)
17 | set_secondary(speed)
18 | while primary_encoder.pulse_count < distance or secondary_encoder.pulse_count < distance:
19 | time.sleep(0.01)
20 | # How far off are we?
21 | error = primary_encoder.pulse_count - secondary_encoder.pulse_count
22 | adjustment = controller.get_value(error)
23 | # How fast should the motor move to get there?
24 | set_primary(int(speed - adjustment))
25 | set_secondary(int(speed + adjustment))
26 | # Some debug
27 | logger.debug(f"Encoders: primary: {primary_encoder.pulse_count}, secondary: {secondary_encoder.pulse_count},"
28 | f"e:{error} adjustment: {adjustment:.2f}")
29 | logger.info(f"Distances: primary: {primary_encoder.distance_in_mm()} mm, secondary: {secondary_encoder.distance_in_mm()} mm")
30 |
31 | logging.basicConfig(level=logging.DEBUG)
32 | bot = Robot()
33 | distance_to_drive = 1000 # in mm - this is a meter
34 | distance_in_ticks = EncoderCounter.mm_to_ticks(distance_to_drive)
35 | drive_distance(bot, distance_in_ticks)
36 |
--------------------------------------------------------------------------------
/chapter17/full_system/drive_north_behavior.py:
--------------------------------------------------------------------------------
1 | """This behavior will turn to seek north, and then drive that way"""
2 | from robot_imu import RobotImu, ImuFusion
3 | from delta_timer import DeltaTimer
4 | from pid_controller import PIController
5 | from robot import Robot
6 | import imu_settings
7 |
8 |
9 | imu = RobotImu(mag_offsets=imu_settings.mag_offsets,
10 | gyro_offsets=imu_settings.gyro_offsets)
11 | fusion = ImuFusion(imu)
12 | timer = DeltaTimer()
13 | pid = PIController(0.7, 0.01)
14 | robot = Robot()
15 | base_speed = 70
16 |
17 | # Lets head for this heading
18 | heading_set_point = 0
19 |
20 | while True:
21 | dt, elapsed = timer.update()
22 | fusion.update(dt)
23 | heading_error = fusion.yaw - heading_set_point
24 | steer_value = pid.get_value(heading_error, delta_time=dt)
25 | print(f"Error: {heading_error}, Value:{steer_value:2f}, t: {elapsed}")
26 | robot.set_left(base_speed + steer_value)
27 | robot.set_right(base_speed - steer_value)
28 |
--------------------------------------------------------------------------------
/chapter17/full_system/encoder_counter.py:
--------------------------------------------------------------------------------
1 | from gpiozero import DigitalInputDevice
2 | import math
3 | class EncoderCounter:
4 | ticks_to_mm_const = None # you must set this up before using distance methods
5 | def __init__(self, pin_number):
6 | self.pulse_count = 0
7 | self.direction = 1
8 | self.device = DigitalInputDevice(pin=pin_number)
9 | self.device.pin.when_changed = self.when_changed
10 |
11 | def when_changed(self, time_ticks, state):
12 | self.pulse_count += self.direction
13 |
14 | def set_direction(self, direction):
15 | """This should be -1 or 1. """
16 | assert abs(direction)==1, "Direction %s should be 1 or -1" % direction
17 | self.direction = direction
18 |
19 | def reset(self):
20 | self.pulse_count = 0
21 |
22 | def stop(self):
23 | self.device.close()
24 |
25 | def distance_in_mm(self):
26 | return int(self.pulse_count * EncoderCounter.ticks_to_mm_const)
27 |
28 | @staticmethod
29 | def mm_to_ticks(mm):
30 | return mm / EncoderCounter.ticks_to_mm_const
31 |
32 | @staticmethod
33 | def set_constants(wheel_diameter_mm, ticks_per_revolution):
34 | EncoderCounter.ticks_to_mm_const = (math.pi / ticks_per_revolution) * wheel_diameter_mm
35 |
--------------------------------------------------------------------------------
/chapter17/full_system/image_app_core.py:
--------------------------------------------------------------------------------
1 | import time
2 | from multiprocessing import Process, Queue
3 |
4 | from flask import Flask, render_template, Response, request
5 |
6 | app = Flask(__name__)
7 | control_queue = Queue()
8 | display_queue = Queue(maxsize=2)
9 | display_template = 'image_server.html'
10 |
11 |
12 | @app.after_request
13 | def add_header(response):
14 | response.headers['Cache-Control'] = "no-cache, no-store, must-revalidate"
15 | return response
16 |
17 |
18 | @app.route('/')
19 | def index():
20 | return render_template(display_template)
21 |
22 |
23 | def frame_generator():
24 | while True:
25 | time.sleep(0.05)
26 | encoded_bytes = display_queue.get()
27 | yield (b'--frame\r\n'
28 | b'Content-Type: image/jpeg\r\n\r\n' + encoded_bytes + b'\r\n')
29 |
30 |
31 | @app.route('/display')
32 | def display():
33 | return Response(frame_generator(),
34 | mimetype='multipart/x-mixed-replace; boundary=frame')
35 |
36 |
37 | @app.route('/control', methods=['POST'])
38 | def control():
39 | control_queue.put(request.form)
40 | return Response('queued')
41 |
42 |
43 | def start_server_process(template_name):
44 | """Start the process, call .terminate to close it"""
45 | global display_template
46 | display_template = template_name
47 | server = Process(target=app.run, kwargs={"host": "0.0.0.0", "port": 5001})
48 | server.start()
49 | return server
50 |
51 |
52 | def put_output_image(encoded_bytes):
53 | """Queue an output image"""
54 | if display_queue.empty():
55 | display_queue.put(encoded_bytes)
56 |
57 |
58 | def get_control_instruction():
59 | if control_queue.empty():
60 | return None
61 | else:
62 | return control_queue.get()
63 |
--------------------------------------------------------------------------------
/chapter17/full_system/image_server.py:
--------------------------------------------------------------------------------
1 | from flask import Flask, render_template, Response
2 | import camera_stream
3 | import time
4 |
5 | app = Flask(__name__)
6 |
7 | @app.route('/')
8 | def index():
9 | return render_template('image_server.html')
10 |
11 | def frame_generator():
12 | """This is our main video feed"""
13 | camera = camera_stream.setup_camera()
14 |
15 | # allow the camera to warm up
16 | time.sleep(0.1)
17 | for frame in camera_stream.start_stream(camera):
18 | encoded_bytes = camera_stream.get_encoded_bytes_for_frame(frame)
19 | yield (b'--frame\r\n'
20 | b'Content-Type: image/jpeg\r\n\r\n' + encoded_bytes + b'\r\n')
21 |
22 | @app.route('/display')
23 | def display():
24 | return Response(frame_generator(),
25 | mimetype='multipart/x-mixed-replace; boundary=frame')
26 |
27 | app.run(host="0.0.0.0", debug=True, port=5001)
28 |
--------------------------------------------------------------------------------
/chapter17/full_system/imu_settings.py:
--------------------------------------------------------------------------------
1 | from vpython import vector
2 | gyro_offsets = vector(-0.583969, 0.675573, -0.530534)
3 | mag_offsets = vector(7.725, 9.6, -25.275)
4 |
--------------------------------------------------------------------------------
/chapter17/full_system/led_rainbow.py:
--------------------------------------------------------------------------------
1 |
2 | import colorsys
3 |
4 |
5 | def show_rainbow(leds, led_range):
6 | led_range = list(led_range)
7 | hue_step = 1.0 / len(led_range)
8 | for index, led_address in enumerate(led_range):
9 | hue = hue_step * index
10 | rgb = colorsys.hsv_to_rgb(hue, 1.0, 0.6)
11 | rgb = [int(c*255) for c in rgb]
12 | leds.set_one(led_address, rgb)
13 |
--------------------------------------------------------------------------------
/chapter17/full_system/leds_led_shim.py:
--------------------------------------------------------------------------------
1 | import ledshim
2 |
3 |
4 | class Leds:
5 | @property
6 | def count(self):
7 | return ledshim.width
8 |
9 | def set_one(self, led_number, color):
10 | ledshim.set_pixel(led_number, *color)
11 |
12 | def set_range(self, led_range, color):
13 | for pixel in led_range:
14 | ledshim.set_pixel(pixel, *color)
15 |
16 | def set_all(self, color):
17 | ledshim.set_all(*color)
18 |
19 | def clear(self):
20 | ledshim.clear()
21 |
22 | def show(self):
23 | ledshim.show()
24 |
--------------------------------------------------------------------------------
/chapter17/full_system/leds_test.py:
--------------------------------------------------------------------------------
1 | from robot import Robot
2 | from time import sleep
3 |
4 | bot = Robot()
5 | red = (255, 0, 0)
6 | blue = (0, 0, 255)
7 |
8 | while True:
9 | print("red")
10 | bot.leds.set_all(red)
11 | bot.leds.show()
12 | sleep(0.5)
13 | print("blue")
14 | bot.leds.set_all(blue)
15 | bot.leds.show()
16 | sleep(0.5)
17 |
--------------------------------------------------------------------------------
/chapter17/full_system/magnetometer_vector.py:
--------------------------------------------------------------------------------
1 | import vpython as vp
2 | import logging
3 | from robot_imu import RobotImu
4 | from robot_pose import robot_view
5 |
6 | logging.basicConfig(level=logging.INFO)
7 | imu = RobotImu()
8 | robot_view()
9 |
10 | mag_arrow = vp.arrow(pos=vp.vector(0, 0, 0))
11 | x_arrow = vp.arrow(axis=vp.vector(1, 0, 0), color=vp.color.red)
12 | y_arrow = vp.arrow(axis=vp.vector(0, 1, 0), color=vp.color.green)
13 | z_arrow = vp.arrow(axis=vp.vector(0, 0, 1), color=vp.color.blue)
14 |
15 | while True:
16 | vp.rate(100)
17 |
18 | mag = imu.read_magnetometer()
19 | mag_arrow.axis = mag.norm()
20 | print(f"Magnetometer: {mag}")
21 |
22 |
--------------------------------------------------------------------------------
/chapter17/full_system/manual_drive.py:
--------------------------------------------------------------------------------
1 | import time
2 | from robot import Robot
3 | from image_app_core import start_server_process, get_control_instruction, put_output_image
4 | import camera_stream
5 |
6 |
7 | TIMEOUT_IN = 1
8 |
9 | class ManualDriveBehavior(object):
10 | def __init__(self, robot):
11 | self.robot = robot
12 | self.last_time = time.time()
13 |
14 | def process_control(self):
15 | instruction = get_control_instruction()
16 | while instruction:
17 | self.last_time = time.time()
18 | self.handle_instruction(instruction)
19 | instruction = get_control_instruction()
20 |
21 | def handle_instruction(self, instruction):
22 | command = instruction['command']
23 | if command == "set_left":
24 | self.robot.set_left(int(instruction['speed']))
25 | elif command == "set_right":
26 | self.robot.set_right(int(instruction['speed']))
27 | elif command == "exit":
28 | print("stopping")
29 | exit()
30 | else:
31 | raise ValueError(f"Unknown instruction: {instruction}")
32 |
33 | def make_display(self, frame):
34 | """Create display output, and put it on the queue"""
35 | encoded_bytes = camera_stream.get_encoded_bytes_for_frame(frame)
36 | put_output_image(encoded_bytes)
37 |
38 | def run(self):
39 | # Set pan and tilt to middle, then clear it.
40 | self.robot.set_pan(0)
41 | self.robot.set_tilt(0)
42 | # start camera
43 | camera = camera_stream.setup_camera()
44 | # warm up and servo move time
45 | time.sleep(0.1)
46 | # Servo's will be in place - stop them for now.
47 | self.robot.servos.stop_all()
48 | print("Setup Complete")
49 |
50 | # Main loop
51 | for frame in camera_stream.start_stream(camera):
52 | self.make_display(frame)
53 | self.process_control()
54 | # Auto stop
55 | if time.time() > self.last_time + TIMEOUT_IN:
56 | self.robot.stop_motors()
57 |
58 | print("Setting up")
59 | behavior = ManualDriveBehavior(Robot())
60 | process = start_server_process('manual_drive.html')
61 | try:
62 | behavior.run()
63 | except:
64 | process.terminate()
65 |
--------------------------------------------------------------------------------
/chapter17/full_system/menu_server.service:
--------------------------------------------------------------------------------
1 | [Unit]
2 | Description=Robot Menu Web Service
3 | After=network.target
4 |
5 | [Install]
6 | WantedBy=multi-user.target
7 |
8 | [Service]
9 | WorkingDirectory=/home/pi
10 | User=pi
11 | Environment=LD_PRELOAD=/usr/lib/arm-linux-gnueabihf/libatomic.so.1
12 | ExecStart=/usr/bin/env python3 control_server.py
13 |
--------------------------------------------------------------------------------
/chapter17/full_system/my-robot-skill/__init__.py:
--------------------------------------------------------------------------------
1 | from adapt.intent import IntentBuilder
2 | from mycroft import MycroftSkill, intent_handler
3 | from mycroft.util.log import LOG
4 |
5 | import requests
6 |
7 |
8 | class MyRobot(MycroftSkill):
9 | def __init__(self):
10 | super().__init__()
11 | self.base_url = self.settings.get("base_url")
12 |
13 | @intent_handler(IntentBuilder("")
14 | .require("Robot")
15 | .require("TestRainbow"))
16 | def handle_test_rainbow(self, message):
17 | self.handle_control('/run/test_rainbow', 'TestingRainbow')
18 |
19 | @intent_handler(IntentBuilder("")
20 | .require("Robot")
21 | .require("stop"))
22 | def handle_stop(self, message):
23 | self.handle_control('/stop', 'stopping')
24 |
25 | def handle_control(self, end_point, dialog_verb):
26 | try:
27 | requests.post(self.base_url + end_point)
28 | self.speak_dialog('Robot')
29 | self.speak_dialog(dialog_verb)
30 | except:
31 | self.speak_dialog("UnableToReach")
32 | LOG.exception("Unable to reach the robot")
33 |
34 |
35 | def create_skill():
36 | return MyRobot()
37 |
38 |
--------------------------------------------------------------------------------
/chapter17/full_system/my-robot-skill/dialog/en-us/Robot.dialog:
--------------------------------------------------------------------------------
1 | The Robot
2 | Robot
3 |
--------------------------------------------------------------------------------
/chapter17/full_system/my-robot-skill/dialog/en-us/TestingRainbow.dialog:
--------------------------------------------------------------------------------
1 | is testing rainbows.
2 | is deploying rainbows.
3 | is starting rainbows.
4 | is lighting up.
5 |
--------------------------------------------------------------------------------
/chapter17/full_system/my-robot-skill/dialog/en-us/UnableToReach.dialog:
--------------------------------------------------------------------------------
1 | Sorry I cannot reach the robot.
2 | The robot is unreachable.
3 | Have you turned the robot on?
4 | Is the control server running on the robot?
5 |
--------------------------------------------------------------------------------
/chapter17/full_system/my-robot-skill/dialog/en-us/stopping.dialog:
--------------------------------------------------------------------------------
1 | is stopping.
2 | will stop.
3 |
--------------------------------------------------------------------------------
/chapter17/full_system/my-robot-skill/requirements.txt:
--------------------------------------------------------------------------------
1 | requests
2 |
--------------------------------------------------------------------------------
/chapter17/full_system/my-robot-skill/settingsmeta.json:
--------------------------------------------------------------------------------
1 | {
2 | "base_url": "http://myrobot.local:5000",
3 | "skillMetadata": {
4 | "sections": [
5 | {
6 | "name": "Robot",
7 | "fields": [
8 | {
9 | "name": "base_url",
10 | "type": "text",
11 | "label": "Base URL for the robot control server",
12 | "value": "https://myrobots.local:5000"
13 | }
14 | ]
15 | }
16 | ]
17 | }
18 | }
19 |
--------------------------------------------------------------------------------
/chapter17/full_system/my-robot-skill/vocab/en-us/TestRainbow.voc:
--------------------------------------------------------------------------------
1 | test rainbow
2 | test the leds
3 | deploy rainbows
4 | turn on the lights
5 |
--------------------------------------------------------------------------------
/chapter17/full_system/my-robot-skill/vocab/en-us/robot.voc:
--------------------------------------------------------------------------------
1 | robot
2 | my robot
3 | ask robot to
4 | tell the robot to
5 |
--------------------------------------------------------------------------------
/chapter17/full_system/my-robot-skill/vocab/en-us/stop.voc:
--------------------------------------------------------------------------------
1 | stop
2 | cease
3 | turn off
4 | stand down
--------------------------------------------------------------------------------
/chapter17/full_system/pid_controller.py:
--------------------------------------------------------------------------------
1 | import logging
2 | logger = logging.getLogger("pid_controller")
3 |
4 | class PIController:
5 | def __init__(self, proportional_constant=0, integral_constant=0, windup_limit=None):
6 | self.proportional_constant = proportional_constant
7 | self.integral_constant = integral_constant
8 | self.windup_limit = windup_limit
9 | # Running sums
10 | self.integral_sum = 0
11 |
12 | def handle_proportional(self, error):
13 | return self.proportional_constant * error
14 |
15 | def handle_integral(self, error, delta_time):
16 | if self.windup_limit is None or \
17 | (abs(self.integral_sum) < self.windup_limit) or \
18 | ((error > 0) != (self.integral_sum > 0)):
19 | self.integral_sum += error * delta_time
20 | return self.integral_constant * self.integral_sum
21 |
22 | def get_value(self, error, delta_time=1):
23 | p = self.handle_proportional(error)
24 | i = self.handle_integral(error, delta_time)
25 | logger.debug(f"P: {p}, I: {i:.2f}")
26 | return p + i
27 |
28 | def reset(self):
29 | self.integral_sum = 0
30 |
--------------------------------------------------------------------------------
/chapter17/full_system/plot_acc_gyro_fusion.py:
--------------------------------------------------------------------------------
1 | import vpython as vp
2 | from robot_imu import RobotImu, ImuFusion
3 | from delta_timer import DeltaTimer
4 | import imu_settings
5 |
6 | imu = RobotImu(gyro_offsets=imu_settings.gyro_offsets)
7 |
8 | fusion = ImuFusion(imu)
9 |
10 | vp.graph(xmin=0, xmax=60, scroll=True)
11 | graph_pitch = vp.gcurve(color=vp.color.red)
12 | graph_roll = vp.gcurve(color=vp.color.green)
13 |
14 |
15 | timer = DeltaTimer()
16 | while True:
17 | vp.rate(100)
18 | dt, elapsed = timer.update()
19 | fusion.update(dt)
20 | graph_pitch.plot(elapsed, fusion.pitch)
21 | graph_roll.plot(elapsed, fusion.roll)
22 |
--------------------------------------------------------------------------------
/chapter17/full_system/plot_accel_debug_pitch_and_roll.py:
--------------------------------------------------------------------------------
1 | import vpython as vp
2 | import logging
3 | import time
4 | from robot_imu import RobotImu
5 |
6 | logging.basicConfig(level=logging.INFO)
7 | imu = RobotImu()
8 |
9 |
10 | pr = vp.graph(xmin=0, xmax=60, scroll=True)
11 | graph_pitch = vp.gcurve(color=vp.color.red, graph=pr)
12 | graph_roll = vp.gcurve(color=vp.color.green, graph=pr)
13 |
14 | xyz = vp.graph(xmin=0, xmax=60, scroll=True)
15 | graph_x = vp.gcurve(color=vp.color.orange, graph=xyz)
16 | graph_y = vp.gcurve(color=vp.color.cyan, graph=xyz)
17 | graph_z = vp.gcurve(color=vp.color.purple, graph=xyz)
18 |
19 | start = time.time()
20 | while True:
21 | vp.rate(100)
22 | elapsed = time.time() - start
23 | pitch, roll = imu.read_accelerometer_pitch_and_roll()
24 | raw_accel = imu.read_accelerometer()
25 | graph_pitch.plot(elapsed, pitch)
26 | graph_roll.plot(elapsed, roll)
27 | print(f"Pitch: {pitch:.2f}, Roll: {roll:.2f}")
28 | graph_x.plot(elapsed, raw_accel.x)
29 | graph_y.plot(elapsed, raw_accel.y)
30 | graph_z.plot(elapsed, raw_accel.z)
31 |
--------------------------------------------------------------------------------
/chapter17/full_system/plot_accel_pitch_and_roll.py:
--------------------------------------------------------------------------------
1 | import vpython as vp
2 | import time
3 | from robot_imu import RobotImu
4 |
5 |
6 | imu = RobotImu()
7 |
8 | vp.graph(xmin=0, xmax=60, scroll=True)
9 | graph_pitch = vp.gcurve(color=vp.color.red)
10 | graph_roll = vp.gcurve(color=vp.color.green)
11 |
12 | start = time.time()
13 | while True:
14 | vp.rate(100)
15 | elapsed = time.time() - start
16 | pitch, roll = imu.read_accelerometer_pitch_and_roll()
17 | graph_pitch.plot(elapsed, pitch)
18 | graph_roll.plot(elapsed, roll)
19 |
--------------------------------------------------------------------------------
/chapter17/full_system/plot_calibrated_gyroscope.py:
--------------------------------------------------------------------------------
1 | import vpython as vp
2 | import time
3 | from robot_imu import RobotImu
4 | import imu_settings
5 |
6 | imu = RobotImu(gyro_offsets=imu_settings.gyro_offsets)
7 |
8 | vp.graph(xmin=0, xmax=60, ymax=360, ymin=-360, scroll=True)
9 | graph_x = vp.gcurve(color=vp.color.red)
10 | graph_y = vp.gcurve(color=vp.color.green)
11 | graph_z = vp.gcurve(color=vp.color.blue)
12 |
13 | start = time.time()
14 | while True:
15 | vp.rate(100)
16 | elapsed = time.time() - start
17 | gyro = imu.read_gyroscope()
18 | print(f"Gyro x: {gyro.x:.2f}, y: {gyro.y:.2f}, z: {gyro.z:.2f}")
19 | graph_x.plot(elapsed, gyro.x)
20 | graph_y.plot(elapsed, gyro.y)
21 | graph_z.plot(elapsed, gyro.z)
22 |
--------------------------------------------------------------------------------
/chapter17/full_system/plot_gyroscope.py:
--------------------------------------------------------------------------------
1 | import vpython as vp
2 | import time
3 | from robot_imu import RobotImu
4 |
5 | imu = RobotImu()
6 |
7 | vp.graph(xmin=0, xmax=60, scroll=True)
8 | graph_x = vp.gcurve(color=vp.color.red)
9 | graph_y = vp.gcurve(color=vp.color.green)
10 | graph_z = vp.gcurve(color=vp.color.blue)
11 |
12 | start = time.time()
13 | while True:
14 | vp.rate(100)
15 | elapsed = time.time() - start
16 | gyro = imu.read_gyroscope()
17 | print(f"Gyro x: {gyro.x:.2f}, y: {gyro.y:.2f}, z: {gyro.z:.2f}")
18 | graph_x.plot(elapsed, gyro.x)
19 | graph_y.plot(elapsed, gyro.y)
20 | graph_z.plot(elapsed, gyro.z)
21 |
--------------------------------------------------------------------------------
/chapter17/full_system/plot_imu_fusion.py:
--------------------------------------------------------------------------------
1 | import vpython as vp
2 | from robot_imu import RobotImu, ImuFusion
3 | from delta_timer import DeltaTimer
4 | import imu_settings
5 |
6 | imu = RobotImu(gyro_offsets=imu_settings.gyro_offsets,
7 | mag_offsets=imu_settings.mag_offsets)
8 | fusion = ImuFusion(imu)
9 |
10 | vp.graph(xmin=0, xmax=60, scroll=True)
11 | graph_pitch = vp.gcurve(color=vp.color.red)
12 | graph_roll = vp.gcurve(color=vp.color.green)
13 | graph_yaw = vp.gcurve(color=vp.color.blue)
14 |
15 | timer = DeltaTimer()
16 | while True:
17 | vp.rate(100)
18 | dt, elapsed = timer.update()
19 | fusion.update(dt)
20 | graph_pitch.plot(elapsed, fusion.pitch)
21 | graph_roll.plot(elapsed, fusion.roll)
22 | graph_yaw.plot(elapsed, fusion.yaw)
23 |
--------------------------------------------------------------------------------
/chapter17/full_system/plot_integrated_gyro.py:
--------------------------------------------------------------------------------
1 | import vpython as vp
2 | import logging
3 | from robot_imu import RobotImu
4 | from delta_timer import DeltaTimer
5 | import imu_settings
6 |
7 |
8 | logging.basicConfig(level=logging.INFO)
9 | imu = RobotImu(gyro_offsets=imu_settings.gyro_offsets)
10 |
11 | vp.graph(xmin=0, xmax=60, scroll=True)
12 | graph_x = vp.gcurve(color=vp.color.red)
13 | graph_y = vp.gcurve(color=vp.color.green)
14 | graph_z = vp.gcurve(color=vp.color.blue)
15 |
16 | timer = DeltaTimer()
17 | pitch = 0
18 | roll = 0
19 | yaw = 0
20 |
21 | while True:
22 | vp.rate(100)
23 | dt, elapsed = timer.update()
24 | gyro = imu.read_gyroscope()
25 | roll += gyro.x * dt
26 | pitch += gyro.y * dt
27 | yaw += gyro.z * dt
28 | print(f"Elapsed: {elapsed:.2f}, Pitch: {pitch:.2f}, Roll: {roll:.2f}, Yaw: {yaw:.2f}")
29 | graph_x.plot(elapsed, pitch)
30 | graph_y.plot(elapsed, roll)
31 | graph_z.plot(elapsed, yaw)
32 |
--------------------------------------------------------------------------------
/chapter17/full_system/plot_mag_heading.py:
--------------------------------------------------------------------------------
1 | import vpython as vp
2 | from robot_imu import RobotImu
3 | from delta_timer import DeltaTimer
4 | import imu_settings
5 |
6 | imu = RobotImu(mag_offsets=imu_settings.mag_offsets)
7 |
8 | vp.graph(xmin=0, xmax=60, scroll=True)
9 | graph_yaw = vp.gcurve(color=vp.color.blue)
10 | timer = DeltaTimer()
11 |
12 | while True:
13 | vp.rate(100)
14 | dt, elapsed = timer.update()
15 | mag = imu.read_magnetometer()
16 | yaw = -vp.degrees(vp.atan2(mag.y, mag.x))
17 | graph_yaw.plot(elapsed, yaw)
18 |
--------------------------------------------------------------------------------
/chapter17/full_system/plot_temperature.py:
--------------------------------------------------------------------------------
1 | """When running,
2 | use VPYTHON_PORT=9020 VPYTHON_NOBROWSER=true"""
3 | import vpython as vp
4 | import time
5 | import logging
6 | from robot_imu import RobotImu
7 |
8 |
9 | logging.basicConfig(level=logging.INFO)
10 | imu = RobotImu()
11 | vp.graph(xmin=0, xmax=60, scroll=True)
12 | temp_graph = vp.gcurve()
13 | start = time.time()
14 | while True:
15 | vp.rate(100)
16 | temperature = imu.read_temperature()
17 | logging.info("Temperature {}".format(temperature))
18 | elapsed = time.time() - start
19 | temp_graph.plot(elapsed, temperature)
20 |
21 |
--------------------------------------------------------------------------------
/chapter17/full_system/robot.py:
--------------------------------------------------------------------------------
1 | from Raspi_MotorHAT import Raspi_MotorHAT
2 | from gpiozero import DistanceSensor
3 |
4 | import atexit
5 | import leds_led_shim
6 | from servos import Servos
7 | from encoder_counter import EncoderCounter
8 |
9 |
10 | class Robot:
11 | wheel_diameter_mm = 70.0
12 | ticks_per_revolution = 40.0
13 | wheel_distance_mm = 132.0
14 | def __init__(self, motorhat_addr=0x6f):
15 | # Setup the motorhat with the passed in address
16 | self._mh = Raspi_MotorHAT(addr=motorhat_addr)
17 |
18 | self.left_motor = self._mh.getMotor(1)
19 | self.right_motor = self._mh.getMotor(2)
20 |
21 | # Setup the Leds
22 | self.leds = leds_led_shim.Leds()
23 | # Set up servo motors for pan and tilt.
24 | self.servos = Servos(addr=motorhat_addr, deflect_90_in_ms=1)
25 |
26 | # Setup The Distance Sensors
27 | self.left_distance_sensor = DistanceSensor(echo=17, trigger=27, queue_len=2)
28 | self.right_distance_sensor = DistanceSensor(echo=5, trigger=6, queue_len=2)
29 |
30 | # Setup the Encoders
31 | EncoderCounter.set_constants(self.wheel_diameter_mm, self.ticks_per_revolution)
32 | self.left_encoder = EncoderCounter(4)
33 | self.right_encoder = EncoderCounter(26)
34 |
35 | # ensure the motors get stopped when the code exits
36 | atexit.register(self.stop_all)
37 |
38 | def convert_speed(self, speed):
39 | # Choose the running mode
40 | mode = Raspi_MotorHAT.RELEASE
41 | if speed > 0:
42 | mode = Raspi_MotorHAT.FORWARD
43 | elif speed < 0:
44 | mode = Raspi_MotorHAT.BACKWARD
45 |
46 | # Scale the speed
47 | output_speed = (abs(speed) * 255) // 100
48 | return mode, int(output_speed)
49 |
50 | def set_left(self, speed):
51 | mode, output_speed = self.convert_speed(speed)
52 | self.left_motor.setSpeed(output_speed)
53 | self.left_motor.run(mode)
54 |
55 | def set_right(self, speed):
56 | mode, output_speed = self.convert_speed(speed)
57 | self.right_motor.setSpeed(output_speed)
58 | self.right_motor.run(mode)
59 |
60 | def stop_motors(self):
61 | self.left_motor.run(Raspi_MotorHAT.RELEASE)
62 | self.right_motor.run(Raspi_MotorHAT.RELEASE)
63 |
64 | def stop_all(self):
65 | self.stop_motors()
66 |
67 | # Clear the display
68 | self.leds.clear()
69 | self.leds.show()
70 |
71 | # Clear any sensor handlers
72 | self.left_encoder.stop()
73 | self.right_encoder.stop()
74 |
75 | # Reset the servos
76 | self.servos.stop_all()
77 |
78 | def set_pan(self, angle):
79 | self.servos.set_servo_angle(1, angle)
80 |
81 | def set_tilt(self, angle):
82 | self.servos.set_servo_angle(0, angle)
83 |
84 |
--------------------------------------------------------------------------------
/chapter17/full_system/robot_modes.py:
--------------------------------------------------------------------------------
1 | import subprocess
2 |
3 | class RobotModes(object):
4 | """Our robot behaviors and tests as running modes"""
5 |
6 | # Mode config goes from a "mode_name" to a script to run. Configured for look up.
7 | mode_config = {
8 | "avoid_behavior": {"script": "avoid_behavior.py"},
9 | "circle_head": {"script": "circle_pan_tilt_behavior.py"},
10 | "test_rainbow": {"script": "test_rainbow.py"},
11 | "test_leds": {"script": "leds_test.py"},
12 | "line_following": {"script": "line_follow_behavior.py", "server": True},
13 | "color_track": {"script": "color_track_behavior.py", "server": True},
14 | "face_track": {"script": "face_track_behavior.py", "server": True},
15 | "manual_drive": {"script": "manual_drive.py", "server": True},
16 | "behavior_line": {"script": "straight_line_drive.py"},
17 | "drive_north": {"script": "drive_north.py"}
18 | }
19 |
20 | menu_config = [
21 | {"mode_name": "avoid_behavior", "text": "Avoid Behavior"},
22 | {"mode_name": "circle_head", "text": "Circle Head"},
23 | {"mode_name": "test_rainbow", "text": "LED Rainbow"},
24 | {"mode_name": "test_leds", "text": "Test LEDs"},
25 | {"mode_name": "line_following", "text": "Line Following"},
26 | {"mode_name": "color_track", "text": "Color Tracking"},
27 | {"mode_name": "face_track", "text": "Face Tracking"},
28 | {"mode_name": "manual_drive", "text": "Drive Manually"},
29 | {"mode_name": "behavior_line", "text": "Drive In A Line"},
30 | {"mode_name": "drive_north", "text": "Drive North"}
31 | ]
32 |
33 | def __init__(self):
34 | self.current_process = None
35 |
36 | def is_running(self):
37 | """Check if there is a process running. Returncode is only set when a process finishes"""
38 | return self.current_process and self.current_process.returncode is None
39 |
40 | def run(self, mode_name):
41 | """Run the mode as a subprocess, but not if we still have one running"""
42 | while self.is_running():
43 | self.stop()
44 |
45 | script = self.mode_config[mode_name]['script']
46 | self.current_process = subprocess.Popen(["python3", script])
47 |
48 | def stop(self):
49 | """Stop a process"""
50 | if self.is_running():
51 | # Sending the signal sigint is (on Linux) similar to pressing ctrl-c.
52 | # That causes the behavior to clean up and exit.
53 | self.current_process.send_signal(subprocess.signal.SIGINT)
54 | self.current_process = None
55 |
56 | def should_redirect(self, mode_name):
57 | return self.mode_config[mode_name].get('server') is True and self.is_running()
58 |
--------------------------------------------------------------------------------
/chapter17/full_system/robot_pose.py:
--------------------------------------------------------------------------------
1 | import vpython as vp
2 |
3 |
4 | def robot_view():
5 | vp.scene.forward = vp.vector(-3, -1, -1)
6 | vp.scene.up = vp.vector(0, 0, 1)
7 |
--------------------------------------------------------------------------------
/chapter17/full_system/servo_type_position.py:
--------------------------------------------------------------------------------
1 | from Raspi_MotorHAT.Raspi_PWM_Servo_Driver import PWM
2 | import atexit
3 |
4 | pwm = PWM(0x6f)
5 | # This sets the timebase for it all
6 | pwm_frequency = 100
7 | pwm.setPWMFreq(pwm_frequency)
8 | # Mid-point of the servo pulse length in milliseconds.
9 | servo_mid_point_ms = 1.5
10 | # What a deflection of 90 degrees is in pulse length in milliseconds
11 | deflect_90_in_ms = 0.5
12 | # Frequency is 1 divided by period, but working ms, we can use 1000
13 | period_in_ms = 1000 / pwm_frequency
14 | # The chip has 4096 steps in each period.
15 | pulse_steps = 4096
16 | # Steps for every millisecond.
17 | steps_per_ms = pulse_steps / period_in_ms
18 | # Steps for a degree.
19 | steps_per_degree = (deflect_90_in_ms * steps_per_ms) / 90
20 | # Mid-point of the servo in steps
21 | servo_mid_point_steps = servo_mid_point_ms * steps_per_ms
22 |
23 | def convert_degrees_to_steps(position):
24 | return int(servo_mid_point_steps + (position * steps_per_degree))
25 |
26 | atexit.register(pwm.setPWM, 0, 0, 4096)
27 |
28 | while True:
29 | position = int(input("Type your position in degrees (90 to -90, 0 is middle): "))
30 | end_step = convert_degrees_to_steps(position)
31 | pwm.setPWM(0, 0, end_step)
32 |
--------------------------------------------------------------------------------
/chapter17/full_system/servos.py:
--------------------------------------------------------------------------------
1 | from Raspi_MotorHAT.Raspi_PWM_Servo_Driver import PWM
2 |
3 | class Servos:
4 | def __init__(self, addr=0x6f, deflect_90_in_ms = 0.5):
5 | """addr: The i2c address of the PWM chip.
6 | deflect_90_in_ms: set this to calibrate the servo motors.
7 | it is what a deflection of 90 degrees is
8 | in terms of a pulse length in milliseconds."""
9 | self._pwm = PWM(addr)
10 | # This sets the timebase for it all
11 | pwm_frequency = 100
12 | self._pwm.setPWMFreq(pwm_frequency)
13 | # Mid-point of the servo pulse length in milliseconds.
14 | servo_mid_point_ms = 1.5
15 | # Frequency is 1/period, but working ms, we can use 1000
16 | period_in_ms = 1000 / pwm_frequency
17 | # The chip has 4096 steps in each period.
18 | pulse_steps = 4096
19 | # Steps for every millisecond.
20 | steps_per_ms = pulse_steps / period_in_ms
21 | # Steps for a degree
22 | self.steps_per_degree = (deflect_90_in_ms * steps_per_ms) / 90
23 | # Mid-point of the servo in steps
24 | self.servo_mid_point_steps = servo_mid_point_ms * steps_per_ms
25 |
26 | # Map for channels
27 | self.channels = [0, 1, 14, 15]
28 |
29 | def stop_all(self):
30 | # 0 in start is nothing, 4096 sets the OFF bit.
31 | self._pwm.setPWM(self.channels[0], 0, 4096)
32 | self._pwm.setPWM(self.channels[1], 0, 4096)
33 | self._pwm.setPWM(self.channels[2], 0, 4096)
34 | self._pwm.setPWM(self.channels[3], 0, 4096)
35 |
36 | def _convert_degrees_to_steps(self, position):
37 | return int(self.servo_mid_point_steps + (position * self.steps_per_degree))
38 |
39 | def set_servo_angle(self, channel, angle):
40 | """position: The position in degrees from the center. -90 to 90"""
41 | # Validate
42 | if angle > 90 or angle < -90:
43 | raise ValueError("Angle outside of range")
44 | # Then set the position
45 | off_step = self._convert_degrees_to_steps(angle)
46 | self._pwm.setPWM(self.channels[channel], 0, off_step)
47 |
48 |
--------------------------------------------------------------------------------
/chapter17/full_system/simple_avoid_behavior.py:
--------------------------------------------------------------------------------
1 | from robot import Robot
2 | from time import sleep
3 |
4 | class ObstacleAvoidingBehavior:
5 | """Simple obstacle avoiding"""
6 | def __init__(self, the_robot):
7 | self.robot = the_robot
8 | self.speed = 60
9 |
10 | def get_motor_speed(self, distance):
11 | """This method chooses a speed for a motor based on the distance from a sensor"""
12 | if distance < 0.2:
13 | return -self.speed
14 | else:
15 | return self.speed
16 |
17 | def run(self):
18 | while True:
19 | # Get the sensor readings in meters
20 | left_distance = self.robot.left_distance_sensor.distance
21 | right_distance = self.robot.right_distance_sensor.distance
22 | print("Left: {l:.2f}, Right: {r:.2f}".format(l=left_distance, r=right_distance))
23 | # and drive
24 | left_speed = self.get_motor_speed(left_distance)
25 | self.robot.set_right(left_speed)
26 | right_speed = self.get_motor_speed(right_distance)
27 | self.robot.set_left(right_speed)
28 | # Wait a little
29 | sleep(0.05)
30 |
31 | bot = Robot()
32 | behavior = ObstacleAvoidingBehavior(bot)
33 | behavior.run()
34 |
--------------------------------------------------------------------------------
/chapter17/full_system/sonar_scan.py:
--------------------------------------------------------------------------------
1 | import time
2 | import math
3 |
4 | import matplotlib.pyplot as plt
5 |
6 | from robot import Robot
7 |
8 | start_scan =0
9 | lower_bound = -90
10 | upper_bound = 90
11 | scan_step = 5
12 |
13 | the_robot = Robot()
14 | the_robot.set_tilt(0)
15 |
16 | scan_data = {}
17 | # Make the sensor scan
18 | for facing in range(lower_bound, upper_bound, scan_step):
19 | the_robot.set_pan(-facing)
20 | time.sleep(0.1)
21 | scan_data[facing] = the_robot.left_distance_sensor.distance * 100
22 | # make plot
23 | axis = [math.radians(facing) for facing in scan_data.keys()]
24 | print(axis)
25 | print(scan_data.values())
26 | plt.polar(axis, list(scan_data.values()), 'g-')
27 | # dump to png
28 | plt.savefig("scan.png")
29 |
--------------------------------------------------------------------------------
/chapter17/full_system/static/display.css:
--------------------------------------------------------------------------------
1 | .slider_track {
2 | width: 10vw;
3 | height: 90vh;
4 | border: 1px solid blue;
5 | background-color: lightblue;
6 | }
7 | .slider_tick {
8 | fill: mistyrose;
9 | }
10 | #left_slider {
11 | position: absolute;
12 | left: 5vw;
13 | top: 5vh;
14 | }
15 | #right_slider {
16 | position: absolute;
17 | right: 5vw;
18 | top: 5vh;
19 | }
20 | .button {
21 | display: block;
22 | height: 10vh;
23 | text-align: center;
24 | font-size: 2em;
25 | line-height: 10vh;
26 | text-decoration: none;
27 | background-color: blue;
28 | color: white;
29 | }
30 |
31 | #exitbutton {
32 | width: 40vh;
33 | margin-top: 5vh;
34 | margin-left: auto;
35 | margin-right: auto;
36 | }
37 |
38 | #video {
39 | text-align: center;
40 | }
41 |
42 | #video img {
43 | margin-top: 20vh;
44 | width: 80vmin;
45 | height: auto;
46 | }
47 |
48 | .menu {
49 | width: 100%;
50 | margin-top: 0;
51 | margin-bottom: 0;
52 | padding: 0;
53 | }
54 |
55 | .menu li {
56 | list-style-type: none;
57 | list-style-position: initial;
58 | }
59 |
60 | .menu .button {
61 | margin-left: auto;
62 | margin-right: auto;
63 | width: 60vw;
64 | border: 1px solid lightblue;
65 | }
66 |
--------------------------------------------------------------------------------
/chapter17/full_system/static/touch-slider.js:
--------------------------------------------------------------------------------
1 | function makeSlider(id, when_changed) {
2 | // internal data
3 | let touched = false;
4 | let changed = false;
5 | let position = 0;
6 | const slider = $('#' + id);
7 | // internal functions
8 | const set_position = function(new_position) {
9 | position = Math.round(new_position);
10 | slider.find('.slider_tick')[0].setAttribute('cy', position);
11 | changed = true;
12 | };
13 | slider.on('touchmove', event => {
14 | let touch = event.targetTouches[0];
15 | // Get the touch relative to the top of the slider
16 | let from_top = touch.pageY - slider.offset().top;
17 | // Convert to twice a percentage of the track. (0 is the middle)
18 | let relative_touch = (from_top / slider.height()) * 200;
19 | set_position(relative_touch - 100);
20 | touched = true;
21 | event.preventDefault();
22 | });
23 | slider.on('touchend', () => touched = false);
24 |
25 | const update = function() {
26 | if(!touched && Math.abs(position) > 0) {
27 | // drift back to the middle - add 0.5 to the movement, in the right direction to round up to at least 1%
28 | let error = 0 - position;
29 | let change = (0.3 * error) + (Math.sign(error) * 0.5);
30 | set_position(position + change);
31 | // console.log(id + ": " + position);
32 | }
33 | };
34 | setInterval(update, 50);
35 |
36 | const update_if_changed = function() {
37 | if(changed) {
38 | changed = false;
39 | // Call the callback - invert the track so 'up' is positive
40 | when_changed(-position);
41 | }
42 | };
43 | setInterval(update_if_changed, 200);
44 | }
45 |
--------------------------------------------------------------------------------
/chapter17/full_system/straight_line_drive.py:
--------------------------------------------------------------------------------
1 | from robot import Robot
2 | from pid_controller import PIController
3 | import time
4 | import logging
5 |
6 | logger = logging.getLogger("straight_line")
7 | logging.basicConfig(level=logging.INFO)
8 | logging.getLogger("pid_controller").setLevel(logging.DEBUG)
9 |
10 | bot = Robot()
11 | stop_at_time = time.time() + 15
12 |
13 | speed = 80
14 | bot.set_left(speed)
15 | bot.set_right(speed)
16 |
17 | pid = PIController(proportional_constant=4, integral_constant=0.3)
18 |
19 | while time.time() < stop_at_time:
20 | time.sleep(0.01)
21 | # Calculate the error
22 | left = bot.left_encoder.pulse_count
23 | right = bot.right_encoder.pulse_count
24 | error = left - right
25 |
26 | # Get the speed
27 | adjustment = pid.get_value(error)
28 | right_speed = int(speed + adjustment)
29 | left_speed = int(speed - adjustment)
30 |
31 | logger.debug(f"error: {error} adjustment: {adjustment:.2f}")
32 | logger.info(f"left: {left} right: {right}, left_speed: {left_speed} right_speed: {right_speed}")
33 | bot.set_left(left_speed)
34 | bot.set_right(right_speed)
35 |
--------------------------------------------------------------------------------
/chapter17/full_system/templates/color_track_behavior.html:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 | Robot Image Server
5 |
6 |
7 | Robot Image Server
8 |
9 | Start
10 | Stop
11 | Exit
12 |
13 |
14 |
--------------------------------------------------------------------------------
/chapter17/full_system/templates/control_image_behavior.html:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 | Robot Image Server
5 |
6 |
7 | Robot Image Server
8 |
9 | Exit
10 |
11 |
12 |
--------------------------------------------------------------------------------
/chapter17/full_system/templates/image_server.html:
--------------------------------------------------------------------------------
1 |
2 |
3 | Robot Image Server
4 |
5 |
6 | Robot Image Server
7 |
8 |
9 |
10 |
--------------------------------------------------------------------------------
/chapter17/full_system/templates/manual_drive.html:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
5 |
6 |
7 |
8 | Manually Drive The Robot
9 |
10 |
11 |
14 | Exit
15 |
16 |
19 |
33 |
34 |
35 |
--------------------------------------------------------------------------------
/chapter17/full_system/templates/menu.html:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 | My Robot Menu
5 |
6 |
7 |
8 |
9 | My Robot Menu
10 |
11 |
21 |
22 |
32 |
33 |
--------------------------------------------------------------------------------
/chapter17/full_system/test_distance_sensors.py:
--------------------------------------------------------------------------------
1 | import time
2 | from gpiozero import DistanceSensor
3 |
4 | print("Prepare GPIO Pins")
5 | sensor_l = DistanceSensor(echo=17, trigger=27, queue_len=2)
6 | sensor_r = DistanceSensor(echo=5, trigger=6, queue_len=2)
7 |
8 | while True:
9 | print("Left: {l:.2f}, Right: {r:.2f}".format(
10 | l=sensor_l.distance * 100,
11 | r=sensor_r.distance * 100))
12 | time.sleep(0.1)
13 |
--------------------------------------------------------------------------------
/chapter17/full_system/test_distance_travelled.py:
--------------------------------------------------------------------------------
1 | from robot import Robot
2 | import time
3 | import math
4 | import logging
5 | logger = logging.getLogger("test_distance_travelled")
6 |
7 | wheel_diameter_mm = 70.0
8 | ticks_per_revolution = 40.0
9 | ticks_to_mm_const = (math.pi / ticks_per_revolution) * wheel_diameter_mm
10 |
11 | def ticks_to_mm(ticks):
12 | return int(ticks_to_mm_const * ticks)
13 |
14 | bot = Robot()
15 | stop_at_time = time.time() + 1
16 |
17 | logging.basicConfig(level=logging.INFO)
18 | bot.set_left(90)
19 | bot.set_right(90)
20 |
21 | while time.time() < stop_at_time:
22 | logger.info("Left: {} Right: {}".format(
23 | ticks_to_mm(bot.left_encoder.pulse_count),
24 | ticks_to_mm(bot.right_encoder.pulse_count)))
25 | time.sleep(0.05)
26 |
27 |
--------------------------------------------------------------------------------
/chapter17/full_system/test_encoders.py:
--------------------------------------------------------------------------------
1 | from robot import Robot
2 | import time
3 | import logging
4 |
5 | from gpiozero import DigitalInputDevice
6 |
7 | logger = logging.getLogger("test_encoders")
8 |
9 | class EncoderCounter(object):
10 | def __init__(self, pin_number):
11 | self.pulse_count = 0
12 | self.device = DigitalInputDevice(pin=pin_number)
13 | self.device.pin.when_changed = self.when_changed
14 |
15 | def when_changed(self, time_ticks, state):
16 | self.pulse_count += 1
17 |
18 | bot = Robot()
19 | left_encoder = EncoderCounter(4)
20 | right_encoder = EncoderCounter(26)
21 |
22 | stop_at_time = time.time() + 1
23 |
24 | logging.basicConfig(level=logging.INFO)
25 | bot.set_left(90)
26 | bot.set_right(90)
27 | while time.time() < stop_at_time:
28 | logger.info(f"Left: {left_encoder.pulse_count} Right: {right_encoder.pulse_count}")
29 | time.sleep(0.05)
30 |
--------------------------------------------------------------------------------
/chapter17/full_system/test_line_find.py:
--------------------------------------------------------------------------------
1 | import cv2
2 | import numpy as np
3 | from matplotlib import pyplot as plt
4 |
5 | image = cv2.imread("carpet_line1 2.jpg")
6 | assert image is not None, "Unable to read file"
7 |
8 | resized = cv2.resize(image, (320, 240))
9 |
10 | gray = cv2.cvtColor(resized, cv2.COLOR_BGR2GRAY)
11 | blur = cv2.blur(gray, (5, 80))
12 | row = blur[180].astype(np.int32)
13 | diff = np.diff(row)
14 |
15 | max_d = np.amax(diff, 0)
16 | min_d = np.amin(diff, 0)
17 |
18 | highest = np.where(diff == max_d)[0][0]
19 | lowest = np.where(diff == min_d)[0][0]
20 | middle = (highest + lowest) // 2
21 |
22 | x = np.arange(len(diff))
23 | plt.plot(x, diff)
24 | plt.plot([lowest, lowest], [max_d, min_d], "g--")
25 | plt.plot([middle, middle], [max_d, min_d], "r-")
26 | plt.plot([highest, highest], [max_d, min_d], "g--")
27 | plt.savefig("carpet_line1_2_blur580.png")
28 |
--------------------------------------------------------------------------------
/chapter17/full_system/test_motors.py:
--------------------------------------------------------------------------------
1 | from Raspi_MotorHAT import Raspi_MotorHAT
2 |
3 | import time
4 | import atexit
5 |
6 | mh = Raspi_MotorHAT(addr=0x6f)
7 | lm = mh.getMotor(1)
8 | rm = mh.getMotor(2)
9 |
10 | def turn_off_motors():
11 | lm.run(Raspi_MotorHAT.RELEASE)
12 | rm.run(Raspi_MotorHAT.RELEASE)
13 |
14 | atexit.register(turn_off_motors)
15 |
16 | lm.setSpeed(255)
17 | rm.setSpeed(255)
18 |
19 | lm.run(Raspi_MotorHAT.FORWARD)
20 | rm.run(Raspi_MotorHAT.FORWARD)
21 | time.sleep(1)
22 |
23 |
--------------------------------------------------------------------------------
/chapter17/full_system/test_rainbow.py:
--------------------------------------------------------------------------------
1 | from time import sleep
2 | from robot import Robot
3 | from led_rainbow import show_rainbow
4 |
5 | bot = Robot()
6 |
7 | while True:
8 | print("on")
9 | show_rainbow(bot.leds, range(bot.leds.count))
10 | bot.leds.show()
11 | sleep(0.5)
12 | print("off")
13 | bot.leds.clear()
14 | bot.leds.show()
15 | sleep(0.5)
16 |
--------------------------------------------------------------------------------
/chapter17/full_system/virtual_robot.py:
--------------------------------------------------------------------------------
1 | import vpython as vp
2 | from robot_pose import robot_view
3 |
4 |
5 | def make_robot():
6 | chassis_width = 155 # left side to right
7 | chassis_thickness = 3 # plastic thickness
8 | chassis_length = 200 # front to back
9 | wheel_thickness = 26
10 | wheel_diameter = 70
11 | axle_x = 30 # wheel axle from
12 | axle_z = -20
13 | rear_castor_position = vp.vector(-80, -6, -30)
14 | rear_castor_radius = 14
15 | rear_caster_thickness = 12
16 |
17 | base = vp.box(length=chassis_length,
18 | height=chassis_thickness,
19 | width=chassis_width)
20 | # rotate to match body - so Z is height and Y is width
21 | base.rotate(angle=vp.radians(90),
22 | axis=vp.vector(1, 0, 0))
23 | wheel_dist = chassis_width/2
24 | wheel_l = vp.cylinder(radius=wheel_diameter/2,
25 | length=wheel_thickness,
26 | pos=vp.vector(axle_x, -wheel_dist, axle_z),
27 | axis=vp.vector(0, -1, 0))
28 | wheel_r = vp.cylinder(radius=wheel_diameter/2,
29 | length=wheel_thickness,
30 | pos=vp.vector(axle_x, wheel_dist, axle_z),
31 | axis=vp.vector(0, 1, 0))
32 | rear_castor = vp.cylinder(radius=rear_castor_radius,
33 | length=rear_caster_thickness,
34 | pos=rear_castor_position,
35 | axis=vp.vector(0, 1, 0))
36 | return vp.compound([base, wheel_l, wheel_r, rear_castor])
37 |
38 |
39 | if __name__ == "__main__":
40 | robot_view()
41 | x_arrow = vp.arrow(axis=vp.vector(200, 0, 0), color=vp.color.red)
42 | y_arrow = vp.arrow(axis=vp.vector(0, 200, 0), color=vp.color.green)
43 | z_arrow = vp.arrow(axis=vp.vector(0, 0, 200), color=vp.color.blue)
44 | robot=make_robot()
45 |
--------------------------------------------------------------------------------
/chapter17/full_system/visual_filtered_accelerometer.py:
--------------------------------------------------------------------------------
1 | import vpython as vp
2 | import logging
3 | from robot_imu import RobotImu, ComplementaryFilter
4 | import virtual_robot
5 |
6 | logging.basicConfig(level=logging.INFO)
7 | imu = RobotImu()
8 | model = virtual_robot.make_robot()
9 | virtual_robot.robot_view()
10 |
11 | filter = ComplementaryFilter(0.95).filter
12 |
13 | pitch = 0
14 | roll = 0
15 |
16 | while True:
17 | vp.rate(100)
18 |
19 | new_pitch, new_roll = imu.read_accelerometer_pitch_and_roll()
20 | pitch = filter(pitch, new_pitch)
21 | roll = filter(roll, new_roll)
22 |
23 | print(f"Pitch: {pitch:.2f}, Roll: {roll:.2f}")
24 | # reset the model
25 | model.up = vp.vector(0, 1, 0)
26 | model.axis = vp.vector(1, 0, 0)
27 | # Reposition it
28 | model.rotate(angle=vp.radians(roll), axis=vp.vector(1, 0, 0))
29 | model.rotate(angle=vp.radians(pitch), axis=vp.vector(0, 1, 0))
30 |
--------------------------------------------------------------------------------
/chapter17/full_system/visual_fusion.py:
--------------------------------------------------------------------------------
1 | import vpython as vp
2 | from robot_imu import RobotImu, ImuFusion
3 | from delta_timer import DeltaTimer
4 | import imu_settings
5 | import virtual_robot
6 |
7 | imu = RobotImu(gyro_offsets=imu_settings.gyro_offsets,
8 | mag_offsets=imu_settings.mag_offsets)
9 | fusion = ImuFusion(imu)
10 |
11 | model = virtual_robot.make_robot()
12 | virtual_robot.robot_view()
13 |
14 | timer = DeltaTimer()
15 |
16 | while True:
17 | vp.rate(100)
18 | dt, elapsed = timer.update()
19 | fusion.update(dt)
20 | # reset the model
21 | model.up = vp.vector(0, 1, 0)
22 | model.axis = vp.vector(1, 0, 0)
23 | # Reposition it
24 | model.rotate(angle=vp.radians(fusion.roll), axis=vp.vector(1, 0, 0))
25 | model.rotate(angle=vp.radians(fusion.pitch), axis=vp.vector(0, 1, 0))
26 | model.rotate(angle=vp.radians(fusion.yaw), axis=vp.vector(0, 0, 1))
27 |
--------------------------------------------------------------------------------
/chapter17/full_system/visual_gyroscope.py:
--------------------------------------------------------------------------------
1 | import vpython as vp
2 | from robot_imu import RobotImu
3 | import time
4 | import imu_settings
5 | import virtual_robot
6 |
7 |
8 | imu = RobotImu(gyro_offsets=imu_settings.gyro_offsets)
9 |
10 | pitch = 0
11 | roll = 0
12 | yaw = 0
13 |
14 | model = virtual_robot.make_robot()
15 | virtual_robot.robot_view()
16 |
17 | latest = time.time()
18 |
19 | while True:
20 | vp.rate(1000)
21 | current = time.time()
22 | dt = latest - current
23 | latest = current
24 | gyro = imu.read_gyroscope()
25 | pitch += gyro.x * dt
26 | roll += gyro.y * dt
27 | yaw += gyro.z * dt
28 |
29 | # reset the model
30 | model.up = vp.vector(0, 1, 0)
31 | model.axis = vp.vector(1, 0, 0)
32 | # Reposition it
33 | model.rotate(angle=vp.radians(pitch), axis=vp.vector(1, 0, 0))
34 | model.rotate(angle=vp.radians(roll), axis=vp.vector(0, 1, 0))
35 | model.rotate(angle=vp.radians(yaw), axis=vp.vector(0, 0, 1))
36 |
--------------------------------------------------------------------------------
/chapter4/readme.md:
--------------------------------------------------------------------------------
1 | Creating A Headless Pi
2 | ======================
3 |
4 | This chapter is about making a Raspberry Pi headless, and reaching it from the network.
5 |
6 | Using WiFi and SSH
7 | ==================
8 |
9 | This example wpa_supplicant should be copied onto the BOOT partition of a Raspberry Pi SD card, as wpa_supplicant.conf.
10 | The SSID and PSK will need to be substituted for your own details.
11 |
12 | Also an "ssh" file should be present on the SD Card BOOT partition to enable this too.
13 |
14 | These will make your Raspberry Pi contactable on your WiFi network.
15 |
16 | Contacting The Pi
17 | =================
18 |
19 | When it is turned on, the Raspberry Pi will respond on a network to the name "raspberrypi.local" until you change it to a more unique hostname.
20 |
21 | You will need to ensure your computer supports MDNS. Most current Linux and Apple Mac desktops already have this support.
22 |
23 | ## Windows
24 |
25 | You will need to install the Apple Bonjour softwarem from https://support.apple.com/downloads/bonjour-for-windows, unless you already have iTunes or Skype installed.
--------------------------------------------------------------------------------
/chapter4/wpa_supplicant_example.conf:
--------------------------------------------------------------------------------
1 | country=
2 | update_config=1
3 | ctrl_interface=/var/run/wpa_supplicant
4 |
5 | network={
6 | ssid=""
7 | psk=""
8 | }
--------------------------------------------------------------------------------
/chapter5/hello.py:
--------------------------------------------------------------------------------
1 | import socket
2 | print('%s is alive!' % socket.gethostname())
3 |
--------------------------------------------------------------------------------
/chapter5/readme.md:
--------------------------------------------------------------------------------
1 | # Using sftp to Upload Files From Your PC
2 |
3 | This hello.py is just a sample file, to upload to the Pi using SFTP and then run using python.
4 |
5 | Use Filezilla, or a preferred SFTP client to link to the Pi and send this file to it to get comfortable with SFTP file transfers.
--------------------------------------------------------------------------------
/chapter7/behavior_line.py:
--------------------------------------------------------------------------------
1 | import robot
2 | from Raspi_MotorHAT import Raspi_MotorHAT
3 | from time import sleep
4 |
5 | r = robot.Robot()
6 | r.set_left(100)
7 | r.set_right(70)
8 | sleep(1)
9 |
10 |
--------------------------------------------------------------------------------
/chapter7/behavior_path.py:
--------------------------------------------------------------------------------
1 | import robot
2 | from time import sleep
3 |
4 | def straight(bot, seconds):
5 | bot.set_left(100)
6 | bot.set_right(100)
7 | sleep(seconds)
8 |
9 | def turn_left(bot, seconds):
10 | bot.set_left(20)
11 | bot.set_right(80)
12 | sleep(seconds)
13 |
14 | def turn_right(bot, seconds):
15 | bot.set_left(80)
16 | bot.set_right(20)
17 | sleep(seconds)
18 |
19 | def spin_left(bot, seconds):
20 | bot.set_left(-100)
21 | bot.set_right(100)
22 | sleep(seconds)
23 |
24 | bot = robot.Robot()
25 | straight(bot, 1)
26 | turn_right(bot, 0.6)
27 | straight(bot, 0.6)
28 | turn_left(bot, 0.6)
29 | straight(bot, 0.6)
30 | turn_left(bot, 0.6)
31 | straight(bot, 0.3)
32 | spin_left(bot, 1)
33 |
34 |
--------------------------------------------------------------------------------
/chapter7/robot.py:
--------------------------------------------------------------------------------
1 | from Raspi_MotorHAT import Raspi_MotorHAT
2 |
3 | import atexit
4 |
5 |
6 | class Robot:
7 | def __init__(self, motorhat_addr=0x6f):
8 | # Setup the motorhat with the passed in address
9 | self._mh = Raspi_MotorHAT(addr=motorhat_addr)
10 |
11 | # get local variable for each motor
12 | self.left_motor = self._mh.getMotor(1)
13 | self.right_motor = self._mh.getMotor(2)
14 |
15 | # ensure the motors get stopped when the code exits
16 | atexit.register(self.stop_motors)
17 |
18 | def convert_speed(self, speed):
19 | # Choose the running mode
20 | mode = Raspi_MotorHAT.RELEASE
21 | if speed > 0:
22 | mode = Raspi_MotorHAT.FORWARD
23 | elif speed < 0:
24 | mode = Raspi_MotorHAT.BACKWARD
25 |
26 | # Scale the speed
27 | output_speed = (abs(speed) * 255) // 100
28 | return mode, int(output_speed)
29 |
30 | def set_left(self, speed):
31 | mode, output_speed = self.convert_speed(speed)
32 | self.left_motor.setSpeed(output_speed)
33 | self.left_motor.run(mode)
34 |
35 | def set_right(self, speed):
36 | mode, output_speed = self.convert_speed(speed)
37 | self.right_motor.setSpeed(output_speed)
38 | self.right_motor.run(mode)
39 |
40 | def stop_motors(self):
41 | self.left_motor.run(Raspi_MotorHAT.RELEASE)
42 | self.right_motor.run(Raspi_MotorHAT.RELEASE)
43 |
44 |
--------------------------------------------------------------------------------
/chapter7/test_motors.py:
--------------------------------------------------------------------------------
1 | from Raspi_MotorHAT import Raspi_MotorHAT
2 |
3 | import time
4 | import atexit
5 |
6 | mh = Raspi_MotorHAT(addr=0x6f)
7 | lm = mh.getMotor(1)
8 | rm = mh.getMotor(2)
9 |
10 | def turn_off_motors():
11 | lm.run(Raspi_MotorHAT.RELEASE)
12 | rm.run(Raspi_MotorHAT.RELEASE)
13 |
14 | atexit.register(turn_off_motors)
15 |
16 | lm.setSpeed(255)
17 | rm.setSpeed(255)
18 |
19 | lm.run(Raspi_MotorHAT.FORWARD)
20 | rm.run(Raspi_MotorHAT.FORWARD)
21 | time.sleep(1)
22 |
23 |
--------------------------------------------------------------------------------
/chapter8/avoid_behavior.py:
--------------------------------------------------------------------------------
1 | from robot import Robot
2 | from time import sleep
3 |
4 | class ObstacleAvoidingBehavior:
5 | """Simple obstacle avoiding"""
6 | def __init__(self, the_robot):
7 | self.robot = the_robot
8 | self.speed = 60
9 |
10 | def get_speeds(self, nearest_distance):
11 | if nearest_distance >= 1.0:
12 | nearest_speed = self.speed
13 | furthest_speed = self.speed
14 | delay = 100
15 | elif nearest_distance > 0.5:
16 | nearest_speed = self.speed
17 | furthest_speed = self.speed * 0.8
18 | delay = 100
19 | elif nearest_distance > 0.2:
20 | nearest_speed = self.speed
21 | furthest_speed = self.speed * 0.6
22 | delay = 100
23 | elif nearest_distance > 0.1:
24 | nearest_speed = -self.speed * 0.4
25 | furthest_speed = -self.speed
26 | delay = 100
27 | else: # collison
28 | nearest_speed = -self.speed
29 | furthest_speed = -self.speed
30 | delay = 250
31 | return nearest_speed, furthest_speed, delay
32 |
33 | def run(self):
34 | while True:
35 | # Get the sensor readings in meters
36 | left_distance = self.robot.left_distance_sensor.distance
37 | right_distance = self.robot.right_distance_sensor.distance
38 | # Get speeds for motors from distances
39 | nearest_speed, furthest_speed, delay = self.get_speeds(min(left_distance, right_distance))
40 | print(f"Distances: l {left_distance:.2f}, r {right_distance:.2f}. Speeds: n: {nearest_speed}, f: {furthest_speed}. Delay: {delay}")
41 | # and drive
42 | # Send this to the motors
43 | if left_distance < right_distance:
44 | self.robot.set_left(nearest_speed)
45 | self.robot.set_right(furthest_speed)
46 | else:
47 | self.robot.set_right(nearest_speed)
48 | self.robot.set_left(furthest_speed)
49 | # Wait our delay time
50 | sleep(delay * 0.001)
51 |
52 |
53 | bot = Robot()
54 | behavior = ObstacleAvoidingBehavior(bot)
55 | behavior.run()
56 |
57 |
--------------------------------------------------------------------------------
/chapter8/robot.py:
--------------------------------------------------------------------------------
1 | from Raspi_MotorHAT import Raspi_MotorHAT
2 | from gpiozero import DistanceSensor
3 |
4 | import atexit
5 |
6 |
7 | class Robot:
8 | def __init__(self, motorhat_addr=0x6f):
9 | # Setup the motorhat with the passed in address
10 | self._mh = Raspi_MotorHAT(addr=motorhat_addr)
11 |
12 | self.left_motor = self._mh.getMotor(1)
13 | self.right_motor = self._mh.getMotor(2)
14 |
15 | # Setup The Distance Sensors
16 | self.left_distance_sensor = DistanceSensor(echo=17, trigger=27, queue_len=2)
17 | self.right_distance_sensor = DistanceSensor(echo=5, trigger=6, queue_len=2)
18 |
19 | # ensure the motors get stopped when the code exits
20 | atexit.register(self.stop_all)
21 |
22 | def convert_speed(self, speed):
23 | # Choose the running mode
24 | mode = Raspi_MotorHAT.RELEASE
25 | if speed > 0:
26 | mode = Raspi_MotorHAT.FORWARD
27 | elif speed < 0:
28 | mode = Raspi_MotorHAT.BACKWARD
29 |
30 | # Scale the speed
31 | output_speed = (abs(speed) * 255) // 100
32 | return mode, int(output_speed)
33 |
34 | def set_left(self, speed):
35 | mode, output_speed = self.convert_speed(speed)
36 | self.left_motor.setSpeed(output_speed)
37 | self.left_motor.run(mode)
38 |
39 | def set_right(self, speed):
40 | mode, output_speed = self.convert_speed(speed)
41 | self.right_motor.setSpeed(output_speed)
42 | self.right_motor.run(mode)
43 |
44 | def stop_motors(self):
45 | self.left_motor.run(Raspi_MotorHAT.RELEASE)
46 | self.right_motor.run(Raspi_MotorHAT.RELEASE)
47 |
48 | def stop_all(self):
49 | self.stop_motors()
50 |
--------------------------------------------------------------------------------
/chapter8/simple_avoid_behavior.py:
--------------------------------------------------------------------------------
1 | from robot import Robot
2 | from time import sleep
3 |
4 | class ObstacleAvoidingBehavior:
5 | """Simple obstacle avoiding"""
6 | def __init__(self, the_robot):
7 | self.robot = the_robot
8 | self.speed = 60
9 |
10 | def get_motor_speed(self, distance):
11 | """This method chooses a speed for a motor based on the distance from a sensor"""
12 | if distance < 0.2:
13 | return -self.speed
14 | else:
15 | return self.speed
16 |
17 | def run(self):
18 | while True:
19 | # Get the sensor readings in meters
20 | left_distance = self.robot.left_distance_sensor.distance
21 | right_distance = self.robot.right_distance_sensor.distance
22 | print("Left: {l:.2f}, Right: {r:.2f}".format(l=left_distance, r=right_distance))
23 | # and drive
24 | left_speed = self.get_motor_speed(left_distance)
25 | self.robot.set_right(left_speed)
26 | right_speed = self.get_motor_speed(right_distance)
27 | self.robot.set_left(right_speed)
28 | # Wait a little
29 | sleep(0.05)
30 |
31 | bot = Robot()
32 | behavior = ObstacleAvoidingBehavior(bot)
33 | behavior.run()
34 |
--------------------------------------------------------------------------------
/chapter8/test_distance_sensors.py:
--------------------------------------------------------------------------------
1 | import time
2 | from gpiozero import DistanceSensor
3 |
4 | print("Prepare GPIO Pins")
5 | sensor_l = DistanceSensor(echo=17, trigger=27, queue_len=2)
6 | sensor_r = DistanceSensor(echo=5, trigger=6, queue_len=2)
7 |
8 | while True:
9 | print("Left: {l:.2f}, Right: {r:.2f}".format(
10 | l=sensor_l.distance * 100,
11 | r=sensor_r.distance * 100))
12 | time.sleep(0.1)
13 |
--------------------------------------------------------------------------------
/chapter9/avoid_behavior.py:
--------------------------------------------------------------------------------
1 | from robot import Robot
2 | from time import sleep
3 |
4 | class ObstacleAvoidingBehavior:
5 | """Simple obstacle avoiding"""
6 | def __init__(self, the_robot):
7 | self.robot = the_robot
8 | self.speed = 60
9 | # Calculations for the LEDs
10 | self.led_half = int(self.robot.leds.count/2)
11 | print(self.led_half)
12 | self.sense_colour = 255, 0, 0
13 |
14 | def distance_to_led_bar(self, distance):
15 | # Invert so closer means more LED's.
16 | inverted = max(0, 1.0 - distance)
17 | led_bar = int(round(inverted * self.led_half)) + 1
18 | return led_bar
19 |
20 | def display_state(self, left_distance, right_distance):
21 | # Clear first
22 | self.robot.leds.clear()
23 | # Left side
24 | led_bar = self.distance_to_led_bar(left_distance)
25 | # print(led_bar)
26 | self.robot.leds.set_range(range(led_bar), self.sense_colour)
27 | # Right side
28 | led_bar = self.distance_to_led_bar(right_distance)
29 | # Bit trickier - must go from below the leds count, to the leds count.
30 | start = (self.robot.leds.count - 1) - led_bar
31 | self.robot.leds.set_range(range(start, self.robot.leds.count - 1), self.sense_colour)
32 | # Now show this display
33 | self.robot.leds.show()
34 |
35 | def get_speeds(self, nearest_distance):
36 | if nearest_distance >= 1.0:
37 | nearest_speed = self.speed
38 | furthest_speed = self.speed
39 | delay = 100
40 | elif nearest_distance > 0.5:
41 | nearest_speed = self.speed
42 | furthest_speed = self.speed * 0.8
43 | delay = 100
44 | elif nearest_distance > 0.2:
45 | nearest_speed = self.speed
46 | furthest_speed = self.speed * 0.6
47 | delay = 100
48 | elif nearest_distance > 0.1:
49 | nearest_speed = -self.speed * 0.4
50 | furthest_speed = -self.speed
51 | delay = 100
52 | else: # collison
53 | nearest_speed = -self.speed
54 | furthest_speed = -self.speed
55 | delay = 250
56 | return nearest_speed, furthest_speed, delay
57 |
58 | def run(self):
59 | while True:
60 | # Get the sensor readings in meters
61 | left_distance = self.robot.left_distance_sensor.distance
62 | right_distance = self.robot.right_distance_sensor.distance
63 | # Display this
64 | self.display_state(left_distance, right_distance)
65 | # Get speeds for motors from distances
66 | nearest_speed, furthest_speed, delay = self.get_speeds(min(left_distance, right_distance))
67 | print(f"Distances: l {left_distance:.2f}, r {right_distance:.2f}. Speeds: n: {nearest_speed}, f: {furthest_speed}. Delay: {delay}")
68 | # and drive
69 | # Send this to the motors
70 | if left_distance < right_distance:
71 | self.robot.set_left(nearest_speed)
72 | self.robot.set_right(furthest_speed)
73 | else:
74 | self.robot.set_right(nearest_speed)
75 | self.robot.set_left(furthest_speed)
76 | # Wait our delay time
77 | sleep(delay * 0.001)
78 |
79 |
80 | bot = Robot()
81 | behavior = ObstacleAvoidingBehavior(bot)
82 | behavior.run()
83 |
84 |
--------------------------------------------------------------------------------
/chapter9/avoid_with_rainbows.py:
--------------------------------------------------------------------------------
1 | from robot import Robot
2 | from time import sleep
3 | from led_rainbow import show_rainbow
4 |
5 |
6 | class ObstacleAvoidingBehavior:
7 | """Simple obstacle avoiding"""
8 | def __init__(self, the_robot):
9 | self.robot = the_robot
10 | self.speed = 60
11 | # Calculations for the LEDs
12 | self.led_half = int(self.robot.leds.count/2)
13 | self.sense_colour = 255, 0, 0
14 |
15 | def distance_to_led_bar(self, distance):
16 | # Invert so closer means more LED's.
17 | inverted = max(0, 1.0 - distance)
18 | led_bar = int(round(inverted * self.led_half)) + 1
19 | return led_bar
20 |
21 | def display_state(self, left_distance, right_distance):
22 | # Clear first
23 | self.robot.leds.clear()
24 | # Left side
25 | led_bar = self.distance_to_led_bar(left_distance)
26 | show_rainbow(self.robot.leds, range(led_bar))
27 | # Right side
28 | led_bar = self.distance_to_led_bar(right_distance)
29 | # Bit trickier - must go from below the leds count, to the leds count.
30 | start = (self.robot.leds.count - 1) - (led_bar)
31 | right_range = range(self.robot.leds.count - 1, start, -1)
32 | show_rainbow(self.robot.leds, right_range)
33 | # Now show this display
34 | self.robot.leds.show()
35 |
36 | def get_speeds(self, nearest_distance):
37 | if nearest_distance >= 1.0:
38 | nearest_speed = self.speed
39 | furthest_speed = self.speed
40 | delay = 100
41 | elif nearest_distance > 0.5:
42 | nearest_speed = self.speed
43 | furthest_speed = self.speed * 0.8
44 | delay = 100
45 | elif nearest_distance > 0.2:
46 | nearest_speed = self.speed
47 | furthest_speed = self.speed * 0.6
48 | delay = 100
49 | elif nearest_distance > 0.1:
50 | nearest_speed = -self.speed * 0.4
51 | furthest_speed = -self.speed
52 | delay = 100
53 | else: # collison
54 | nearest_speed = -self.speed
55 | furthest_speed = -self.speed
56 | delay = 250
57 | return nearest_speed, furthest_speed, delay
58 |
59 | def run(self):
60 | while True:
61 | # Get the sensor readings in meters
62 | left_distance = self.robot.left_distance_sensor.distance
63 | right_distance = self.robot.right_distance_sensor.distance
64 | # Display this
65 | self.display_state(left_distance, right_distance)
66 | # Get speeds for motors from distances
67 | nearest_speed, furthest_speed, delay = self.get_speeds(min(left_distance, right_distance))
68 | print(f"Distances: l {left_distance:.2f}, r {right_distance:.2f}. Speeds: n: {nearest_speed}, f: {furthest_speed}. Delay: {delay}")
69 | # and drive
70 | # Send this to the motors
71 | if left_distance < right_distance:
72 | self.robot.set_left(nearest_speed)
73 | self.robot.set_right(furthest_speed)
74 | else:
75 | self.robot.set_right(nearest_speed)
76 | self.robot.set_left(furthest_speed)
77 | # Wait our delay time
78 | sleep(delay * 0.001)
79 |
80 |
81 | bot = Robot()
82 | behavior = ObstacleAvoidingBehavior(bot)
83 | behavior.run()
84 |
--------------------------------------------------------------------------------
/chapter9/diagrams/generate_colour_wheel.py:
--------------------------------------------------------------------------------
1 | from matplotlib import pyplot as plt, colors, cm
2 | import numpy as np
3 |
4 | hue = np.arange(0, 2* np.pi, 1/360)
5 | hue_array = np.ones_like(hue)
6 |
7 | hsv_map = plt.get_cmap('hsv')
8 | norm = colors.Normalize(0, 2*np.pi)
9 |
10 | tick_units = np.pi/3
11 |
12 | fig = plt.figure(figsize=(5, 5))
13 | # rgb_color = colors.hsv_to_rgb(hue)
14 | ax = plt.subplot(polar=True)
15 |
16 | theta_ticks = np.arange(0, 6, 1) * tick_units
17 | ax.xaxis.set_ticks(theta_ticks)
18 | ax.xaxis.set_ticklabels(
19 | ['0\N{DEGREE SIGN}\nred',
20 | '60\N{DEGREE SIGN}\nyellow',
21 | '120\N{DEGREE SIGN}\ngreen',
22 | '180\N{DEGREE SIGN}\ncyan',
23 | '240\N{DEGREE SIGN}\nblue',
24 | '300\N{DEGREE SIGN}\npurple'], fontsize=16
25 | )
26 | ax.tick_params(pad=-70, rotation='auto', grid_alpha=0.3)
27 |
28 | for n in theta_ticks:
29 | ax.plot([n, n], [1.0, 1.1], linewidth=4, color=[0, 0, 0])
30 |
31 | ax.plot(hue, np.full_like(hue, 0.7), c=[0,0,0])
32 | ax.scatter(hue, hue_array, c=hue, s=80, cmap=hsv_map)
33 | ax.set_rlim([0.5, 1.1])
34 | ax.set_yticks([])
35 | ax.set_theta_zero_location('N')
36 | ax.set_theta_direction(-1)
37 | plt.show()
38 |
39 |
--------------------------------------------------------------------------------
/chapter9/led_rainbow.py:
--------------------------------------------------------------------------------
1 |
2 | import colorsys
3 |
4 |
5 | def show_rainbow(leds, led_range):
6 | led_range = list(led_range)
7 | hue_step = 1.0 / len(led_range)
8 | for index, led_address in enumerate(led_range):
9 | hue = hue_step * index
10 | rgb = colorsys.hsv_to_rgb(hue, 1.0, 0.6)
11 | rgb = [int(c*255) for c in rgb]
12 | leds.set_one(led_address, rgb)
13 |
--------------------------------------------------------------------------------
/chapter9/leds_led_shim.py:
--------------------------------------------------------------------------------
1 | import ledshim
2 |
3 |
4 | class Leds:
5 | @property
6 | def count(self):
7 | return ledshim.width
8 |
9 | def set_one(self, led_number, color):
10 | ledshim.set_pixel(led_number, *color)
11 |
12 | def set_range(self, led_range, color):
13 | for pixel in led_range:
14 | ledshim.set_pixel(pixel, *color)
15 |
16 | def set_all(self, color):
17 | ledshim.set_all(*color)
18 |
19 | def clear(self):
20 | ledshim.clear()
21 |
22 | def show(self):
23 | ledshim.show()
24 |
--------------------------------------------------------------------------------
/chapter9/leds_test.py:
--------------------------------------------------------------------------------
1 | from robot import Robot
2 | from time import sleep
3 |
4 | bot = Robot()
5 | red = (255, 0, 0)
6 | blue = (0, 0, 255)
7 |
8 | while True:
9 | print("red")
10 | bot.leds.set_all(red)
11 | bot.leds.show()
12 | sleep(0.5)
13 | print("blue")
14 | bot.leds.set_all(blue)
15 | bot.leds.show()
16 | sleep(0.5)
17 |
--------------------------------------------------------------------------------
/chapter9/robot.py:
--------------------------------------------------------------------------------
1 | from Raspi_MotorHAT import Raspi_MotorHAT
2 | from gpiozero import DistanceSensor
3 |
4 | import atexit
5 | import leds_led_shim
6 |
7 |
8 | class Robot:
9 | def __init__(self, motorhat_addr=0x6f):
10 | # Setup the motorhat with the passed in address
11 | self._mh = Raspi_MotorHAT(addr=motorhat_addr)
12 |
13 | # get local variable for each motor
14 | self.left_motor = self._mh.getMotor(1)
15 | self.right_motor = self._mh.getMotor(2)
16 |
17 | # Setup The Distance Sensors
18 | self.left_distance_sensor = DistanceSensor(echo=17, trigger=27, queue_len=2)
19 | self.right_distance_sensor = DistanceSensor(echo=5, trigger=6, queue_len=2)
20 |
21 | # Setup the Leds
22 | self.leds = leds_led_shim.Leds()
23 | # ensure the motors get stopped when the code exits
24 | atexit.register(self.stop_all)
25 |
26 | def convert_speed(self, speed):
27 | # Choose the running mode
28 | mode = Raspi_MotorHAT.RELEASE
29 | if speed > 0:
30 | mode = Raspi_MotorHAT.FORWARD
31 | elif speed < 0:
32 | mode = Raspi_MotorHAT.BACKWARD
33 |
34 | # Scale the speed
35 | output_speed = (abs(speed) * 255) // 100
36 | return mode, int(output_speed)
37 |
38 | def set_left(self, speed):
39 | mode, output_speed = self.convert_speed(speed)
40 | self.left_motor.setSpeed(output_speed)
41 | self.left_motor.run(mode)
42 |
43 | def set_right(self, speed):
44 | mode, output_speed = self.convert_speed(speed)
45 | self.right_motor.setSpeed(output_speed)
46 | self.right_motor.run(mode)
47 |
48 | def stop_motors(self):
49 | self.left_motor.run(Raspi_MotorHAT.RELEASE)
50 | self.right_motor.run(Raspi_MotorHAT.RELEASE)
51 |
52 | def stop_all(self):
53 | self.stop_motors()
54 |
55 | # Clear the display
56 | self.leds.clear()
57 | self.leds.show()
58 |
--------------------------------------------------------------------------------
/chapter9/test_rainbow.py:
--------------------------------------------------------------------------------
1 | from time import sleep
2 | from robot import Robot
3 | from led_rainbow import show_rainbow
4 |
5 | bot = Robot()
6 |
7 | while True:
8 | print("on")
9 | show_rainbow(bot.leds, range(bot.leds.count))
10 | bot.leds.show()
11 | sleep(0.5)
12 | print("off")
13 | bot.leds.clear()
14 | bot.leds.show()
15 | sleep(0.5)
16 |
--------------------------------------------------------------------------------