def __init__(self):
rospy.init_node('actuators_handler')
rospy.loginfo(rospy.get_caller_id() +
'Initializing actuators_handler node')
#Get all parameters from config (rosparam)
name = 'engine'
engine_output_pin = int(
rospy.get_param('actuators/' + name + '/output_pin', 1))
engine_board_pin = int(
rospy.get_param('actuators/' + name + '/board_pin', 60))
engine_period_us = int(
1e6 /
float(rospy.get_param('actuators/' + name + '/frequency', 60)))
name = 'steering'
steering_output_pin = int(
rospy.get_param('actuators/' + name + '/output_pin', 1))
steering_board_pin = int(
rospy.get_param('actuators/' + name + '/board_pin', 62))
steering_period_us = int(
1e6 /
float(rospy.get_param('actuators/' + name + '/frequency', 60)))
#Initialize PWM
self.dev1 = mraa.Pwm(engine_board_pin)
self.dev1.period_us(engine_period_us)
self.dev1.enable(True)
self.dev1.pulsewidth_us(1500)
self.dev2 = mraa.Pwm(steering_board_pin)
self.dev2.period_us(steering_period_us)
self.dev2.enable(True)
self.dev2.pulsewidth_us(1500)
def FullControl(mosq, obj, msg): #Mqtt execute command
print "MQTT dataMessageHandler %s %s" % (msg.topic, msg.payload)
if (msg.payload=="off" or msg.payload=="Off" or msg.payload=="OFF"):
x = mraa.Pwm(3)
x.enable(False)
x.write(0.0)
#SyncSignal(0.0)
#return
elif (msg.payload=="on" or msg.payload=="ON" or msg.payload=="On"):
x = mraa.Pwm(3)
x.enable(True)
SyncSignal(1.0)
#return
elif (msg.payload=="%"):
PtValue = msg.payload[1:2]
PwValue = PtValue/100
SyncSignal(PwValue)
#return
elif (msg.payload=="break" or msg.payload=="Break"):
print "Program terminated by remote user..."
SyncSignal(0)
os._exit(1)
else:
print "write only 'on' or 'off' or break to terminate..."
def __init__(self, pin_red, pin_blue, pin_green):
"""
Constructor
:param pin_red: GPIO Pin Number of Red LED
:param pin_blue: GPIO Pin Number of Blue LED
:param pin_green: GPIO Pin Number of Green LED
"""
self.power = 0
self.pwm_red = mraa.Pwm(pin_red)
self.pwm_green = mraa.Pwm(pin_green)
self.pwm_blue = mraa.Pwm(pin_blue)
# Enables the pin's
self.pwm_red.enable(True)
self.pwm_green.enable(True)
self.pwm_blue.enable(True)
# self.processedSamplesQueue = processedSamplesQueue
# self.processedSamplesQueueLock = processedSamplesQueueLock
# STATE CONSTANTS
self.CONST_STATE_GREEN = "green"
self.CONST_STATE_YELLOW = "yellow"
self.CONST_STATE_RED = "red"
# Leds State
self.current_state = self.CONST_STATE_GREEN
self.past_state = self.CONST_STATE_GREEN
# The color min value
self.CONST_GRADIENT = 0.10
# Starts with the led in green
self.pin_red = 0.0000
self.pin_blue = 0.0000
self.pin_green = 0.0000
"""self.led(self.pinGreen,self.pinRed)
""" # Data that is receive by the thread
self.power_buffer = []
self.led_semaphore_controller = threading.Semaphore(
value=0) # This will controll the led reads on the list that has the current values
def __init__(self):
# Important variables.
self.threads = {}
self.running = False
self.sensor_value = 0
self.seconds = 0
self.mode = 1
self.pwm_dryer = 0
# Configure the digital output for the LED 3.
self.led_system = mraa.Gpio(LED_SYSTEM_PIN)
self.led_system.dir(mraa.DIR_OUT)
# Configures the PWM generators (LEDs and dryer).
self.led_sensor = mraa.Pwm(LED_SENSOR_PWM_PIN)
self.led_sensor.period_ms(5)
self.led_dryer = mraa.Pwm(LED_DRYER_PWM_PIN)
self.led_dryer.period_ms(5)
# Configures the Hi-Z button, with pull-up and falling edge.
self.button = mraa.Gpio(BUTTON_PULLUP_PIN)
self.button.dir(mraa.DIR_IN)
self.button.mode(mraa.MODE_PULLUP)
self.button.isr(mraa.EDGE_FALLING, button_pressed, self.button)
# Configures the ADC.
self.adc = mraa.Aio(SENSOR_ADC_PIN)
# Configures the socket.
self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
# Starts the Network.
self.network_start()
def __init__(self):
#ser.Serial('/dev/ttyO2', 57600)
#ser = serial.Serial('/dev/ttyO2', 57600)
#self.dev = serial.Serial('/dev/ttyO2', 57600)
self.dev = serial.Serial('/dev/ttyO2', 115200)
rospy.init_node('remote_reading_handler');
#Get all parameters from config (rosparam)
name = 'engine'
engine_output_pin = int(rospy.get_param('actuators/' + name + '/output_pin', 1))
engine_board_pin = int(rospy.get_param('actuators/' + name + '/board_pin', 60))
engine_period_us = int(1e6 / float(rospy.get_param('actuators/' + name + '/frequency', 60)))
name = 'steering'
steering_output_pin = int(rospy.get_param('actuators/' + name + '/output_pin', 1))
steering_board_pin = int(rospy.get_param('actuators/' + name + '/board_pin', 62))
steering_period_us = int(1e6 / float(rospy.get_param('actuators/' + name + '/frequency', 60)))
#Initialize PWM
self.dev1 = mraa.Pwm(engine_board_pin)
self.dev1.period_us(engine_period_us)
self.dev1.enable(True)
self.dev1.pulsewidth_us(1500)
self.dev2 = mraa.Pwm(steering_board_pin)
self.dev2.period_us(steering_period_us)
self.dev2.enable(True)
self.dev2.pulsewidth_us(1500)
def __init__(self):
self.led1 = m.Pwm(22)
self.led2 = m.Pwm(24)
self.led1.period_us(70)
self.led2.period_us(70)
self.led1.enable(True)
self.led2.enable(True)
def pwmMotor():
global systemOn
global curve
global valueMotor
global shutdown
global curve
global valueSensor
motor = mraa.Pwm(11)
motor.period_us(5000)
motor.enable(True)
valueMotor = 0.0
oldValueMotor = 0.0
motor.write(valueMotor)
motorLed = mraa.Pwm(5)
motorLed.period_us(5000)
motorLed.enable(True)
while not shutdown:
if (systemOn):
if (curve == 1):
pond = (1024 - valueSensor) / 1024.0 + 0.3
if (timestamp >= 0 and timestamp < 30):
valueMotor = pond * (0.3 * (timestamp / 30.0))
elif (timestamp >= 30 and timestamp < 60):
valueMotor = pond * 0.3
elif (timestamp >= 60 and timestamp < 90):
valueMotor = pond * (0.3 + (0.45 *
((timestamp - 60) / 30.0)))
elif (timestamp >= 90 and timestamp <= 120):
valueMotor = pond * 0.75
elif (timestamp >= 120 and timestamp < 180):
valueMotor = pond * (0.75 - ((timestamp - 120) *
(0.75 / 60.0)))
else:
#valueMotor = 0.0
systemOn = False
elif (curve == 2):
pond = (1024 - valueSensor) / 1024.0 + 0.3
if (timestamp >= 0 and timestamp < 180):
valueMotor = pond * (
0.5 + 0.5 * math.sin(timestamp * 6.28 / 180.0))
else:
systemOn = False
if oldValueMotor != valueMotor:
motor.write(valueMotor)
motorLed.write(valueMotor)
oldValueMotor = valueMotor
else:
motor.write(0.0)
motorLed.write(0.0)
motor.write(0.0)
motorLed.write(0.0)
def __init__(self):
self.pwm_ina = mraa.Pwm(0)
self.pwm_inb = mraa.Pwm(14)
self.pin_stby = mraa.Gpio(15) # 0 = motor off
self.pin_ina1 = mraa.Gpio(45) # 0/1 -> go forward/backward
self.pin_ina2 = mraa.Gpio(46)
self.pin_inb1 = mraa.Gpio(47) # 0/1 -> go left/right
self.pin_inb2 = mraa.Gpio(48)
self.period = 1
def __init__(self):
#initialize node
rospy.init_node('RoboCapeController')
rospy.loginfo(rospy.get_caller_id() +
'Initializing RoboCapeController node')
#initialize serial communication (with remote control)
self.dev = serial.Serial('/dev/ttyO2', 115200)
#initialize global variables
self.Roll = 0
self.RollRate = 0
self.ThrottleREMOTE = 0
self.SteeringREMOTE = 0
self.SteeringCONTROLdeg = 0
self.SteeringCONTROLpulse = 0
self.Kp = -0.654276982979989
self.Ki = -2.28191654162741
self.Kd = -0.0211479278661121
#initialize PID controller
self.pid = PID(self.Kp, self.Ki, self.Kd)
self.pid.SetPoint = 0.0
self.pid.setSampleTime(1 / 30)
#initialize actuators
#Get all parameters from config (rosparam)
name = 'engine'
engine_output_pin = int(
rospy.get_param('actuators/' + name + '/output_pin', 1))
engine_board_pin = int(
rospy.get_param('actuators/' + name + '/board_pin', 60))
engine_period_us = int(
1e6 /
float(rospy.get_param('actuators/' + name + '/frequency', 60)))
name = 'steering'
steering_output_pin = int(
rospy.get_param('actuators/' + name + '/output_pin', 1))
steering_board_pin = int(
rospy.get_param('actuators/' + name + '/board_pin', 62))
steering_period_us = int(
1e6 /
float(rospy.get_param('actuators/' + name + '/frequency', 60)))
#Initialize PWM
self.dev1 = mraa.Pwm(engine_board_pin)
self.dev1.period_us(engine_period_us)
self.dev1.enable(True)
self.dev1.pulsewidth_us(1500)
self.dev2 = mraa.Pwm(steering_board_pin)
self.dev2.period_us(steering_period_us)
self.dev2.enable(True)
self.dev2.pulsewidth_us(1500)
def __init__(self, fwd_pin, rev_pin):
self.fwd = mraa.Pwm(fwd_pin)
self.rev = mraa.Pwm(rev_pin)
self.fwd.write(0.0)
self.rev.write(0.0)
self.fwd.enable(False)
self.rev.enable(False)
self.fwd.period_us(CFG.PWM_PERIOD_US)
self.rev.period_us(CFG.PWM_PERIOD_US)
def __init__(self):
self.horizontal_pwm = mraa.Pwm(5)
self.horizontal_pwm.period_ms(20)
self.horizontal_pwm.enable(True)
self.vertical_pwm = mraa.Pwm(6)
self.vertical_pwm.period_ms(20)
self.vertical_pwm.enable(True)
self.last_x = 90
self.last_y = 45
self.placeServo(90, 45)
def __init__(self):
"""
Initialize the MRAA library pins.
"""
# Declare MRAA Lib Pins
self._pin_drive_motor_pwm = mraa.Pwm(0) # drive_motor_pwm pin
self._pin_steering_motor_pwm = mraa.Pwm(14) # steering_motor_pwm
self._drive_motor_pins = (mraa.Gpio(45), mraa.Gpio(46)
) # Direction Forward and Backward
self._steering_motor_pins = (mraa.Gpio(47), mraa.Gpio(48)
) # Direction Left and Right
self._pin_standby = mraa.Gpio(15)
self.setup()
def initPWMs(): #Import Globals global main_pwm global jib_pwm global rudder_pwm main_pwm = mraa.Pwm(main_winch_servo_pin) jib_pwm = mraa.Pwm(jib_winch_servo_pin) rudder_pwm = mraa.Pwm(rudder_servo_pin); main_pwm.period_us(int(winch_period)) jib_pwm.period_us(int(winch_period)) rudder_pwm.period_us(int(rudder_period)) main_pwm.enable(True) jib_pwm.enable(True) rudder_pwm.enable(True) rotate(RotateMock())
def attach(self, pin):
"""
Attaches a servo motor to a PWM pin.
Parameters:
pin: pin where the servo motor will be attached. Currently only pins 3, 5, 6, and 9 are supported.
"""
if pin in [3, 5, 6, 9]:
self._pinAttached = pin
else:
while True: # Keep asking for a valid pin.
print "The pin '" + str(pin) + "' is not a valid input. The valid pins are 3, 5, 6 or 9."
pin = raw_input("Select a valid pin: ")
if pin in ["3", "5", "6", "9"]:
pin = int(pin)
self._pinAttached = pin
break
# Configure the selected pin as PWM output.
if self._pinAttached != None:
self._pwm = mraa.Pwm(self._pinAttached)
self._pwm.period(self._period)
self._pwm.enable(True)
self._pwm.write((self._uSecs/1000000.0)/self._period)
def main():
light = pyupm_grove.GroveLight(LIGHT_SENSOR_PIN)
pwm = mraa.Pwm(LED_PWM_PIN)
pwm.period_us(5000) # Set the period as 5000 us or 5ms
pwm.enable(True) # enable PWM
pwm.write(0)
print "Light sensor bar:"
while True:
ambientLight = light.value()
sys.stdout.write("Light sensor: %02d " % ambientLight)
sys.stdout.write("[")
# Control the intensity of the LED connected to PWM depending on the
# intensity of the ambient light, if intensity is more, the LED will light less brightly
tempLight = ambientLight
if tempLight > MAX_LIGHT:
tempLight = MAX_LIGHT # Nromalize the value
pwmValue = (MAX_LIGHT - tempLight) / float(MAX_LIGHT)
pwm.write(pwmValue)
for i in range(0, MAX_LIGHT):
if ambientLight > i:
sys.stdout.write("=")
elif ambientLight == i:
sys.stdout.write("|")
else:
sys.stdout.write(" ")
#sys.stdout.write("] pwm:%f\r" %pwmValue) # un comment this line if you want to see PWM value
sys.stdout.write("] \r")
sys.stdout.flush()
time.sleep(0.1)
def __init__(self, pin, percent, period, isPWM):
self.pin = pin
self.isPWM = isPWM
if isPWM:
x = mraa.Pwm(pin)
self.percent = percent
self.period = period
def __init__(self, pin):
super(Esc, self).__init__()
self.NEUTRAL = 1300
self.FORWARD_MAX = 2000
self.BACKWARD_MAX = 500
self.LIMIT_SPEED = 100
self.f_step = int((self.FORWARD_MAX - self.NEUTRAL) / 100)
self.b_step = int((self.NEUTRAL - self.BACKWARD_MAX) / 100)
self.x = mraa.Pwm(pin)
self.x.period_ms(20)
self.x.enable(True)
self.current_pulsewidth = self.NEUTRAL
self.step = 100
self.wait_time = 0.2
self.thread_wait = 0.1
self.stop_event = Event()
# logger
self.log = logging.getLogger('Esc')
self.log.setLevel(logging.INFO)
logHandler = logging.FileHandler(
datetime.datetime.now().strftime('Esc_%Y%m%d_%H%M%S.log'))
logHandler.setFormatter(logging.Formatter('%(asctime)s %(message)s'))
self.log.addHandler(logHandler)
self.logpath = ""
self.log.info('Starting Esc...')
def __init__(self,
mosfetPin,
outputVoltDriver,
inputVoltsDriver,
currentDriver,
targetVolts,
minInputVolts=6):
self.mosfet = mraa.Pwm(mosfetPin)
self.mosfet.period_us(700)
self.mosfet.enable(True)
self.output = outputVoltDriver
self.input = inputVoltsDriver
self.outputCurrent = currentDriver
self.highWatts = 0
self.prevWatts = 0
self.maxWatts = 10
self.pmwInc = 1
self.targetVolts = targetVolts
self.inputVoltThreshold = minInputVolts
self.volts = 0
self.current = 0
self.sweepMode = Mode.SweepUp
self.pwmValue = 0
self.pwmInc = 1
def __init__(self, pin, pwm_period_us=700):
self._pwm = mraa.Pwm(pin)
self._pwm.period_us(pwm_period_us)
self._pwm.enable(True)
self._loop = True
self._pattern = [0]
self._pattern_index = 0
def init_mraa(self):
# mraa will make sure the Pwm is configured properly
x = mraa.Pwm(3)
x.period_us(10)
x.pulsewidth_us(5)
x.enable(True)
x.enable(False)
def configureArduinoPwm(self):
ckboxtree = CheckboxTree(height=6, scroll=0)
ckboxtree.append(text='PWM 4', item=4, selected=self.checkPinmuxConfig('PWM_4'))
ckboxtree.append(text='PWM 5', item=5, selected=self.checkPinmuxConfig('PWM_5'))
ckboxtree.append(text='PWM 6', item=6, selected=self.checkPinmuxConfig('PWM_6'))
ckboxtree.append(text='PWM 7', item=7, selected=self.checkPinmuxConfig('PWM_7'))
ckboxtree.append(text='PWM 8', item=8, selected=self.checkPinmuxConfig('PWM_8'))
ckboxtree.append(text='PWM 9', item=9, selected=self.checkPinmuxConfig('PWM_9'))
buttonbar = ButtonBar(screen=self.topmenu.gscreen, buttonlist=[('Ok', 'ok'), ('Cancel', 'cancel', 'ESC')])
g = GridForm(self.topmenu.gscreen, # screen
'Enable PWM on IO4-IO9', # title
1, 2) # 1x1 grid
g.add(ckboxtree, 0, 0)
g.add(buttonbar, 0, 1)
result = g.runOnce()
if buttonbar.buttonPressed(result) == 'cancel':
return
selected = ckboxtree.getSelection()
for n in range(4, 10):
if n in selected:
pwm = mraa.Pwm(n)
self.setPinmuxOfUserConfig('PWM_' + str(n))
else:
self.resetPinmuxOfUserConfig('PWM_' + str(n))
self.saveConfig(self.config)
def pwm():
global pwm_var
global ADC1
global ADC2
ldr = ADC1
x = mraa.Pwm(9)
x.period_us(700)
x.enable(True)
y = mraa.Pwm(10)
y.period_us(700)
y.enable(True)
##adc2 aprox 0.18
while (tempo <= tempoTotal + 1):
pwm_ADC = ADC1
x.write(pwm_ADC)
ldr = ADC1
if (tempo >= 0 and tempo <= 10):
pwm_var = (0.05 * tempo) * ADC1 / ldr + 0 * ADC2 ## y = 0.05x
y.write(pwm_var)
elif (tempo <= 12):
pwm_var = (0.5) ##mantem
y.write(pwm_var)
elif (tempo <= 15):
pwm_var = (-0.0666667 *
tempo) * ADC1 / ldr + ADC2 * 7.2 ##y = 1.3 - 0.066667x
y.write(pwm_var)
elif (tempo <= 20):
pwm_var = (0.3) ##mantem
y.write(pwm_var)
elif (tempo <= 25):
pwm_var = (0.1 * tempo) * ADC1 / ldr - ADC2 * 9.4 ##y = 0.1x - 1.7
y.write(pwm_var)
elif (tempo < 30):
pwm_var = (-0.16 *
tempo) * ADC1 / ldr + ADC2 * 25.8 ##y = 4.8 - 0.16x
y.write(pwm_var)
else:
x.write(0)
y.write(0)
x.write(0)
y.write(0)
def __init__(self, pin, name):
self.pin = pin
self.name = name
self.pwm = mraa.Pwm(pin)
self.pwm.period_us(700)
self.pwm.enable(True)
self.brightness_value = 0
self.set_brightness(0)
def __init__(self,
bot_id=None,
sensor_event_1lf=None,
config=None,
on_robot=False):
# on_robot = False
if bot_id is not None:
for bot in config["bots"]:
if bot["bot_id"] == bot_id and bot["is_real"]:
on_robot = True
if on_robot is True:
mraa.pwma = mraa.Pwm(20)
mraa.pwma.period_us(1000)
mraa.pwma.enable(True)
mraa.pwmb = mraa.Pwm(14)
mraa.pwmb.period_us(1000)
mraa.pwmb.enable(True)
mraa.a1 = mraa.Gpio(33)
mraa.a1.dir(mraa.DIR_OUT)
mraa.a2 = mraa.Gpio(46)
mraa.a2.dir(mraa.DIR_OUT)
mraa.b1 = mraa.Gpio(48)
mraa.b1.dir(mraa.DIR_OUT)
mraa.b2 = mraa.Gpio(36)
mraa.b2.dir(mraa.DIR_OUT)
mraa.pwma.write(0)
mraa.pwmb.write(0)
mraa.a1.write(0)
mraa.b1.write(0)
mraa.a2.write(0)
mraa.b2.write(0)
if sensor_event_1lf is not None:
self.sensor_event_1lf = sensor_event_1lf
self.sensors_dict = {
"1lf":
Sensor(Control.sensor_1lf,
self.sensor_event_1lf,
config=config)
}
def __init__(self, pin, brake, direction):
self.m = mraa.Pwm(pin)
self.m.period_us(700)
self.m.enable(True)
self.brake = mraa.Gpio(brake)
self.brake.dir(mraa.DIR_OUT)
self.brake.write(0)
self.dir = mraa.Gpio(direction)
self.dir.dir(mraa.DIR_OUT)
def PWM_init():
global pwm_pin
try:
pwm_pin = m.Pwm(p)
print("Initializing pin " + str(p) + " as PWM")
return 0
except ValueError:
print(sys.exc_info()[1][0])
return 1
def get_pins():
pins = {}
for pin_number in LED_PIN_NUMBERS:
pin = mraa.Pwm(pin_number)
pin.period_us(PWM_INTERVAL)
pin.enable(True)
pin.write(0.0)
pins[pin_number] = pin
return pins
class ws(tornado.websocket.WebSocketHandler):
x1 = mraa.Pwm(10) #selecting pin for each fingure from thumb to pinky
x2 = mraa.Pwm(9)
x3 = mraa.Pwm(6)
x4 = mraa.Pwm(5)
x5 = mraa.Pwm(3)
x1.period_us(
4640
) #defining pwm period. as i've said earlier it should be 20ms but it better to work with 4640 micro second for me. but you try with 20 ms first
x2.period_us(4640)
x3.period_us(4640)
x4.period_us(4640)
x5.period_us(4640)
x1.enable(True) #enable all pins
x2.enable(True)
x3.enable(True)
x4.enable(True)
x5.enable(True)
def open(self):
print "connection open..."
def on_message(self, m):
#separate angle from message.
angles = m.split('|')
print self.angle_to_val(angles[0])
#write value on pwm pins
ws.x1.write(self.angle_to_val(angles[0]))
print self.angle_to_val(angles[1])
ws.x2.write(self.angle_to_val(angles[1]))
print self.angle_to_val(angles[2])
ws.x3.write(self.angle_to_val(angles[2]))
print self.angle_to_val(angles[3])
ws.x4.write(self.angle_to_val(angles[3]))
print self.angle_to_val(angles[4])
ws.x5.write(self.angle_to_val(angles[4]))
#map angle on pwm value
def angle_to_val(self, angle):
val = float(angle) * 0.0023888
return val + 0.13
def setup(self):
# Sets up the servo pins
self.servo1 = mraa.Pwm(32)
self.servo1.enable(True)
self.servo1.period_us(20000)
self.Helpers.logger.info("Servo 1 ready.")
self.servo2 = mraa.Pwm(16)
self.servo2.enable(True)
self.servo2.period_us(20000)
self.Helpers.logger.info("Servo 2 ready.")
self.servo3 = mraa.Pwm(33)
self.servo3.enable(True)
self.servo3.period_us(30000)
self.Helpers.logger.info("Servo 3 ready.")
def setAngle(self, angle):
period = (self.max_pulse - self.min_pulse) / self.MAX_ANGLE
cycles = int(100.0 *
(abs(self.current_angle - angle) / self.MAX_ANGLE))
servo = mraa.Pwm(self.pin.location)
for cycle in range(0, cycles):
servo.period_us(self.MAX_PERIOD)
servo.pulsewidth_us(self.calculatePulse(angle))
self.current_angle = angle
print 'pulse = ', self.calculatePulse(angle), ', cycles = ', cycles