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