def __init__(self):
global k, pd, pid, comb_pid, max_pwm, max_theta, max_x, frequency, pwm_offset, arduino_port, display_camera_output, initialize_theta_set
#Cart variables
self.max_pwm = max_pwm
self.max_theta = max_theta
self.max_x = max_x
self.frequency = frequency
self.pwm_offset = pwm_offset
self.arduino_port = arduino_port
#Booleans
self.display_camera_output = display_camera_output
self.initialize_theta_set = initialize_theta_set
#Motor, Controller, and Camera Instance Definition
self.motors = Motors.Motors(max_pwm = self.max_pwm, frequency=self.frequency, arduino_port=self.arduino_port)
self.controller = Controller.Controller(max_theta = self.max_theta, max_x = self.max_x)
self.camera = Camera.Camera(self.display_camera_output)
#Controller Vectors
self.k = [-10.0000/1.5, -29.9836/1.5, 822.2578, 85.5362]
self.pd = pd
self.pid = pid
self.comb_pid = comb_pid
self.status = 1
def __init__(self):
"""
:type maxParts: NumberOfPart[]
:type currParts: NumberOfPart[]
:type location: Location
"""
self.location = Location.Location()
self.maxParts = []
self.currParts = []
self.motors = Motors.Motors()
self.name = 0
def main():
try:
dataLog = DataLog.DataLog(logDir = 'logs/')
dataLog.updateLog({'velocity_units':velocityUnits})
encoders = Encoders.Encoders(countsPerRevolution = countsPerRevolution, units = velocityUnits)
#time.sleep(2) #allow encoder time to attach
motors = Motors.Motors(i2c_bus_num,nmotors,min_throttle_percentage, max_throttle_percentage, min_throttle_pulse_width, max_throttle_pulse_width)
#start motor communication
motors.setPWMfreq(PWM_frequency)
motors.motor_startup()
for i in xrange(3):
motors.setPWM(i, 0)
freq = 1/2.0
dataLog.updateLog({'start_time': time.time()})
while True:
#get measured velocity array
#calculate commanded velocities array
#convert commanded velocities to throttle
#send commanded PWM signal to motors
loop_start_time = time.time()
count_array = encoders.returnCountArray()
measured_velocities = encoders.getVelocities()
print measured_velocities
command_time = time.time()
commanded_throttles = [wave(100,2*pi/nmotors*pwmNum, freq, command_time) for pwmNum in xrange(nmotors)]
for pwmNum, throttle in enumerate(commanded_throttles):
motors.setPWM(pwmNum,throttle)
loop_end_time = time.time()
#write information to logs
log_info = {
'counts': dict(zip(['time', 0, 1, 2], count_array)),
'commanded_velocity': dict(zip(['time', 0, 1, 2],[command_time] + map(actuatorVelocityModel, commanded_throttles))),
'commanded_throttle': dict(zip(['time', 0, 1, 2],[command_time] + commanded_throttles)),
'measured_velocity': dict(zip(['time', 0, 1, 2], measured_velocities)),
'iteration_latency': loop_end_time - loop_start_time,
'command_latency': loop_end_time - command_time,
'measurement_to_command_latency': command_time - measured_velocities[0]
}
dataLog.updateLog(log_info)
except KeyboardInterrupt:
for i in xrange(3):
motors.setPWM(i, 0)
dataLog.saveLog(baseName = 'Closed_Loop_Test')
def testClawOpenWidth():
testMotors = Motors.Motors()
testMotors.openClawWidth(testMotors.CWL.getWidth(5))
assert testMotors.CWL.getWidth(5) == 40
import CmdMessenger
import CmdManager
import Motors
import Gripper
import Led
import SpeedControl
if __name__ == '__main__':
cMes = CmdMessenger.CmdMessenger()
cMes.start()
cMan = CmdManager.CmdManager(cMes)
cMotor = Motors.Motors(cMes, 5)
cMotor.start()
cSpeed = SpeedControl.SpeedControl(cMotor)
gripper = Gripper.Gripper(cMes)
led = Led.Led(cMes)
import TrayDB
import PartDatabase
import Motors
import UsSensor
import Jetson.GPIO
import time
window = tk.Tk()
filename = "ColorFiles/Default.png"
resulting_color = "light grey"
#load = Image.open(filename)
#render = ImageTk.PhotoImage(load)
#gRender = ""
myFont = font.Font(family='Helvetica', size=20)
CGPDb = PartDatabase.PartDatabase()
CGMotors = Motors.Motors()
CGUsSensor = UsSensor.UsSensor()
# going with 5 trays as default since that is the max the robot can hold
CGTDb = TrayDB.TrayDB(5)
#resets the robot position between GUI launches
CGMotors.MotorGoTo(CGMotors.PORTY, -1000)
CGMotors.MotorGoTo(CGMotors.PORTX,-1000)
# Gives part name if it exists, and if not, returns "Part not valid"
def getLabelPartname():
partColor = str(entry_color.get())
partShape = str(entry_shape.get())
if CGPDb.checkIfPart(partColor, partShape):
label_partname.configure(text="The part name is: " + str(CGPDb.returnPart(partColor, partShape)),
import Motors import time LEFT_TRIM = 0 RIGHT_TRIM = 0 robot = Motors.Motors(left_trim=LEFT_TRIM, right_trim=RIGHT_TRIM) # Now move the robot around! # Each call below takes two parameters: # - speed: The speed of the movement, a value from 0-255. The higher the value # the faster the movement. You need to start with a value around 100 # to get enough torque to move the robot. # - time (seconds): Amount of time to perform the movement. After moving for # this amount of seconds the robot will stop. This parameter # is optional and if not specified the robot will start moving # forever. robot.right(160) # Spin left at speed 200 for 0.5 seconds. # Spin in place slowly for a few seconds. #robot.right(100) # No time is specified so the robot will start spinning forever. time.sleep(10.0) # Pause for a few seconds while the robot spins (you could do # other processing here though!). robot.stop() # Stop the robot from moving. # Now move backwards and spin right a few times. #robot.right(200, 0.5)
import Motors
import math
import time
i2c_bus_num = 1
nmotors = 3
min_throttle_percentage = -100
max_throttle_percentage = 100
min_throttle_pulse_width = 1100 #in microseconds
max_throttle_pulse_width = 1940 #in microseconds
PWM_frequency = 500 #in Hz
#instantiate motor group object
motors = Motors.Motors(i2c_bus_num,nmotors,min_throttle_percentage, max_throttle_percentage, min_throttle_pulse_width, max_throttle_pulse_width)
def wave(amp, phi, f, t):
return amp*math.sin(f*t + phi)
if __name__ == "__main__":
try:
motors.setPWMfreq(PWM_frequency)
print 'PWM frequency before setting PWM: {0:.02f} Hz'.format(motors.PWM_frequency)
motors.motor_startup()
for pwmNum in xrange(nmotors):
print 'PWM Num = {0}'.format(pwmNum)
motors.setPWM(pwmNum,50)
while True:
time.sleep(1)
#drive motors in wave
