#!/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))
