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")