#!/usr/bin/python from Adafruit_PWM_Servo_Driver import PWM import time pwm = PWM(0x41) pwm.setPWMFreq(60) # Set frequency to 60 Hz pwm.setAllPWM(0, 0)
pwm.setPWMFreq(60) # Set frequency to 60 Hz #while (True): # Change speed of continuous servo on channel O #pwm.setPWM(3, 0, servoMin) #pwm.setAllPWM(0,servoMin) #time.sleep(1) #pwm.setPWM(3, 0, servoMax) #pwm.setAllPWM(0,servoMax) #time.sleep(1) def servoTest(channel): pwm.setPWM(channel, 0, servoMin) time.sleep(1) pwm.setPWM(channel, 0, servoMax) time.sleep(1) pwm.setPWM(channel, 0, (servoMax / 2)) time.sleep(1) pwm.setAllPWM(0, servoMax / 2) time.sleep(2) #testa motor 1-3 for x in range(1, 4): servoTest(x) #testa motor 4-6 for x in range(8, 11): servoTest(x)
from Adafruit_PWM_Servo_Driver import PWM import time pwm = PWM(0x40) pwm.setPWM(1, 0, 450) pwm.setPWM(8, 0, 150) time.sleep(3) pwm.setAllPWM(0, 300) #pwm.setPWM(1,0,300) #pwm.setPWM(8,0,300)
class Head(HeadLocal): GPPins = [ None, None, 3, 5, 7, 29, 31, 26, 24, 21, 19, 23, 32, 33, 8, 10, 36, 11, 12, 35, 38, 40, 15, 16, 18, 22, 37, 13 ] # max time of movement servo the whole amplitude SERVO_CYCLE = [0.2, 0.65, 0.75, 4.0] #0.65 # constant delay for servo control SERVO_COMMAND_PAUSE = 0.1 #sensors pins # sensor wires: YGBrBlW (7,8,25,G,24) Sensors = [24, 10] #motors pins MOTOR_IN1 = 9 MOTOR_IN2 = 25 MOTOR_IN3 = 18 MOTOR_IN4 = 23 LED = 7 STEPPER_PINS = [27, 22, 4, 17] def __init__(self, config=None): super(Head, self).__init__(config) GPIO.setmode(GPIO.BCM) GPIO.setwarnings(False) self.PINS = [ True, True, True, True, True, True, True, True, True, True, True, True, True, True, True, True, True, True, True, True, True, True, True, True, True, True, True, True, True, True ] self.Pins[4] = 'PWR' self.Pins[6] = 'PWR' self.Pins[self.GPPins[self.Sensors[0]]] = 'S-0R' self.Pins[14] = 'S-0R' self.Pins[self.GPPins[self.Sensors[1]]] = 'S-1L' self.Pins[20] = 'S-1L' """ Calibration of center: r = min + (max - min) * center_value / 5000 """ self.intervals = [[0, 150, 330], [1, 230, 545], [2, 173, 627], [1, 0, 1024]] self.pwm = PWM(0x40, debug=True) if self.pwm.Device: self.pwm.setPWMFreq(60) self.Pins[1] = 'I2C1-VCC' self.Pins[3] = 'I2C1-SDA' self.Pins[5] = 'I2C1-SCL' self.Pins[9] = 'I2C1-GND' # x = -213..446 y = -844..-242 #self.StaticCompass = HMC5883(1, 116.5, -543, 0) #self.StaticCompass = MPU6050(1, 0, 0, 0) # x = 11..601 y = -927..-387 #self.DynamicCompass = HMC5883(0, 306, -657, 0) #self.Stepper = Stepper(self, self.STEPPER_PINS) self.LeftMotor = DCMotor(self, [self.MOTOR_IN1, self.MOTOR_IN2]) self.Pins[self.GPPins[self.MOTOR_IN1]] = "M-IN1" self.Pins[self.GPPins[self.MOTOR_IN2]] = "M-IN2" self.RightMotor = DCMotor(self, [self.MOTOR_IN3, self.MOTOR_IN4]) self.Pins[self.GPPins[self.MOTOR_IN3]] = "M-IN3" self.Pins[self.GPPins[self.MOTOR_IN4]] = "M-IN4" self.ServoB = A116() self.Pins[8] = 'TX' self.Pins[10] = 'RX' self.Pins[2] = 'A-5V' self.Pins[17] = 'A-3.3V' self.Pins[30] = 'A-GND' self.Pins[34] = 'A-GND' #self.HeadDCMotor = DCMotor(self, self.STEPPER_PINS[0:2]) #self.HeadMotor = CompasMotor(self.HeadDCMotor, self.StaticCompass, self.DynamicCompass, 0) #self.Serial = serial.Serial('/dev/ttyAMA0', 9600, timeout = 1) #self.Serial.open() self.LOW(self.LED) self.Pins[self.GPPins[self.LED]] = 'LED' self.Pins[25] = 'LED' def Read(self, id): if self.PINS[id]: GPIO.setup(id, GPIO.IN, GPIO.PUD_UP) self.PINS[id] = False return GPIO.input(id) def Write(self, id, value): if self.PINS[id]: GPIO.setup(id, GPIO.OUT) self.PINS[id] = False GPIO.output(id, value) def HIGH(self, id): self.Write(id, GPIO.HIGH) def LOW(self, id): self.Write(id, GPIO.LOW) def Shutdown(self): if self.pwm.Device: self.pwm.setAllPWM(0, 0) self.ServoB.Close() GPIO.cleanup() # return time of the movement def SetServo(self, id, s): if id >= len(self.intervals): return 0 if s < self.MinS: s = self.MinS else: if s > self.MaxS: s = self.MaxS old_value = self.GetServo(id) super(Head, self).SetServo(id, s) pin = self.intervals[id][0] minServo = self.intervals[id][1] maxServo = self.intervals[id][2] value = int((s - self.MinS) * (maxServo - minServo) / (self.MaxS - self.MinS) + minServo) if id == 3: #self.HeadMotor.SetPosition(value) self.ServoB.ISetPosition(pin, value) else: if self.pwm.Device: self.pwm.setPWM(pin, 0, value) else: print "setPWM(%d,0,%d)" % (pin, value) #self.ExecuteCommand("P%d,%d" % (pin, value)) return abs(s - old_value) * self.SERVO_CYCLE[id] / ( self.MaxS - self.MinS) + self.SERVO_COMMAND_PAUSE def ReadSensors(self): result = [] for s in self.Sensors: result.append(self.Read(s)) return result def ReadCompass(self): #return {"static": {"a":self.StaticCompass.getAngle(), # "xyz": self.StaticCompass.getXYZ()}, # "dynamic": {"a": self.DynamicCompass.getAngle(), # "xyz": self.DynamicCompass.getXYZ()}, # "head": self.HeadMotor.GetRealPosition()} return {} def MoveForward(self): self.LeftMotor.Forward() self.RightMotor.Forward() def MoveBack(self): self.LeftMotor.Back() self.RightMotor.Back() def MoveRight(self): self.LeftMotor.Forward() self.RightMotor.Back() def MoveLeft(self): self.LeftMotor.Back() self.RightMotor.Forward() def MoveStop(self): self.LeftMotor.Stop() self.RightMotor.Stop() def SetLED(self, value): if value: self.HIGH(self.LED) else: self.LOW(self.LED) def ExecuteCommand(self, command): cmd = command[0] if cmd == 'R': pin = int(command[1:]) return self.Read(pin) if cmd == 'H': pin = int(command[1:]) return self.HIGH(pin) if cmd == 'L': pin = int(command[1:]) return self.LOW(pin) if cmd == 'S': if command[1] == 'L': return self.Stepper.Move(1) else: return self.Stepper.Move(-1) if cmd == 'D': self.Stepper.Delay = float(command[1:]) return self.Stepper.Delay if cmd == 'X': if command[1] == 'd': compas = self.DynamicCompass else: compas = self.StaticCompass num = int(command[2:]) result = [] for i in range(num): result.append(compas.getXYZ()) time.sleep(0.3) return result if cmd == 'C': if command[1] == 'd': compas = self.DynamicCompass else: compas = self.StaticCompass num = int(command[2:]) result = [] for i in range(num): result.append(compas.getAngle() * 180 / pi) time.sleep(0.3) return result if cmd == 'P': d = command[1:].split(":") channel = int(d[0]) value = int(d[1]) if self.pwm.Device: self.pwm.setPWM(self.intervals[channel][0], 0, value) else: print "no pwm" return (channel, value) if cmd == 'J': value = int(command[1:]) self.ServoB.ISetPosition(1, value) return value if cmd == 'M': d = command[1:].split(":") channel = int(d[0]) result = {"c": channel} if d[1] != "": self.intervals[channel][1] = int(d[1]) result["min"] = self.intervals[channel][1] if d[2] != "": self.intervals[channel][2] = int(d[2]) result["max"] = self.intervals[channel][2] return result if cmd == 'B': times = command[2:].split(":") sleep = float(times[0]) if len(times) == 1 or (len(times) > 1 and times[1].strip() == ""): if command[1] == 'L': self.HeadDCMotor.Forward() else: self.HeadDCMotor.Back() time.sleep(sleep) self.HeadDCMotor.Stop() return {"sleep": sleep} after = float(times[1]) measurements = [] currentTime = 0 startTime = datetime.datetime.now() measurements.append({ "static": self.StaticCompass.getAngle(), "dynamic": self.DynamicCompass.getAngle(), "time": currentTime }) if command[1] == 'L': self.HeadDCMotor.Forward() else: self.HeadDCMotor.Back() while currentTime < sleep: currentTime = (datetime.datetime.now() - startTime).total_seconds() measurements.append({ "static": self.StaticCompass.getAngle(), "dynamic": self.DynamicCompass.getAngle(), "time": currentTime }) self.HeadDCMotor.Stop() while currentTime < sleep + after: currentTime = (datetime.datetime.now() - startTime).total_seconds() measurements.append({ "static": self.StaticCompass.getAngle(), "dynamic": self.DynamicCompass.getAngle(), "time": currentTime }) return {"sleep": sleep, "measurements": measurements}
# Initialise the PWM device using the default address pwm = PWM(0x40) # Note if you'd like more debug output you can instead run: #pwm = PWM(0x40, debug=True) servoMin = 150 # Min pulse length out of 4096 servoMax = 600 # Max pulse length out of 4096 def setServoPulse(channel, pulse): pulseLength = 1000000 # 1,000,000 us per second pulseLength /= 60 # 60 Hz print "%d us per period" % pulseLength pulseLength /= 4096 # 12 bits of resolution print "%d us per bit" % pulseLength pulse *= 1000 pulse /= pulseLength pwm.setPWM(channel, 0, pulse) pwm.setPWMFreq(60) # Set frequency to 60 Hz while (True): # Change speed of continuous servo on channel O #pwm.setPWM(3, 0, servoMin) pwm.setAllPWM(0, servoMin) time.sleep(1) #pwm.setPWM(3, 0, servoMax) pwm.setAllPWM(0, servoMax) time.sleep(1)
def int_handler(signal, frame): pwm = PWM(0x40) pwm.setAllPWM(0, 0) GPIO.cleanup() sys.exit(0)
import os import sys sys.path.append('/home/pi/Desktop/Se Project 4/Adafruit_PWM_Servo_Driver') from Adafruit_PWM_Servo_Driver import PWM RED, GREEN, BLUE = 15, 1, 14 pw2 = PWM(0x60) pw2.setPWMFreq(500) pw2.setAllPWM(0, 0) pw2.setPWM(RED, 0, 60) os.system("echo !!!!RESTARTING!!!!") #os.system("sudo reboot")
#from Adafruit_PWM_Servo_Driver1.Adafruit_PWM_Servo_Driver import PWM import sys #sys.path.insert(0,'/home/pi/Desktop/Se Project 4/Adafruit_PWM_Servo_Driver') sys.path.append('/home/pi/Desktop/Se Project 4/Adafruit_PWM_Servo_Driver') from Adafruit_PWM_Servo_Driver import PWM import time import led_attribute ''' print(moduleTest.bool) moduleTest.bool = True print(moduleTest.bool) ''' RED, GREEN, BLUE = 15, 1, 14 pw = PWM(0x60) pw.setPWMFreq(400) pw.setAllPWM(0, 0) while (True): pw.setPWM(GREEN, 0, 4095) time.sleep(.5) pw.setAllPWM(0, 0) time.sleep(.1) pw.setPWMFreq(400) pw.setAllPWM(0, 0) #pw.setPWM(RED, 0,4095) #pw.setPWM(GREEN, 0,4095) #pw.setAllPWM(40,100) #pw.setPWMFreq(4) #pw.setPWM(14,0,600) '''
class WifiTruckController(object): # Initialises the controller with all gpio's and stuff def __init__(self, pwm=None, servoMin=0, servoNeutral=0, servoMax=0, servoOutputSpan=0): #Initialise the PWM device using the default address (Adafruit) self.pwm = PWM(0x40) self.pwm.setPWMFreq(50) self.servoMinSteering = 230 #Min pulse length out of 4096 self.servoNeutralSteering = 307 #Neutral -||- self.servoMaxSteering = 415 #Max -||- self.servoOutputSpanSteering = self.servoMaxSteering - self.servoMinSteering self.servoMinDCMotor = 230 #Min pulse length out of 4096 self.servoNeutralDCMotor = 315 #Neutrall -||- ? self.servoMaxDCMotor = 400 #Max -||- self.servoOutputSpanDCMotor = self.servoMaxDCMotor - self.servoMinDCMotor # Sets a the pwm value by input (percentage) def setServoPulse(self, channel, inputPercentage): # print("setServoPules - input:%f" % inputPercentage ) if (0.0 <= inputPercentage <= 1.0): if (channel == 0): pwmOutput = int( self.servoMinSteering + self.servoOutputSpanSteering * inputPercentage ) # starting at min add the input times the span (min + 165 * 1 == 100%) # print("setServoPulse - pwmOutput:%d" %pwmOutput) self.pwm.setPWM(channel, 0, pwmOutput) elif (channel == 1): pwmOutput = int( self.servoMinDCMotor + self.servoOutputSpanDCMotor * inputPercentage ) # starting at min add the input times the span (min + 165 * 1 == 100%) # print("setServoPulse - pwmOutput:%d" %pwmOutput) self.pwm.setPWM(channel, 0, pwmOutput) else: self.pwm.setPWM(channel, 0, 0) def stop(self): self.releaseMovement() self.releaseSteering() # print("WTC - STOP") def releaseSteering(self): self.setServoPulse(0, 0.5) time.sleep(0.5) self.pwm.setPWM(0, 0, 0) def releaseMovement(self): self.setServoPulse(1, 0.25) time.sleep(0.5) self.pwm.setPWM(1, 0, 0) def releaseAllServos(self): self.pwm.setAllPWM(0, 0) def move(self, percent): # Since the speed regulator doesn't map to the joystick divide by 2 # when reversing. The reverse is active in the 0-25% area if (percent < 0.35): percent = 0 elif (0.4 < percent < 0.5): percent = 0.25 elif (0.5 < percent < 0.6): percent *= 0.75 self.setServoPulse(1, percent) # print("WTC - MOVE") def steer(self, percent): self.setServoPulse(0, percent)
import os import sys sys.path.append('/home/pi/Desktop/Se Project 4/Adafruit_PWM_Servo_Driver') from Adafruit_PWM_Servo_Driver import PWM RED, GREEN, BLUE = 15,1,14 pw2 = PWM(0x60) pw2.setPWMFreq(500) pw2.setAllPWM(0,0) pw2.setPWM(RED, 0,60) os.system("echo !!!!RESTARTING!!!!") #os.system("sudo reboot")
class RotorDriver(object): def __init__(self, channels=[0], magnitude=0.5, period=4., runtime=60., comint=1e-2, periodMultiple=1): """ Constructor :type channels: list :type magnitude: float :type period: float :type runtime: int :type comint: float :param channels: which channels to control rotors on :param magnitude: % of maximum rotor power :param period: period of the sin wave, in seconds :param runtime: how long to run the rotors for, in seconds :param comint: desired refresh rate of commands to rotors """ #Parse the input parameters self.channels = channels self.mag = np.array([magnitude, magnitude]) self.per = period self.runtime = runtime self.comint = comint self.multiple = int(periodMultiple) #FINAL variables self.PWMFREQ = 250 #Frequency of PWM signal in Hz self.NUMTICK = 4096 #Resolution of signal (i.e. 4096 = 12 bit) self.ZEROSPD = [ 1577, 1573 ] #PWM-width, in microseconds, of 0 servo movement [Rotor 0 (arm without pressure sensor: dead range: [1551,1603]; Rotor 1: dead range: [1546,1599]] self.MAXSPD = [ int(round(1990 * self.ZEROSPD[x] / 1500)) for x in range(2) ] #PWM-width, in microseconds of maximum forward speed self.MINSPD = 1100 #PWM-width, in microseconds of maximum reverse speed self.SERVO_ADDRESS = 0x40 #Address of I2C servo in hex self.BIAS = 0 #Bias with which to adjust value inside cos self.MULT2A = -0.2056 self.MULT2B = -1.096 self.MULT2C = 0.3652 self.MULT3A = -0.2127 self.MULT3B = -1.134 self.MULT3C = 0.3781 #These coefficients are used to alter the pattern of rotor output #to force the apparatus to oscillate at periods greater than #the system's harmonic period self.COEFFICIENTS = [[1, 0, 0], [self.MULT2A, self.MULT2B, self.MULT2C], [self.MULT3A, self.MULT3B, self.MULT3C]] #Additional variables self.usPerTick = 1. / self.PWMFREQ * 1e6 / self.NUMTICK #microseconds per tick self.spdRange = [400, 400] self.spdRange[0] = max(self.MAXSPD[0] - self.ZEROSPD[0], self.ZEROSPD[0] - self.MINSPD) self.spdRange[1] = max(self.MAXSPD[1] - self.ZEROSPD[1], self.ZEROSPD[1] - self.MINSPD) if self.per == 0: self.freq = 0 self.BIAS = 0 else: self.freq = 2. * math.pi / self.per self.running = False self.flipper = 1. self.startTime = 0 #Initialize the rotors self.pwm = PWM(self.SERVO_ADDRESS) self.pwm.setPWMFreq(self.PWMFREQ) self.pwm.setAllPWM(0, math.floor(self.ZEROSPD[0] / self.usPerTick)) def daemonize(self): #Now daemonize the run function self.thread = threading.Thread(target=self.run, args=(), daemon=True) self.thread.start() #The threaded method that runs the rotors in the background def run(self): #Record the start time from which to determine when to stop self.startTime = time.perf_counter() self.running = True while self.running: #Determine new speed as a sin function tim = time.perf_counter() newSpeed = np.array(self.spdRange) * ( self.COEFFICIENTS[self.multiple - 1][0] * math.cos(self.freq * (tim - self.startTime) + self.BIAS) + self.COEFFICIENTS[self.multiple - 1][1] * math.cos(self.freq * (tim - self.startTime) + self.BIAS)**3 + self.COEFFICIENTS[self.multiple - 1][2] * math.cos(self.freq * (tim - self.startTime) + self.BIAS)**5) for channel in self.channels: #Direction of the rotor controlled by whether channel is even (-) or odd (+) self.pwm.setPWM( int(channel), 0, math.floor( (self.ZEROSPD[int(channel % 2)] + math.copysign(1, self.flipper) * math.copysign( 1, channel % 2 - 1) * newSpeed[int(channel % 2)] * float(self.mag[int(channel % 2)])) / self.usPerTick)) #self.pwm.setPWM(int(channel),0,math.floor((self.ZEROSPD[int(channel%2)]+math.copysign(1,self.flipper)*math.copysign(1,channel%2-1)*newSpeed[int(channel%2)])/self.usPerTick)) #If we come to the intended end of the run, stop the rotors and exit loop if ((time.perf_counter() - self.startTime) > self.runtime): self.pwm.setAllPWM( 0, math.floor(self.ZEROSPD[0] / self.usPerTick)) self.running = False time.sleep(self.comint) if (self.freq == 0): self.flipper = -self.flipper def stop(self): self.running = False self.pwm.setAllPWM(0, math.floor(self.ZEROSPD[0] / self.usPerTick)) def setPeriod(self, per): self.per = per if self.per == 0: self.freq = 0 self.BIAS = 0 else: self.freq = 2. * math.pi / self.per self.BIAS = 0 def setMultiple(self, mult): self.multiple = int(mult) def setMagnitude(self, mag): if (isinstance(mag, list) | isinstance(mag, tuple) | isinstance(mag, np.ndarray)): self.mag[0] = mag[0] self.mag[1] = mag[1] else: if np.max(self.mag) > 0: self.mag = np.array([ self.mag[0] / np.max(self.mag) * mag, self.mag[1] / np.max(self.mag) * mag ]) else: self.mag = np.array([mag, mag]) def setRuntime(self, tim): self.runtime = tim def setComint(self, com): self.comint = com def setChannels(self, chan): self.channels = chan self.pwm.setAllPWM(0, math.floor(0 / self.usPerTick)) def addChannels(self, chan): if isinstance(chan, list): self.channels += chan else: self.channels += [chan] self.pwm.setAllPWM(0, math.floor(0 / self.usPerTick)) def remChannels(self, chan): if not isinstance(chan, list): chan = [chan] self.channels = [x for x in self.channels if x not in chan] self.pwm.setAllPWM(0, math.floor(0 / self.usPerTick))