mirror of
https://gitlab.com/yikestone/qt_pi.git
synced 2025-08-03 05:34:13 +05:30
Initial Commit
This commit is contained in:
commit
0f4b22c8ba
21
CMakeLists.txt
Normal file
21
CMakeLists.txt
Normal file
@ -0,0 +1,21 @@
|
|||||||
|
cmake_minimum_required(VERSION 2.8.3)
|
||||||
|
project(qt_pi)
|
||||||
|
find_package(catkin REQUIRED)
|
||||||
|
|
||||||
|
catkin_python_setup()
|
||||||
|
catkin_package(DEPENDS)
|
||||||
|
|
||||||
|
include_directories(
|
||||||
|
)
|
||||||
|
|
||||||
|
install(DIRECTORY config
|
||||||
|
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
||||||
|
)
|
||||||
|
|
||||||
|
install(DIRECTORY launch
|
||||||
|
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
||||||
|
)
|
||||||
|
|
||||||
|
install(DIRECTORY nodes
|
||||||
|
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
||||||
|
)
|
11
config/base_local_planner_params.yaml
Normal file
11
config/base_local_planner_params.yaml
Normal file
@ -0,0 +1,11 @@
|
|||||||
|
TrajectoryPlannerROS:
|
||||||
|
max_vel_x: 0.45
|
||||||
|
min_vel_x: 0.1
|
||||||
|
max_vel_theta: 1.0
|
||||||
|
min_in_place_vel_theta: 0.4
|
||||||
|
|
||||||
|
acc_lim_theta: 3.2
|
||||||
|
acc_lim_x: 2.5
|
||||||
|
acc_lim_y: 2.5
|
||||||
|
|
||||||
|
holonomic_robot: true
|
11
config/costmap_common_params.yaml
Normal file
11
config/costmap_common_params.yaml
Normal file
@ -0,0 +1,11 @@
|
|||||||
|
obstacle_range: 2.5
|
||||||
|
raytrace_range: 3.0
|
||||||
|
footprint: [[-15, -12], [15, -12], [15, 12], [-15, 12]]
|
||||||
|
#robot_radius: ir_of_robot
|
||||||
|
inflation_radius: 0.55
|
||||||
|
|
||||||
|
observation_sources: laser_scan_kinect
|
||||||
|
|
||||||
|
laser_scan_kinect: {sensor_frame: kinect_frame, data_type: LaserScan, topic: /LaserScan, marking: true, clearing: true}
|
||||||
|
|
||||||
|
point_cloud_kinect: {sensor_frame: kinect_frame, data_type: PointCloud, topic: /PointCloud, marking: true, clearing: true}
|
5
config/global_costmap_params.yaml
Normal file
5
config/global_costmap_params.yaml
Normal file
@ -0,0 +1,5 @@
|
|||||||
|
global_costmap:
|
||||||
|
global_frame: /map
|
||||||
|
robot_base_frame: base_link
|
||||||
|
update_frequency: 5.0
|
||||||
|
static_map: true
|
10
config/local_costmap_params.yaml
Normal file
10
config/local_costmap_params.yaml
Normal file
@ -0,0 +1,10 @@
|
|||||||
|
local_costmap:
|
||||||
|
global_frame: odom
|
||||||
|
robot_base_frame: base_link
|
||||||
|
update_frequency: 5.0
|
||||||
|
publish_frequency: 2.0
|
||||||
|
static_map: false
|
||||||
|
rolling_window: true
|
||||||
|
width: 6.0
|
||||||
|
height: 6.0
|
||||||
|
resolution: 0.05
|
30
config/qt_pi_arduino_params.yaml
Normal file
30
config/qt_pi_arduino_params.yaml
Normal file
@ -0,0 +1,30 @@
|
|||||||
|
|
||||||
|
port: /dev/ttyUSB0
|
||||||
|
baud: 57600
|
||||||
|
timeout: 0.1
|
||||||
|
|
||||||
|
rate: 100
|
||||||
|
|
||||||
|
use_base_controller: True
|
||||||
|
base_controller_rate: 50
|
||||||
|
|
||||||
|
# For a robot that uses base_footprint, change base_frame to base_footprint
|
||||||
|
base_frame: base_link
|
||||||
|
camera_frame: camera_depth_frame
|
||||||
|
# === Robot drivetrain parameters
|
||||||
|
wheel_diameter: 0.0703464848466
|
||||||
|
wheel_track: 0.22
|
||||||
|
encoder_resolution: 240 # from Pololu for 131:1 motors
|
||||||
|
motors_reversed: False
|
||||||
|
gear_reduction: 1.0
|
||||||
|
# === PID parameters
|
||||||
|
lKp: 50
|
||||||
|
lKd: 10
|
||||||
|
lKi: 0
|
||||||
|
lKo: 20
|
||||||
|
rKp: 50
|
||||||
|
rKd: 10
|
||||||
|
rKi: 0
|
||||||
|
rKo: 20
|
||||||
|
|
||||||
|
#accel_limit: 0.1
|
6
launch/arduino.launch
Normal file
6
launch/arduino.launch
Normal file
@ -0,0 +1,6 @@
|
|||||||
|
<launch>
|
||||||
|
<node name="arduino" pkg="qt_pi" type="arduino_node.py" output="screen">
|
||||||
|
<rosparam file="$(find qt_pi)/config/qt_pi_arduino_params.yaml" command="load" />
|
||||||
|
</node>
|
||||||
|
|
||||||
|
</launch>
|
17
launch/move_base.launch
Normal file
17
launch/move_base.launch
Normal file
@ -0,0 +1,17 @@
|
|||||||
|
<launch>
|
||||||
|
<master auto="start"/>
|
||||||
|
|
||||||
|
<!-- Run the map server -->
|
||||||
|
<node name="map_server" pkg="map_server" type="map_server" args="$(find qt_pi)/map/Untitled.pgm 0.05"/>
|
||||||
|
|
||||||
|
<!--- Run AMCL -->
|
||||||
|
<include file="$(find amcl)/examples/amcl_diff.launch" />
|
||||||
|
|
||||||
|
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
|
||||||
|
<rosparam file="$(find qt_pi)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
|
||||||
|
<rosparam file="$(find qt_pi)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
|
||||||
|
<rosparam file="$(find qt_pi)/config/local_costmap_params.yaml" command="load" />
|
||||||
|
<rosparam file="$(find qt_pi)/config/global_costmap_params.yaml" command="load" />
|
||||||
|
<rosparam file="$(find qt_pi)/config/base_local_planner_params.yaml" command="load" />
|
||||||
|
</node>
|
||||||
|
</launch>
|
17
launch/qt_pi.launch
Normal file
17
launch/qt_pi.launch
Normal file
@ -0,0 +1,17 @@
|
|||||||
|
<launch>
|
||||||
|
<node name="arduino" pkg="qt_pi" type="arduino_node.py" output="screen">
|
||||||
|
<rosparam file="$(find qt_pi)/config/qt_pi_arduino_params.yaml" command="load" />
|
||||||
|
</node>
|
||||||
|
|
||||||
|
<node name="kinect2_bridge_1455208236617866858" pkg="kinect2_bridge" type="kinect2_bridge">
|
||||||
|
</node>
|
||||||
|
|
||||||
|
<node name="depthimage_to_laserscan" pkg="depthimage_to_laserscan" type="depthimage_to_laserscan">
|
||||||
|
<param name="image" value="kinect2/sd/image_depth_rect" />
|
||||||
|
</node>
|
||||||
|
|
||||||
|
<node name="gmapping" pkg="gmapping" type="slam_gmapping">
|
||||||
|
<param name="base_frame" value="base_link" type="string" />
|
||||||
|
</node>
|
||||||
|
|
||||||
|
</launch>
|
5
map/F404.pgm
Normal file
5
map/F404.pgm
Normal file
File diff suppressed because one or more lines are too long
7
map/F404.yaml
Normal file
7
map/F404.yaml
Normal file
@ -0,0 +1,7 @@
|
|||||||
|
image: F404.pgm
|
||||||
|
resolution: 0.050000
|
||||||
|
origin: [-2000,-2000, 0.000000]
|
||||||
|
negate: 0
|
||||||
|
occupied_thresh: 0.65
|
||||||
|
free_thresh: 0.196
|
||||||
|
|
BIN
map/Untitled.pgm
Normal file
BIN
map/Untitled.pgm
Normal file
Binary file not shown.
7
map/Untitled.yaml
Normal file
7
map/Untitled.yaml
Normal file
@ -0,0 +1,7 @@
|
|||||||
|
image: F404.pgm
|
||||||
|
resolution: 0.050000
|
||||||
|
origin: [10,10, 0.000000]
|
||||||
|
negate: 0
|
||||||
|
occupied_thresh: 0.65
|
||||||
|
free_thresh: 0.196
|
||||||
|
|
87
nodes/arduino_node.py
Executable file
87
nodes/arduino_node.py
Executable file
@ -0,0 +1,87 @@
|
|||||||
|
#!/usr/bin/env python
|
||||||
|
import rospy
|
||||||
|
from qt_pi.arduino_driver import Arduino
|
||||||
|
from qt_pi.base_controller import BaseController
|
||||||
|
from geometry_msgs.msg import Twist
|
||||||
|
import os, time
|
||||||
|
import thread
|
||||||
|
from serial.serialutil import SerialException
|
||||||
|
|
||||||
|
class ArduinoROS():
|
||||||
|
def __init__(self):
|
||||||
|
rospy.init_node('arduino', log_level=rospy.INFO)
|
||||||
|
|
||||||
|
# Get the actual node name in case it is set in the launch file
|
||||||
|
self.name = rospy.get_name()
|
||||||
|
|
||||||
|
# Cleanup when termniating the node
|
||||||
|
rospy.on_shutdown(self.shutdown)
|
||||||
|
|
||||||
|
self.port = rospy.get_param("~port", "/dev/ttyACM0")
|
||||||
|
self.baud = int(rospy.get_param("~baud", 57600))
|
||||||
|
self.timeout = rospy.get_param("~timeout", 0.5)
|
||||||
|
self.base_frame = rospy.get_param("~base_frame", 'base_link')
|
||||||
|
self.camera_frame = rospy.get_param("~camera_frame", 'depth_camera_link')
|
||||||
|
self.motors_reversed = rospy.get_param("~motors_reversed", False)
|
||||||
|
# Overall loop rate: should be faster than fastest sensor rate
|
||||||
|
self.rate = int(rospy.get_param("~rate", 50))
|
||||||
|
r = rospy.Rate(self.rate)
|
||||||
|
|
||||||
|
self.use_base_controller = rospy.get_param("~use_base_controller", False)
|
||||||
|
|
||||||
|
# Initialize a Twist message
|
||||||
|
self.cmd_vel = Twist()
|
||||||
|
|
||||||
|
# A cmd_vel publisher so we can stop the robot when shutting down
|
||||||
|
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
|
||||||
|
|
||||||
|
# Initialize the controlller
|
||||||
|
self.controller = Arduino(self.port, self.baud, self.timeout, self.motors_reversed)
|
||||||
|
|
||||||
|
# Make the connection
|
||||||
|
self.controller.connect()
|
||||||
|
|
||||||
|
rospy.loginfo("Connected to Arduino on port " + self.port + " at " + str(self.baud) + " baud")
|
||||||
|
|
||||||
|
# Reserve a thread lock
|
||||||
|
mutex = thread.allocate_lock()
|
||||||
|
|
||||||
|
# Initialize the base controller if used
|
||||||
|
if self.use_base_controller:
|
||||||
|
self.qt_pi_BaseController = BaseController(self.controller, self.base_frame, self.camera_frame, self.name + "_base_controller")
|
||||||
|
|
||||||
|
# Start polling the sensors and base controller
|
||||||
|
while not rospy.is_shutdown():
|
||||||
|
if self.use_base_controller:
|
||||||
|
mutex.acquire()
|
||||||
|
self.qt_pi_BaseController.poll()
|
||||||
|
mutex.release()
|
||||||
|
|
||||||
|
r.sleep()
|
||||||
|
|
||||||
|
def shutdown(self):
|
||||||
|
rospy.loginfo("Shutting down Arduino Node...")
|
||||||
|
|
||||||
|
# Stop the robot
|
||||||
|
try:
|
||||||
|
rospy.loginfo("Stopping the robot...")
|
||||||
|
self.cmd_vel_pub.Publish(Twist())
|
||||||
|
rospy.sleep(2)
|
||||||
|
except:
|
||||||
|
pass
|
||||||
|
|
||||||
|
# Close the serial port
|
||||||
|
try:
|
||||||
|
self.controller.close()
|
||||||
|
except:
|
||||||
|
pass
|
||||||
|
finally:
|
||||||
|
rospy.loginfo("Serial port closed.")
|
||||||
|
os._exit(0)
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
try:
|
||||||
|
qt_pi_Arduino = ArduinoROS()
|
||||||
|
except SerialException:
|
||||||
|
rospy.logerr("Serial exception trying to open port.")
|
||||||
|
os._exit(0)
|
17
package.xml
Normal file
17
package.xml
Normal file
@ -0,0 +1,17 @@
|
|||||||
|
<package format="2">
|
||||||
|
<name>qt_pi</name>
|
||||||
|
<version>0.0.0</version>
|
||||||
|
<description>The qt_pi package</description>
|
||||||
|
<maintainer email="rishabhkundu13@gmail.com">Rishabh Kundu</maintainer>
|
||||||
|
<license>MIT</license>
|
||||||
|
<buildtool_depend>catkin</buildtool_depend>
|
||||||
|
|
||||||
|
<exec_depend>rospy</exec_depend>
|
||||||
|
<exec_depend>std_msgs</exec_depend>
|
||||||
|
<exec_depend>geometry_msgs</exec_depend>
|
||||||
|
<exec_depend>nav_msgs</exec_depend>
|
||||||
|
<exec_depend>tf</exec_depend>
|
||||||
|
<exec_depend>python-serial</exec_depend>
|
||||||
|
<export>
|
||||||
|
</export>
|
||||||
|
</package>
|
0
scripts/qt_pi/__init__.py
Normal file
0
scripts/qt_pi/__init__.py
Normal file
291
scripts/qt_pi/arduino_driver.py
Executable file
291
scripts/qt_pi/arduino_driver.py
Executable file
@ -0,0 +1,291 @@
|
|||||||
|
|
||||||
|
import thread
|
||||||
|
from math import pi as PI, degrees, radians
|
||||||
|
import os
|
||||||
|
import time
|
||||||
|
import sys, traceback
|
||||||
|
from serial.serialutil import SerialException
|
||||||
|
from serial import Serial
|
||||||
|
|
||||||
|
class Arduino:
|
||||||
|
|
||||||
|
def __init__(self, port="/dev/ttyUSB0", baudrate=57600, timeout=0.5, motors_reversed=False):
|
||||||
|
|
||||||
|
self.PID_RATE = 30 # Do not change this! It is a fixed property of the Arduino PID controller.
|
||||||
|
self.PID_INTERVAL = 1000 / 30
|
||||||
|
|
||||||
|
self.port = port
|
||||||
|
self.baudrate = baudrate
|
||||||
|
self.timeout = timeout
|
||||||
|
self.encoder_count = 0
|
||||||
|
self.writeTimeout = timeout
|
||||||
|
self.interCharTimeout = timeout / 30.
|
||||||
|
self.motors_reversed = motors_reversed
|
||||||
|
# Keep things thread safe
|
||||||
|
self.mutex = thread.allocate_lock()
|
||||||
|
|
||||||
|
def connect(self):
|
||||||
|
try:
|
||||||
|
print "Connecting to Arduino on port", self.port, "..."
|
||||||
|
self.port = Serial(port=self.port, baudrate=self.baudrate, timeout=self.timeout, writeTimeout=self.writeTimeout)
|
||||||
|
# The next line is necessary to give the firmware time to wake up.
|
||||||
|
time.sleep(1)
|
||||||
|
test = self.get_baud()
|
||||||
|
if test != self.baudrate:
|
||||||
|
time.sleep(1)
|
||||||
|
test = self.get_baud()
|
||||||
|
if test != self.baudrate:
|
||||||
|
raise SerialException
|
||||||
|
print "Connected at", self.baudrate
|
||||||
|
print "Arduino is ready."
|
||||||
|
|
||||||
|
except SerialException:
|
||||||
|
print "Serial Exception:"
|
||||||
|
print sys.exc_info()
|
||||||
|
print "Traceback follows:"
|
||||||
|
traceback.print_exc(file=sys.stdout)
|
||||||
|
print "Cannot connect to Arduino!"
|
||||||
|
os._exit(1)
|
||||||
|
|
||||||
|
def open(self):
|
||||||
|
''' Open the serial port.
|
||||||
|
'''
|
||||||
|
self.port.open()
|
||||||
|
|
||||||
|
def close(self):
|
||||||
|
''' Close the serial port.
|
||||||
|
'''
|
||||||
|
self.port.close()
|
||||||
|
|
||||||
|
def send(self, cmd):
|
||||||
|
''' This command should not be used on its own: it is called by the execute commands
|
||||||
|
below in a thread safe manner.
|
||||||
|
'''
|
||||||
|
self.port.write(cmd + '\r')
|
||||||
|
|
||||||
|
def recv(self, timeout=0.5):
|
||||||
|
timeout = min(timeout, self.timeout)
|
||||||
|
''' This command should not be used on its own: it is called by the execute commands
|
||||||
|
below in a thread safe manner. Note: we use read() instead of readline() since
|
||||||
|
readline() tends to return garbage characters from the Arduino
|
||||||
|
'''
|
||||||
|
c = ''
|
||||||
|
value = ''
|
||||||
|
attempts = 0
|
||||||
|
while c != '\r':
|
||||||
|
c = self.port.read(1)
|
||||||
|
value += c
|
||||||
|
attempts += 1
|
||||||
|
if attempts * self.interCharTimeout > timeout:
|
||||||
|
return None
|
||||||
|
|
||||||
|
value = value.strip('\r')
|
||||||
|
|
||||||
|
return value
|
||||||
|
|
||||||
|
def recv_ack(self):
|
||||||
|
''' This command should not be used on its own: it is called by the execute commands
|
||||||
|
below in a thread safe manner.
|
||||||
|
'''
|
||||||
|
ack = self.recv(self.timeout)
|
||||||
|
return ack == 'OK'
|
||||||
|
|
||||||
|
def recv_int(self):
|
||||||
|
''' This command should not be used on its own: it is called by the execute commands
|
||||||
|
below in a thread safe manner.
|
||||||
|
'''
|
||||||
|
value = self.recv(self.timeout)
|
||||||
|
try:
|
||||||
|
return int(value)
|
||||||
|
except:
|
||||||
|
return None
|
||||||
|
|
||||||
|
|
||||||
|
def execute(self, cmd):
|
||||||
|
''' Thread safe execution of "cmd" on the Arduino returning a single integer value.
|
||||||
|
'''
|
||||||
|
self.mutex.acquire()
|
||||||
|
|
||||||
|
try:
|
||||||
|
self.port.flushInput()
|
||||||
|
except:
|
||||||
|
pass
|
||||||
|
|
||||||
|
ntries = 1
|
||||||
|
attempts = 0
|
||||||
|
|
||||||
|
try:
|
||||||
|
self.port.write(cmd + '\r')
|
||||||
|
value = self.recv(self.timeout)
|
||||||
|
while attempts < ntries and (value == '' or value == 'Invalid Command' or value == None):
|
||||||
|
try:
|
||||||
|
self.port.flushInput()
|
||||||
|
self.port.write(cmd + '\r')
|
||||||
|
value = self.recv(self.timeout)
|
||||||
|
except:
|
||||||
|
print "Exception executing command: " + cmd
|
||||||
|
attempts += 1
|
||||||
|
except:
|
||||||
|
self.mutex.release()
|
||||||
|
print "Exception executing command: " + cmd
|
||||||
|
value = None
|
||||||
|
|
||||||
|
self.mutex.release()
|
||||||
|
return int(value)
|
||||||
|
|
||||||
|
def recv_array(self):
|
||||||
|
''' This command should not be used on its own: it is called by the execute commands
|
||||||
|
below in a thread safe manner.
|
||||||
|
'''
|
||||||
|
try:
|
||||||
|
values = self.recv(self.timeout * 8).split()
|
||||||
|
return map(int, values)
|
||||||
|
except:
|
||||||
|
return []
|
||||||
|
|
||||||
|
def execute_array(self, cmd):
|
||||||
|
''' Thread safe execution of "cmd" on the Arduino returning an array.
|
||||||
|
'''
|
||||||
|
self.mutex.acquire()
|
||||||
|
try:
|
||||||
|
self.port.flushInput()
|
||||||
|
except:
|
||||||
|
pass
|
||||||
|
ntries = 1
|
||||||
|
attempts = 0
|
||||||
|
|
||||||
|
try:
|
||||||
|
self.port.write(cmd + '\r')
|
||||||
|
values = self.recv_array()
|
||||||
|
while attempts < ntries and (values == '' or values == 'Invalid Command' or values == [] or values == None):
|
||||||
|
try:
|
||||||
|
self.port.flushInput()
|
||||||
|
self.port.write(cmd + '\r')
|
||||||
|
values = self.recv_array()
|
||||||
|
except:
|
||||||
|
print("Exception executing command: " + cmd)
|
||||||
|
attempts += 1
|
||||||
|
except:
|
||||||
|
self.mutex.release()
|
||||||
|
print "Exception executing command: " + cmd
|
||||||
|
raise SerialException
|
||||||
|
return []
|
||||||
|
|
||||||
|
try:
|
||||||
|
values = map(int, values)
|
||||||
|
except:
|
||||||
|
values = []
|
||||||
|
self.mutex.release()
|
||||||
|
return values
|
||||||
|
|
||||||
|
def execute_ack(self, cmd):
|
||||||
|
''' Thread safe execution of "cmd" on the Arduino returning True if response is ACK.
|
||||||
|
'''
|
||||||
|
self.mutex.acquire()
|
||||||
|
|
||||||
|
try:
|
||||||
|
self.port.flushInput()
|
||||||
|
except:
|
||||||
|
pass
|
||||||
|
|
||||||
|
ntries = 1
|
||||||
|
attempts = 0
|
||||||
|
|
||||||
|
try:
|
||||||
|
self.port.write(cmd + '\r')
|
||||||
|
ack = self.recv(self.timeout)
|
||||||
|
while attempts < ntries and (ack == '' or ack == 'Invalid Command' or ack == None):
|
||||||
|
try:
|
||||||
|
self.port.flushInput()
|
||||||
|
self.port.write(cmd + '\r')
|
||||||
|
ack = self.recv(self.timeout)
|
||||||
|
except:
|
||||||
|
print "Exception executing command: " + cmd
|
||||||
|
attempts += 1
|
||||||
|
except:
|
||||||
|
self.mutex.release()
|
||||||
|
print "execute_ack exception when executing", cmd
|
||||||
|
print sys.exc_info()
|
||||||
|
return 0
|
||||||
|
|
||||||
|
self.mutex.release()
|
||||||
|
return ack == 'OK'
|
||||||
|
|
||||||
|
def update_pid(self, lKp, lKd, lKi, lKo, rKp, rKd, rKi, rKo):
|
||||||
|
''' Set the PID parameters on the Arduino
|
||||||
|
'''
|
||||||
|
print "Updating PID parameters"
|
||||||
|
cmd = 'L ' + str(lKp) + ':' + str(lKd) + ':' + str(lKi) + ':' + str(lKo)
|
||||||
|
self.execute_ack(cmd)
|
||||||
|
cmd = 'R ' + str(rKp) + ':' + str(rKd) + ':' + str(rKi) + ':' + str(rKo)
|
||||||
|
self.execute_ack(cmd)
|
||||||
|
|
||||||
|
def get_baud(self):
|
||||||
|
''' Get the current baud rate on the serial port.
|
||||||
|
'''
|
||||||
|
try:
|
||||||
|
return int(self.execute('b'));
|
||||||
|
except:
|
||||||
|
return None
|
||||||
|
|
||||||
|
def get_encoder_counts(self):
|
||||||
|
values = self.execute_array('e')
|
||||||
|
if len(values) != 2:
|
||||||
|
print "Encoder count was not 2"
|
||||||
|
raise SerialException
|
||||||
|
return None
|
||||||
|
else:
|
||||||
|
if self.motors_reversed:
|
||||||
|
values[0], values[1] = -values[0], -values[1]
|
||||||
|
print values
|
||||||
|
return values
|
||||||
|
|
||||||
|
def reset_encoders(self):
|
||||||
|
''' Reset the encoder counts to 0
|
||||||
|
'''
|
||||||
|
return self.execute_ack('r')
|
||||||
|
|
||||||
|
def drive(self, right, left):
|
||||||
|
''' Speeds are given in encoder ticks per PID interval
|
||||||
|
'''
|
||||||
|
if self.motors_reversed:
|
||||||
|
left, right = -left, -right
|
||||||
|
return self.execute_ack('m %d %d' %(right, left))
|
||||||
|
|
||||||
|
def drive_m_per_s(self, right, left):
|
||||||
|
''' Set the motor speeds in meters per second.
|
||||||
|
'''
|
||||||
|
left_revs_per_second = float(left) / (self.wheel_diameter * PI)
|
||||||
|
right_revs_per_second = float(right) / (self.wheel_diameter * PI)
|
||||||
|
|
||||||
|
left_ticks_per_loop = int(left_revs_per_second * self.encoder_resolution * self.PID_INTERVAL * self.gear_reduction)
|
||||||
|
right_ticks_per_loop = int(right_revs_per_second * self.encoder_resolution * self.PID_INTERVAL * self.gear_reduction)
|
||||||
|
|
||||||
|
self.drive(right_ticks_per_loop , left_ticks_per_loop )
|
||||||
|
|
||||||
|
def stop(self):
|
||||||
|
''' Stop both motors.
|
||||||
|
'''
|
||||||
|
self.drive(0, 0)
|
||||||
|
def get_batt_lvl(self):
|
||||||
|
return self.execute('B')
|
||||||
|
if __name__ == "__main__":
|
||||||
|
if os.name == "posix":
|
||||||
|
portName = "/dev/ttyACM0"
|
||||||
|
else:
|
||||||
|
portName = "COM43" # Windows style COM port.
|
||||||
|
|
||||||
|
baudRate = 57600
|
||||||
|
|
||||||
|
qt_pi_Arduino = Arduino(port=portName, baudrate=baudRate, timeout=0.5)
|
||||||
|
qt_pi_Arduino.connect()
|
||||||
|
|
||||||
|
print "Sleeping for 1 second..."
|
||||||
|
time.sleep(1)
|
||||||
|
|
||||||
|
print "Connection test successful.",
|
||||||
|
|
||||||
|
qt_pi_Arduino.stop()
|
||||||
|
qt_pi_Arduino.close()
|
||||||
|
|
||||||
|
print "Shutting down Arduino."
|
BIN
scripts/qt_pi/arduino_driver.pyc
Normal file
BIN
scripts/qt_pi/arduino_driver.pyc
Normal file
Binary file not shown.
248
scripts/qt_pi/base_controller.py
Executable file
248
scripts/qt_pi/base_controller.py
Executable file
@ -0,0 +1,248 @@
|
|||||||
|
#!/usr/bin/env python
|
||||||
|
|
||||||
|
import roslib; roslib.load_manifest('qt_pi')
|
||||||
|
import rospy
|
||||||
|
import os
|
||||||
|
|
||||||
|
from math import sin, cos, pi
|
||||||
|
from geometry_msgs.msg import Quaternion, Twist, Pose
|
||||||
|
from std_msgs.msg import Float32
|
||||||
|
from nav_msgs.msg import Odometry
|
||||||
|
from tf.broadcaster import TransformBroadcaster
|
||||||
|
|
||||||
|
""" Class to receive Twist commands and publish Odometry data """
|
||||||
|
class BaseController:
|
||||||
|
def __init__(self, arduino, base_frame, camera_frame, name="base_controllers"):
|
||||||
|
self.arduino = arduino
|
||||||
|
self.name = name
|
||||||
|
self.base_frame = base_frame
|
||||||
|
self.camera_frame = camera_frame
|
||||||
|
self.rate = float(rospy.get_param("~base_controller_rate", 10))
|
||||||
|
self.timeout = rospy.get_param("~base_controller_timeout", 1.0)
|
||||||
|
self.stopped = False
|
||||||
|
|
||||||
|
pid_params = dict()
|
||||||
|
pid_params['wheel_diameter'] = rospy.get_param("~wheel_diameter", "")
|
||||||
|
pid_params['wheel_track'] = rospy.get_param("~wheel_track", "")
|
||||||
|
pid_params['encoder_resolution'] = rospy.get_param("~encoder_resolution", "")
|
||||||
|
pid_params['gear_reduction'] = rospy.get_param("~gear_reduction", 1.0)
|
||||||
|
pid_params['lKp'] = rospy.get_param("~lKp", 20)
|
||||||
|
pid_params['lKd'] = rospy.get_param("~lKd", 12)
|
||||||
|
pid_params['lKi'] = rospy.get_param("~lKi", 0)
|
||||||
|
pid_params['lKo'] = rospy.get_param("~lKo", 50)
|
||||||
|
pid_params['rKp'] = rospy.get_param("~rKp", 20)
|
||||||
|
pid_params['rKd'] = rospy.get_param("~rKd", 12)
|
||||||
|
pid_params['rKi'] = rospy.get_param("~rKi", 0)
|
||||||
|
pid_params['rKo'] = rospy.get_param("~rKo", 50)
|
||||||
|
|
||||||
|
|
||||||
|
self.accel_limit = rospy.get_param('~accel_limit', 0.1)
|
||||||
|
self.motors_reversed = rospy.get_param("~motors_reversed", False)
|
||||||
|
self.gear_reduction = rospy.get_param("~gear_reduction", 1.0)
|
||||||
|
# Set up PID parameters and check for missing values
|
||||||
|
self.setup_pid(pid_params)
|
||||||
|
|
||||||
|
# How many encoder ticks are there per meter?
|
||||||
|
self.ticks_per_meter = self.encoder_resolution / (self.wheel_diameter * pi)
|
||||||
|
|
||||||
|
# What is the maximum acceleration we will tolerate when changing wheel speeds?
|
||||||
|
self.max_accel = self.accel_limit * self.ticks_per_meter / self.rate
|
||||||
|
|
||||||
|
# Track how often we get a bad encoder count (if any)
|
||||||
|
self.bad_encoder_count = 0
|
||||||
|
|
||||||
|
now = rospy.Time.now()
|
||||||
|
self.then = now # time for determining dx/dy
|
||||||
|
self.t_delta = rospy.Duration(1.0 / self.rate)
|
||||||
|
self.t_next = now + self.t_delta
|
||||||
|
|
||||||
|
# Internal data
|
||||||
|
self.enc_left = None # encoder readings
|
||||||
|
self.enc_right = None
|
||||||
|
self.x = 0 # position in xy plane
|
||||||
|
self.y = 0
|
||||||
|
self.th = 0 # rotation in radians
|
||||||
|
self.v_left = 0
|
||||||
|
self.v_right = 0
|
||||||
|
self.v_des_left = 0 # cmd_vel setpoint
|
||||||
|
self.v_des_right = 0
|
||||||
|
self.last_cmd_vel = now
|
||||||
|
self.batt_lvl = 0.0
|
||||||
|
# Subscriptions
|
||||||
|
rospy.Subscriber("cmd_vel", Twist, self.cmdVelCallback)
|
||||||
|
|
||||||
|
# Clear any old odometry info
|
||||||
|
self.arduino.reset_encoders()
|
||||||
|
|
||||||
|
# Set up the odometry broadcaster
|
||||||
|
self.odomPub = rospy.Publisher('odom', Odometry, queue_size=5)
|
||||||
|
self.batt_lvl_pub = rospy.Publisher('battery_level', Float32, queue_size=1)
|
||||||
|
self.odomBroadcaster = TransformBroadcaster()
|
||||||
|
self.cameraBroadcaster = TransformBroadcaster()
|
||||||
|
rospy.loginfo("Started base controller for a base of " + str(self.wheel_track) + "m wide with " + str(self.encoder_resolution) + " ticks per rev")
|
||||||
|
rospy.loginfo("Publishing odometry data at: " + str(self.rate) + " Hz using " + str(self.base_frame) + " as base frame")
|
||||||
|
|
||||||
|
def setup_pid(self, pid_params):
|
||||||
|
# Check to see if any PID parameters are missing
|
||||||
|
missing_params = False
|
||||||
|
for param in pid_params:
|
||||||
|
if pid_params[param] == "":
|
||||||
|
print("*** PID Parameter " + param + " is missing. ***")
|
||||||
|
missing_params = True
|
||||||
|
|
||||||
|
if missing_params:
|
||||||
|
os._exit(1)
|
||||||
|
|
||||||
|
self.wheel_diameter = pid_params['wheel_diameter']
|
||||||
|
self.wheel_track = pid_params['wheel_track']
|
||||||
|
self.encoder_resolution = pid_params['encoder_resolution']
|
||||||
|
|
||||||
|
self.lKp = pid_params['lKp']
|
||||||
|
self.lKd = pid_params['lKd']
|
||||||
|
self.lKi = pid_params['lKi']
|
||||||
|
self.lKo = pid_params['lKo']
|
||||||
|
|
||||||
|
self.rKp = pid_params['rKp']
|
||||||
|
self.rKd = pid_params['rKd']
|
||||||
|
self.rKi = pid_params['rKi']
|
||||||
|
self.rKo = pid_params['rKo']
|
||||||
|
|
||||||
|
self.arduino.update_pid(self.lKp, self.lKd, self.lKi, self.lKo,self.rKp, self.rKd, self.rKi, self.rKo)
|
||||||
|
|
||||||
|
def poll(self):
|
||||||
|
now = rospy.Time.now()
|
||||||
|
if now > self.t_next:
|
||||||
|
# Read the encoders
|
||||||
|
try:
|
||||||
|
left_enc, right_enc = self.arduino.get_encoder_counts()
|
||||||
|
except:
|
||||||
|
self.bad_encoder_count += 1
|
||||||
|
rospy.logerr("Encoder exception count: " + str(self.bad_encoder_count))
|
||||||
|
return
|
||||||
|
|
||||||
|
dt = now - self.then
|
||||||
|
self.then = now
|
||||||
|
dt = dt.to_sec()
|
||||||
|
|
||||||
|
# Calculate odometry
|
||||||
|
if self.enc_left == None:
|
||||||
|
dright = 0
|
||||||
|
dleft = 0
|
||||||
|
else:
|
||||||
|
dright = (right_enc - self.enc_right) / self.ticks_per_meter
|
||||||
|
dleft = (left_enc - self.enc_left) / self.ticks_per_meter
|
||||||
|
|
||||||
|
self.enc_right = right_enc
|
||||||
|
self.enc_left = left_enc
|
||||||
|
|
||||||
|
dxy_ave = (dright + dleft) / 2.0
|
||||||
|
dth = (dright - dleft) / self.wheel_track
|
||||||
|
vxy = dxy_ave / dt
|
||||||
|
vth = dth / dt
|
||||||
|
|
||||||
|
if (dxy_ave != 0):
|
||||||
|
dx = cos(dth) * dxy_ave
|
||||||
|
dy = -sin(dth) * dxy_ave
|
||||||
|
self.x += (cos(self.th) * dx - sin(self.th) * dy)
|
||||||
|
self.y += (sin(self.th) * dx + cos(self.th) * dy)
|
||||||
|
|
||||||
|
if (dth != 0):
|
||||||
|
self.th += dth
|
||||||
|
|
||||||
|
quaternion = Quaternion()
|
||||||
|
quaternion.x = 0.0
|
||||||
|
quaternion.y = 0.0
|
||||||
|
quaternion.z = sin(self.th / 2.0)
|
||||||
|
quaternion.w = cos(self.th / 2.0)
|
||||||
|
|
||||||
|
# Create the odometry transform frame broadcaster.
|
||||||
|
self.odomBroadcaster.sendTransform(
|
||||||
|
(self.x, self.y, 0),
|
||||||
|
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
|
||||||
|
rospy.Time.now(),
|
||||||
|
self.base_frame,
|
||||||
|
"odom"
|
||||||
|
)
|
||||||
|
#self.cameraBroadcaster.sendTransform((self.x + 0.1, self.y - 0.05, 0.125),
|
||||||
|
self.cameraBroadcaster.sendTransform((0.1, -0.05, 0.125),
|
||||||
|
(0,0,0,1),
|
||||||
|
rospy.Time.now(),
|
||||||
|
self.camera_frame,
|
||||||
|
self.base_frame)
|
||||||
|
|
||||||
|
odom = Odometry()
|
||||||
|
odom.header.frame_id = "odom"
|
||||||
|
odom.child_frame_id = self.base_frame
|
||||||
|
odom.header.stamp = now
|
||||||
|
odom.pose.pose.position.x = self.x
|
||||||
|
odom.pose.pose.position.y = self.y
|
||||||
|
odom.pose.pose.position.z = 0
|
||||||
|
odom.pose.pose.orientation = quaternion
|
||||||
|
odom.twist.twist.linear.x = vxy
|
||||||
|
odom.twist.twist.linear.y = 0
|
||||||
|
odom.twist.twist.angular.z = vth
|
||||||
|
|
||||||
|
self.odomPub.publish(odom)
|
||||||
|
|
||||||
|
if now > (self.last_cmd_vel + rospy.Duration(self.timeout)):
|
||||||
|
self.v_des_left = 0
|
||||||
|
self.v_des_right = 0
|
||||||
|
|
||||||
|
if self.v_left < self.v_des_left:
|
||||||
|
self.v_left += self.max_accel
|
||||||
|
if self.v_left > self.v_des_left:
|
||||||
|
self.v_left = self.v_des_left
|
||||||
|
else:
|
||||||
|
self.v_left -= self.max_accel
|
||||||
|
if self.v_left < self.v_des_left:
|
||||||
|
self.v_left = self.v_des_left
|
||||||
|
|
||||||
|
if self.v_right < self.v_des_right:
|
||||||
|
self.v_right += self.max_accel
|
||||||
|
if self.v_right > self.v_des_right:
|
||||||
|
self.v_right = self.v_des_right
|
||||||
|
else:
|
||||||
|
self.v_right -= self.max_accel
|
||||||
|
if self.v_right < self.v_des_right:
|
||||||
|
self.v_right = self.v_des_right
|
||||||
|
|
||||||
|
# Set motor speeds in encoder ticks per PID loop
|
||||||
|
if not self.stopped:
|
||||||
|
self.arduino.drive(self.v_left, self.v_right)
|
||||||
|
|
||||||
|
self.t_next = now + self.t_delta
|
||||||
|
self.batt_lvl = self.arduino.get_batt_lvl()
|
||||||
|
|
||||||
|
self.batt_lvl_pub.publish(((self.batt_lvl) / 5) * 5 * 12.85 / 900)
|
||||||
|
def stop(self):
|
||||||
|
self.stopped = True
|
||||||
|
self.arduino.drive(0, 0)
|
||||||
|
|
||||||
|
def cmdVelCallback(self, req):
|
||||||
|
# Handle velocity-based movement requests
|
||||||
|
self.last_cmd_vel = rospy.Time.now()
|
||||||
|
|
||||||
|
x = req.linear.x # m/s
|
||||||
|
th = req.angular.z # rad/s
|
||||||
|
|
||||||
|
if x == 0:
|
||||||
|
# Turn in place
|
||||||
|
right = th * self.wheel_track * self.gear_reduction / 2.0
|
||||||
|
left = -right
|
||||||
|
elif th == 0:
|
||||||
|
# Pure forward/backward motion
|
||||||
|
left = right = x
|
||||||
|
else:
|
||||||
|
# Rotation about a point in space
|
||||||
|
left = x - th * self.wheel_track * self.gear_reduction / 2.0
|
||||||
|
right = x + th * self.wheel_track * self.gear_reduction / 2.0
|
||||||
|
|
||||||
|
self.v_des_left = int(left * self.ticks_per_meter / self.arduino.PID_RATE)
|
||||||
|
self.v_des_right = int(right * self.ticks_per_meter / self.arduino.PID_RATE)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
BIN
scripts/qt_pi/base_controller.pyc
Normal file
BIN
scripts/qt_pi/base_controller.pyc
Normal file
Binary file not shown.
11
setup.py
Normal file
11
setup.py
Normal file
@ -0,0 +1,11 @@
|
|||||||
|
#!/usr/bin/env python
|
||||||
|
|
||||||
|
from distutils.core import setup
|
||||||
|
from catkin_pkg.python_setup import generate_distutils_setup
|
||||||
|
|
||||||
|
d = generate_distutils_setup(
|
||||||
|
packages=['qt_pi'],
|
||||||
|
package_dir={'': 'scripts'},
|
||||||
|
)
|
||||||
|
|
||||||
|
setup(**d)
|
205
src/libraries/Arduino/Arduino.ino
Normal file
205
src/libraries/Arduino/Arduino.ino
Normal file
@ -0,0 +1,205 @@
|
|||||||
|
#define BAUDRATE 57600
|
||||||
|
#define MAX_PWM 255
|
||||||
|
|
||||||
|
#if defined(ARDUINO) && ARDUINO >= 100
|
||||||
|
#include "Arduino.h"
|
||||||
|
#else
|
||||||
|
#include "WProgram.h"
|
||||||
|
#endif
|
||||||
|
#include "commands.h"
|
||||||
|
#include "motor_driver.h"
|
||||||
|
#include "encoder_driver.h"
|
||||||
|
#include "diff_controller.h"
|
||||||
|
#define PID_RATE 30 // Hz
|
||||||
|
const int PID_INTERVAL = 1000 / PID_RATE;
|
||||||
|
unsigned long nextPID = PID_INTERVAL;
|
||||||
|
#define AUTO_STOP_INTERVAL 2000
|
||||||
|
long lastMotorCommand = AUTO_STOP_INTERVAL;
|
||||||
|
int arg = 0;
|
||||||
|
int index = 0;
|
||||||
|
char chr;
|
||||||
|
char cmd;
|
||||||
|
char argv1[16];
|
||||||
|
char argv2[16];
|
||||||
|
long arg1;
|
||||||
|
long arg2;
|
||||||
|
|
||||||
|
void resetCommand() {
|
||||||
|
cmd = NULL;
|
||||||
|
memset(argv1, 0, sizeof(argv1));
|
||||||
|
memset(argv2, 0, sizeof(argv2));
|
||||||
|
arg1 = 0;
|
||||||
|
arg2 = 0;
|
||||||
|
arg = 0;
|
||||||
|
index = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
int runCommand() {
|
||||||
|
int i = 0;
|
||||||
|
char *p = argv1;
|
||||||
|
char *str;
|
||||||
|
int pid_args[4];
|
||||||
|
arg1 = atoi(argv1);
|
||||||
|
arg2 = atoi(argv2);
|
||||||
|
|
||||||
|
switch(cmd) {
|
||||||
|
case GET_BAUDRATE:
|
||||||
|
Serial.println(BAUDRATE);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case READ_ENCODERS:
|
||||||
|
Serial.print(readEncoder(LEFT));
|
||||||
|
Serial.print(" ");
|
||||||
|
Serial.println(readEncoder(RIGHT));
|
||||||
|
break;
|
||||||
|
|
||||||
|
case RESET_ENCODERS:
|
||||||
|
resetEncoders();
|
||||||
|
resetPID();
|
||||||
|
Serial.println("OK");
|
||||||
|
break;
|
||||||
|
|
||||||
|
case MOTOR_SPEEDS:
|
||||||
|
/* Reset the auto stop timer */
|
||||||
|
lastMotorCommand = millis();
|
||||||
|
if (arg1 == 0 && arg2 == 0) {
|
||||||
|
setMotorSpeeds(0, 0);
|
||||||
|
resetPID();
|
||||||
|
moving = 0;
|
||||||
|
}
|
||||||
|
else moving = 1;
|
||||||
|
leftPID.TargetTicksPerFrame = arg1;
|
||||||
|
rightPID.TargetTicksPerFrame = arg2;
|
||||||
|
Serial.println("OK");
|
||||||
|
break;
|
||||||
|
|
||||||
|
case UPDATE_PID_L:
|
||||||
|
while ((str = strtok_r(p, ":", &p)) != '\0') {
|
||||||
|
pid_args[i] = atoi(str);
|
||||||
|
i++;
|
||||||
|
}
|
||||||
|
leftPID.Kp = pid_args[0];
|
||||||
|
leftPID.Kd = pid_args[1];
|
||||||
|
leftPID.Ki = pid_args[2];
|
||||||
|
leftPID.Ko = pid_args[3];
|
||||||
|
Serial.println("OK");
|
||||||
|
break;
|
||||||
|
|
||||||
|
case UPDATE_PID_R:
|
||||||
|
while ((str = strtok_r(p, ":", &p)) != '\0') {
|
||||||
|
pid_args[i] = atoi(str);
|
||||||
|
i++;
|
||||||
|
}
|
||||||
|
rightPID.Kp = pid_args[0];
|
||||||
|
rightPID.Kd = pid_args[1];
|
||||||
|
rightPID.Ki = pid_args[2];
|
||||||
|
rightPID.Ko = pid_args[3];
|
||||||
|
Serial.println("OK");
|
||||||
|
break;
|
||||||
|
|
||||||
|
case DISP_PID_P:
|
||||||
|
Serial.println(leftPID.Kp);
|
||||||
|
Serial.println(leftPID.Kd);
|
||||||
|
Serial.println(leftPID.Ki);
|
||||||
|
Serial.println(leftPID.Ko);
|
||||||
|
Serial.println(rightPID.Kp);
|
||||||
|
Serial.println(rightPID.Kd);
|
||||||
|
Serial.println(rightPID.Ki);
|
||||||
|
Serial.println(rightPID.Ko);
|
||||||
|
Serial.println("OK");
|
||||||
|
break;
|
||||||
|
case BATTERY_LVL:
|
||||||
|
Serial.println(analogRead(A0));
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
Serial.println("Invalid Command");
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Setup function--runs once at startup. */
|
||||||
|
void setup() {
|
||||||
|
Serial.begin(BAUDRATE);
|
||||||
|
pinMode(A0,INPUT);
|
||||||
|
digitalWrite(A0,1);
|
||||||
|
DDRC &= ~(1<<LEFT_ENC_PIN_A);
|
||||||
|
DDRC &= ~(1<<LEFT_ENC_PIN_B);
|
||||||
|
DDRD &= ~(1<<RIGHT_ENC_PIN_A);
|
||||||
|
DDRD &= ~(1<<RIGHT_ENC_PIN_B);
|
||||||
|
|
||||||
|
//enable pull up resistors
|
||||||
|
PORTC |= (1<<LEFT_ENC_PIN_A);
|
||||||
|
PORTC |= (1<<LEFT_ENC_PIN_B);
|
||||||
|
PORTD |= (1<<RIGHT_ENC_PIN_A);
|
||||||
|
PORTD |= (1<<RIGHT_ENC_PIN_B);
|
||||||
|
|
||||||
|
// tell pin change mask to listen to left encoder pins
|
||||||
|
PCMSK1 |= (1 << LEFT_ENC_PIN_A)|(1 << LEFT_ENC_PIN_B);
|
||||||
|
// tell pin change mask to listen to right encoder pins
|
||||||
|
PCMSK2 |= (1 << RIGHT_ENC_PIN_A)|(1 << RIGHT_ENC_PIN_B);
|
||||||
|
|
||||||
|
// enable PCINT1 and PCINT2 interrupt in the general interrupt mask
|
||||||
|
PCICR |= (1 << PCIE1) | (1 << PCIE2);
|
||||||
|
|
||||||
|
initMotorController();
|
||||||
|
resetPID();
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void loop() {
|
||||||
|
|
||||||
|
|
||||||
|
while (Serial.available() > 0) {
|
||||||
|
|
||||||
|
// Read the next character
|
||||||
|
chr = Serial.read();
|
||||||
|
|
||||||
|
// Terminate a command with a CR
|
||||||
|
if (chr == 13) {
|
||||||
|
if (arg == 1) argv1[index] = NULL;
|
||||||
|
else if (arg == 2) argv2[index] = NULL;
|
||||||
|
runCommand();
|
||||||
|
resetCommand();
|
||||||
|
}
|
||||||
|
// Use spaces to delimit parts of the command
|
||||||
|
else if (chr == ' ') {
|
||||||
|
// Step through the arguments
|
||||||
|
if (arg == 0) arg = 1;
|
||||||
|
else if (arg == 1) {
|
||||||
|
argv1[index] = NULL;
|
||||||
|
arg = 2;
|
||||||
|
index = 0;
|
||||||
|
}
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
if (arg == 0) {
|
||||||
|
// The first arg is the single-letter command
|
||||||
|
cmd = chr;
|
||||||
|
}
|
||||||
|
else if (arg == 1) {
|
||||||
|
// Subsequent arguments can be more than one character
|
||||||
|
argv1[index] = chr;
|
||||||
|
index++;
|
||||||
|
}
|
||||||
|
else if (arg == 2) {
|
||||||
|
argv2[index] = chr;
|
||||||
|
index++;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (millis() > nextPID) {
|
||||||
|
updatePID();
|
||||||
|
nextPID += PID_INTERVAL;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Check to see if we have exceeded the auto-stop interval
|
||||||
|
if ((millis() - lastMotorCommand) > AUTO_STOP_INTERVAL) {;
|
||||||
|
setMotorSpeeds(0, 0);
|
||||||
|
moving = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
16
src/libraries/Arduino/commands.h
Normal file
16
src/libraries/Arduino/commands.h
Normal file
@ -0,0 +1,16 @@
|
|||||||
|
#ifndef COMMANDS_H
|
||||||
|
#define COMMANDS_H
|
||||||
|
|
||||||
|
#define GET_BAUDRATE 'b'
|
||||||
|
#define READ_ENCODERS 'e'
|
||||||
|
#define MOTOR_SPEEDS 'm'
|
||||||
|
#define RESET_ENCODERS 'r'
|
||||||
|
#define UPDATE_PID_L 'L'
|
||||||
|
#define UPDATE_PID_R 'R'
|
||||||
|
#define DISP_PID_P 'z'
|
||||||
|
#define BATTERY_LVL 'B'
|
||||||
|
#define LEFT 0
|
||||||
|
#define RIGHT 1
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
81
src/libraries/Arduino/diff_controller.h
Normal file
81
src/libraries/Arduino/diff_controller.h
Normal file
@ -0,0 +1,81 @@
|
|||||||
|
|
||||||
|
typedef struct {
|
||||||
|
double TargetTicksPerFrame;
|
||||||
|
long Encoder;
|
||||||
|
long PrevEnc;
|
||||||
|
|
||||||
|
int PrevInput;
|
||||||
|
|
||||||
|
int ITerm;
|
||||||
|
int Kp = 500;
|
||||||
|
int Kd = 100;
|
||||||
|
int Ki = 0;
|
||||||
|
int Ko = 50;
|
||||||
|
|
||||||
|
long output;
|
||||||
|
|
||||||
|
}
|
||||||
|
SetPointInfo;
|
||||||
|
|
||||||
|
SetPointInfo leftPID, rightPID;
|
||||||
|
|
||||||
|
|
||||||
|
unsigned char moving = 0;
|
||||||
|
|
||||||
|
void resetPID(){
|
||||||
|
leftPID.TargetTicksPerFrame = 0.0;
|
||||||
|
leftPID.Encoder = readEncoder(LEFT);
|
||||||
|
leftPID.PrevEnc = leftPID.Encoder;
|
||||||
|
leftPID.output = 0;
|
||||||
|
leftPID.PrevInput = 0;
|
||||||
|
leftPID.ITerm = 0;
|
||||||
|
|
||||||
|
rightPID.TargetTicksPerFrame = 0.0;
|
||||||
|
rightPID.Encoder = readEncoder(RIGHT);
|
||||||
|
rightPID.PrevEnc = rightPID.Encoder;
|
||||||
|
rightPID.output = 0;
|
||||||
|
rightPID.PrevInput = 0;
|
||||||
|
rightPID.ITerm = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
void doPID(SetPointInfo * p) {
|
||||||
|
long Perror;
|
||||||
|
long output;
|
||||||
|
int input;
|
||||||
|
|
||||||
|
input = p->Encoder - p->PrevEnc;
|
||||||
|
Perror = p->TargetTicksPerFrame - input;
|
||||||
|
|
||||||
|
output = ((p->Kp) * Perror + (p->Kd) * (input - p->PrevInput) + p->ITerm) / (p->Ko);
|
||||||
|
p->PrevEnc = p->Encoder;
|
||||||
|
|
||||||
|
output += p->output;
|
||||||
|
|
||||||
|
if (output >= MAX_PWM)
|
||||||
|
output = MAX_PWM;
|
||||||
|
else if (output <= -MAX_PWM)
|
||||||
|
output = -MAX_PWM;
|
||||||
|
else
|
||||||
|
|
||||||
|
p->ITerm += (p->Ki) * Perror;
|
||||||
|
|
||||||
|
p->output = output;
|
||||||
|
p->PrevInput = input;
|
||||||
|
}
|
||||||
|
|
||||||
|
void updatePID() {
|
||||||
|
|
||||||
|
leftPID.Encoder = readEncoder(LEFT);
|
||||||
|
rightPID.Encoder = readEncoder(RIGHT);
|
||||||
|
|
||||||
|
if (!moving){
|
||||||
|
if (leftPID.PrevInput != 0 || rightPID.PrevInput != 0) resetPID();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
doPID(&leftPID);
|
||||||
|
doPID(&rightPID);
|
||||||
|
setMotorSpeeds(leftPID.output, rightPID.output);
|
||||||
|
}
|
||||||
|
|
10
src/libraries/Arduino/encoder_driver.h
Normal file
10
src/libraries/Arduino/encoder_driver.h
Normal file
@ -0,0 +1,10 @@
|
|||||||
|
#define LEFT_ENC_PIN_A PC4
|
||||||
|
#define LEFT_ENC_PIN_B PC5
|
||||||
|
|
||||||
|
#define RIGHT_ENC_PIN_A PD2
|
||||||
|
#define RIGHT_ENC_PIN_B PD3
|
||||||
|
|
||||||
|
long readEncoder(int i);
|
||||||
|
void resetEncoder(int i);
|
||||||
|
void resetEncoders();
|
||||||
|
|
43
src/libraries/Arduino/encoder_driver.ino
Normal file
43
src/libraries/Arduino/encoder_driver.ino
Normal file
@ -0,0 +1,43 @@
|
|||||||
|
volatile long left_enc_pos = 0L;
|
||||||
|
volatile long right_enc_pos = 0L;
|
||||||
|
static const int8_t ENC_STATES [] = {0,1,-1,0,-1,0,0,1,1,0,0,-1,0,-1,1,0};
|
||||||
|
|
||||||
|
ISR (PCINT1_vect){
|
||||||
|
static uint8_t enc_last=0;
|
||||||
|
|
||||||
|
enc_last <<=2;
|
||||||
|
enc_last |= (PINC & (3 << 4)) >> 4;
|
||||||
|
|
||||||
|
left_enc_pos += ENC_STATES[(enc_last & 0x0f)];
|
||||||
|
}
|
||||||
|
|
||||||
|
ISR (PCINT2_vect){
|
||||||
|
static uint8_t enc_last=0;
|
||||||
|
|
||||||
|
enc_last <<=2;
|
||||||
|
enc_last |= (PIND & (3 << 2)) >> 2;
|
||||||
|
|
||||||
|
right_enc_pos += ENC_STATES[(enc_last & 0x0f)];
|
||||||
|
}
|
||||||
|
|
||||||
|
long readEncoder(int i) {
|
||||||
|
if (i == LEFT) return left_enc_pos;
|
||||||
|
else return right_enc_pos;
|
||||||
|
}
|
||||||
|
|
||||||
|
void resetEncoder(int i) {
|
||||||
|
if (i == LEFT){
|
||||||
|
left_enc_pos=0L;
|
||||||
|
return;
|
||||||
|
} else {
|
||||||
|
right_enc_pos=0L;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void resetEncoders() {
|
||||||
|
resetEncoder(LEFT);
|
||||||
|
resetEncoder(RIGHT);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
10
src/libraries/Arduino/motor_driver.h
Normal file
10
src/libraries/Arduino/motor_driver.h
Normal file
@ -0,0 +1,10 @@
|
|||||||
|
#define RIGHT_MOTOR_BACKWARD 5
|
||||||
|
#define LEFT_MOTOR_BACKWARD 6
|
||||||
|
#define RIGHT_MOTOR_FORWARD 9
|
||||||
|
#define LEFT_MOTOR_FORWARD 10
|
||||||
|
#define RIGHT_MOTOR_ENABLE 12
|
||||||
|
#define LEFT_MOTOR_ENABLE 13
|
||||||
|
|
||||||
|
void initMotorController();
|
||||||
|
void setMotorSpeed(int i, int spd);
|
||||||
|
void setMotorSpeeds(int leftSpeed, int rightSpeed);
|
32
src/libraries/Arduino/motor_driver.ino
Normal file
32
src/libraries/Arduino/motor_driver.ino
Normal file
@ -0,0 +1,32 @@
|
|||||||
|
void initMotorController() {
|
||||||
|
digitalWrite(RIGHT_MOTOR_ENABLE, HIGH);
|
||||||
|
digitalWrite(LEFT_MOTOR_ENABLE, HIGH);
|
||||||
|
}
|
||||||
|
|
||||||
|
void setMotorSpeed(int i, int spd) {
|
||||||
|
unsigned char reverse = 0;
|
||||||
|
|
||||||
|
if (spd < 0)
|
||||||
|
{
|
||||||
|
spd = -spd;
|
||||||
|
reverse = 1;
|
||||||
|
}
|
||||||
|
if (spd > 255)
|
||||||
|
spd = 255;
|
||||||
|
|
||||||
|
if (i == LEFT) {
|
||||||
|
if(spd == 0) { digitalWrite(LEFT_MOTOR_FORWARD, 1); digitalWrite(LEFT_MOTOR_BACKWARD, 1); }
|
||||||
|
else if (reverse == 0) { analogWrite(LEFT_MOTOR_FORWARD, spd); analogWrite(LEFT_MOTOR_BACKWARD, 0); }
|
||||||
|
else if (reverse == 1) { analogWrite(LEFT_MOTOR_BACKWARD, spd); analogWrite(LEFT_MOTOR_FORWARD, 0); }
|
||||||
|
}
|
||||||
|
else if (i == RIGHT){
|
||||||
|
if(spd == 0) { digitalWrite(RIGHT_MOTOR_FORWARD, 1); digitalWrite(RIGHT_MOTOR_BACKWARD, 1); }
|
||||||
|
else if (reverse == 0) { analogWrite(RIGHT_MOTOR_FORWARD, spd); analogWrite(RIGHT_MOTOR_BACKWARD, 0); }
|
||||||
|
else if (reverse == 1) { analogWrite(RIGHT_MOTOR_BACKWARD, spd); analogWrite(RIGHT_MOTOR_FORWARD, 0); }
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void setMotorSpeeds(int leftSpeed, int rightSpeed) {
|
||||||
|
setMotorSpeed(LEFT, leftSpeed);
|
||||||
|
setMotorSpeed(RIGHT, rightSpeed);
|
||||||
|
}
|
Loading…
x
Reference in New Issue
Block a user