Fix-It Scripts
“Blinky” drive_motor_endpoint.py
This refers to a state that the Electronic Speed Controller gets stuck in a mode where it waits for calibration input from a remote controller. This is due to it being traditionally used in an RC setting. Since our vehicle does not utilize that funcionality, we wrote a script to send data directly to the ESC to knock it out of this calibration mode before launching our race modes.
1 #! /usr/bin/env python3.7
2 # -*- coding: utf-8 -*-
3 from adafruit_servokit import ServoKit
4 import numpy as np
5 import time
6
7 kit = ServoKit(channels=16)
8 command = ""
9 while command != "end":
10 command = input("enter speed:")
11 try:
12 kit.continuous_servo[2].throttle = float(command)
13 except:
14 command = "end"
15
16 kit.continuous_servo[2].throttle = 0
servo_and_drive_calbration.py
This code was used to initially interface with the servo and drive motor directly rather than through a ROS node. It was used to iterate through and identify values which we could use to properly set the steering servo PWM pulse width to make the full range 0-180 degrees match the full range of steering where 0 degrees is a full right turn and 180 degrees is a full left turn and 90 degrees is straight ahead. The drive motor is treated as a continuous servo with values ranging from -1,1 where 0 is full coast, -1 is full brake, and 1 is full throttle.
1 #! /usr/bin/env python3.7
2 # -*- coding: utf-8 -*-
3 from adafruit_servokit import ServoKit
4 import numpy as np
5 import time
6
7 kit = ServoKit(channels=16)
8
9 kit.servo[0].set_pulse_width_range(1400, 1825)
10
11 """
12 #Use this code to set pulse width min and max.
13 kit.servo[0].set_pulse_width_range(1000, 2000)
14 """
15
16 """
17 #Use this code to set drive motor throttle; 0 is stopped,
18 0.5 is half speed, 1 is full speed.
19 kit.continuous_servo[2].throttle = 0
20 """
21
22 """
23 #Use this code to set the steering servo's angle.
24 kit.servo[0].angle = 180
25 """
26
27 """
28 #Use this code to set the steering servo's actuation range.
29 kit.servo[0].actuation_range = 180
30 """
31
32 #Steering Constants
33 STEERING_MIN_ANGLE = 0
34 STEERING_MAX_ANGLE = 180
35 STEERING_STEP = 0.2
36 STEERING_STEP_LIST = np.arange(STEERING_MIN_ANGLE, STEERING_MAX_ANGLE, STEERING_STEP)
37 INVERSE_STEERING_STEP_LIST = np.arange(STEERING_MAX_ANGLE, STEERING_MIN_ANGLE, STEERING_STEP)
38
39 #Drive Constants
40 DRIVE_THROTTLE_MIN = 0
41 DRIVE_THROTTLE_MAX = 0.05
42 DRIVE_STEP = 0.001
43 DRIVE_STEP_LIST = np.arange(DRIVE_THROTTLE_MIN, DRIVE_THROTTLE_MAX, DRIVE_STEP)
44
45
46 #Sweep the steering servo within its entire range.
47 for step in STEERING_STEP_LIST:
48 kit.servo[0].angle = step
49 print(step)
50 time.sleep(.01)
51
52 time.sleep(5)
53
54 #Slowly increase throttle on drive motor within its entire range.
55 for step in DRIVE_STEP_LIST:
56 kit.continuous_servo[2].throttle = step
57 time.sleep(.1)
58 print(step)
59
60
61
62 kit.continuous_servo[2].throttle = 0
63
64 kit.servo[0].angle = 90
real_time_hall_effect.py
This code is the real-time graphing of the suspension sensor data using matplotlib’s animate function.
1 #!/usr/bin/env python3.7
2 import matplotlib.pyplot as plt
3 import numpy as np
4 import time
5 import adafruit_ads1x15.ads1115 as ADS
6 import board
7 import busio
8 import os
9 i2c = busio.I2C(board.SCL_1, board.SDA_1)
10 import sys
11 sys.path.append('../')
12 import time
13 from adafruit_ads1x15.analog_in import AnalogIn
14 import matplotlib.animation as animation
15 from matplotlib.ticker import FuncFormatter
16 import datetime as dt
17
18 def init():
19 line.set_ydata([np.nan] * len(x))
20 return line,
21
22 # This function is called periodically from FuncAnimation
23 def animate(i, xs0, ys0, xs1, ys1, xs2, ys2, xs3, ys3):
24
25 # Read voltage from i2c_bus
26 voltage_0 = hall_0.voltage
27 voltage_1 = hall_1.voltage
28 voltage_2 = hall_2.voltage
29 voltage_3 = hall_3.voltage
30
31 # Add x and y to lists
32 xs0.append(dt.datetime.now().strftime('%H:%M:%S.%f'))
33 ys0.append(voltage_0)
34
35 xs1.append(dt.datetime.now().strftime('%H:%M:%S.%f'))
36 ys1.append(voltage_1)
37
38 xs2.append(dt.datetime.now().strftime('%H:%M:%S.%f'))
39 ys2.append(voltage_2)
40
41 xs3.append(dt.datetime.now().strftime('%H:%M:%S.%f'))
42 ys3.append(voltage_3)
43
44 # Limit x and y lists to 20 items
45 xs0 = xs0[-20:]
46 ys0 = ys0[-20:]
47
48 xs1 = xs1[-20:]
49 ys1 = ys1[-20:]
50
51 xs2 = xs2[-20:]
52 ys2 = ys2[-20:]
53
54 xs3 = xs3[-20:]
55 ys3 = ys3[-20:]
56
57 # Draw x and y lists
58 ax[0][0].clear()
59 ax[0][0].plot(xs3, ys3, 'g')
60 ax[0][0].set_title("Front Left Suspension Sensor")
61 ax[0][0].tick_params(labelrotation=45)
62
63
64 ax[0][1].clear()
65 ax[0][1].plot(xs2, ys2, 'k')
66 ax[0][1].set_title("Front Right Suspension Sensor")
67 ax[0][1].tick_params(labelrotation=45)
68
69 ax[1][0].clear()
70 ax[1][0].plot(xs1, ys1, 'orange')
71 ax[1][0].set_title("Rear Left Suspension Sensor")
72 ax[1][0].tick_params(labelrotation=45)
73
74 ax[1][1].clear()
75 ax[1][1].plot(xs0, ys0, 'm')
76 ax[1][1].set_title("Rear Right Suspension Sensor")
77 ax[1][1].tick_params(labelrotation=45)
78
79
80 # Format plot
81 plt.subplots_adjust(bottom=0.30)
82
83 for a in ax.flat:
84 a.set(xlabel='Time', ylabel='Voltage')
85 plt.tight_layout()
86
87
88 sys.path.append(os.path.dirname(os.path.dirname(os.path.realpath(__file__))))
89 ADS1115_REG_CONFIG_PGA_6_144V = 0x00 # 6.144V range = Gain 2/3
90 ADS1115_REG_CONFIG_PGA_4_096V = 0x02 # 4.096V range = Gain 1
91 ADS1115_REG_CONFIG_PGA_2_048V = 0x04 # 2.048V range = Gain 2 (default)
92 ADS1115_REG_CONFIG_PGA_1_024V = 0x06 # 1.024V range = Gain 4
93 ADS1115_REG_CONFIG_PGA_0_512V = 0x08 # 0.512V range = Gain 8
94 ADS1115_REG_CONFIG_PGA_0_256V = 0x0A # 0.256V range = Gain 16
95 ads1115 = ADS.ADS1115(i2c)
96
97 hall_0 = AnalogIn(ads1115, ADS.P0)
98 hall_1 = AnalogIn(ads1115, ADS.P1)
99 hall_2 = AnalogIn(ads1115, ADS.P2)
100 hall_3 = AnalogIn(ads1115, ADS.P3)
101
102 # set x axis for time
103 hall_0_list = []
104 hall_1_list = []
105 hall_2_list = []
106 hall_3_list = []
107 time0_list = []
108 time1_list = []
109 time2_list = []
110 time3_list = []
111 # Create figure for plotting
112 fig, ax = plt.subplots(2,2)
113 xs0 = []
114 ys0 = []
115 xs1 = []
116 ys1 = []
117 xs2 = []
118 ys2 = []
119 xs3 = []
120 ys3 = []
121
122 while True:
123 #Get the Digital Value of Analog of selected channel
124 #print(hall0.value, hall0.voltage)
125 hall_0_list.append(hall_0.voltage)
126 time0_list.append(time.time())
127 time.sleep(0.02)
128 hall_1_list.append(hall_1.voltage)
129 time1_list.append(time.time())
130 time.sleep(0.02)
131 hall_2_list.append(hall_2.voltage)
132 time2_list.append(time.time())
133 time.sleep(0.02)
134 hall_3_list.append(hall_3.voltage)
135 time3_list.append(time.time())
136 #print("A0:%dmV A1:%dmV A2:%dmV A3:%dmV"%(adc0_list[i],adc1_list[i],adc2_list[i],adc3_list[i]))
137
138
139
140
141 # Set up plot to call animate() function periodically
142 ani = animation.FuncAnimation(fig, animate, fargs=(xs0, ys0, xs1, ys1, xs2, ys2, xs3, ys3), interval=100)
143 #ani_1 = animation.FuncAnimation(fig, animate, fargs=(xs1, ys1), interval=1000)
144 #ani_2 = animation.FuncAnimation(fig, animate, fargs=(xs2, ys2), interval=1000)
145 #ani_3 = animation.FuncAnimation(fig, animate, fargs=(xs3, ys3), interval=1000)
146 plt.tight_layout()
147 plt.xticks(rotation=45, ha='right')
148 plt.show()
real_time_IMU.py
This code is the self-written driver code for accessing data stored on the ADIS16470 IMU’s registers. Note: The baud rate is likely incorrect as we could not readily find the proper one to use.
1 #!/usr/bin/env python3.7
2 import board
3 import busio
4 import digitalio
5 import array
6 import numpy as np
7 from adafruit_bus_device.spi_device import SPIDevice
8 import struct
9 import matplotlib.pyplot as plt
10 import matplotlib.animation as animation
11 from matplotlib.ticker import FuncFormatter
12 import datetime as dt
13 import time
14
15 # This function is called periodically from FuncAnimation
16 def animate(i, xs, ys):
17
18 # Add x and y to lists
19 xs.append(dt.datetime.now().strftime('%H:%M:%S.%f'))
20 ys.append(x_accel)
21
22 # Limit x and y lists to 20 items
23 xs = xs[-20:]
24 ys = ys[-20:]
25
26 # Draw x and y lists
27 ax.clear()
28 ax.plot(xs, ys)
29
30 # Format plot
31 plt.xticks(rotation=45, ha='right')
32 plt.subplots_adjust(bottom=0.30)
33 plt.title('Acceleration Over Time')
34 plt.ylabel("m/(s^2)")
35 return
36
37
38 #Constants
39 X_GYRO_LOW = bytearray([0x04,0x05])
40 X_GYRO_OUT = bytearray([0x06, 0x07])
41 Y_GYRO_LOW = bytearray([0x08,0x09])
42 Y_GYRO_OUT = bytearray([0x0A, 0x0B])
43 Z_GYRO_LOW = bytearray([0x0C,0x0D])
44 Z_GYRO_OUT = bytearray([0x0E, 0x0F])
45 X_ACCEL_LOW = bytearray([0x10,0x11])
46 X_ACCEL_OUT = bytearray([0x12, 0x13])
47 Y_ACCEL_LOW = bytearray([0x14,0x15])
48 Y_ACCEL_OUT = bytearray([0x16, 0x17])
49 Z_ACCEL_LOW = bytearray([0x18,0x19])
50 Z_ACCEL_OUT = bytearray([0x1A, 0x1B])
51 TEMP_OUT = bytearray([0x1C, 0x1D])
52 TIME_STAMP = bytearray([0x1E, 0x1F])
53
54
55 #Setup spi bus
56 spi = busio.SPI(board.SCLK, MISO=board.MISO, MOSI = board.MOSI)
57 #Setup Chip Select
58 cs = digitalio.DigitalInOut(board.CE0_1)
59
60 #Create an instance of the SPIDevice class
61 device = SPIDevice(spi, cs, baudrate=4000, polarity=0, phase=0)
62
63
64 def spi_request_float32(request_low, request_out):
65 result_low = bytearray(2)
66 result_out = bytearray(2)
67 result = bytearray(4)
68 with device as spi:
69 spi.write_readinto(request_low, result_low)
70 spi.write_readinto(request_out, result_out)
71 result_out.extend(result_low)
72 result_float = struct.unpack('f', result_out)
73
74 return result_float[0]
75
76 def spi_request_decimal(request):
77 result = bytearray(2)
78 with device as spi:
79 spi.write_readinto(request, result)
80
81 result_decimal = int.from_bytes(result, byteorder='big', signed=True)
82
83 return result_decimal
84 time_stamp = 0
85 # Create figure for plotting
86 fig = plt.figure()
87 ax = fig.add_subplot(2, 2, 1)
88 xs = []
89 ys = []
90 x_accel_list = []
91 x_gyro_list = []
92 time0_list = []
93 while True:
94 x_accel = spi_request_float32(X_ACCEL_LOW, X_ACCEL_OUT)
95 y_accel = spi_request_float32(Y_ACCEL_LOW, Y_ACCEL_OUT)
96 z_accel = spi_request_float32(Z_ACCEL_LOW, Z_ACCEL_OUT)
97
98 x_gyro = spi_request_float32(X_GYRO_LOW, X_GYRO_OUT)
99 y_gyro = spi_request_float32(Y_GYRO_LOW, Y_GYRO_OUT)
100 z_gyro = spi_request_float32(Z_GYRO_LOW, Z_GYRO_OUT)
101
102 temp = spi_request_decimal(TEMP_OUT)/10
103 time_stamp += spi_request_decimal(TIME_STAMP)
104 print("x_acceleration: " + str(x_accel))
105 print("x_gyro: " + str(x_gyro))
106 print("Temperature: " + str(temp) + "\n")
lidar_ethernet_setup.py
This code configure’s the Jetson Nano’s Ethernet port for the Lidar data rather than being used for network information.
1 #! /usr/bin/env python3.7
2 # -*- coding: utf-8 -*-
3
4 import os
5 import rospy
6
7 os.system("sudo ip addr add 192.168.0.15/24 broadcast 192.168.0.255 dev eth0")
8 os.system("rosrun urg_node urg_node _ip_address:=192.168.0.10")
9
10 print("LIDAR Ethernet port has been configured")