def __init__(self, c1, c2, c3):
self.board = Slush.sBoard()
self.motors = [Slush.Motor(c1), Slush.Motor(c2), Slush.Motor(c3)]
for m in self.motors:
m.setCurrent(20, 100, 100, 100)
m.setAccel(750)
m.setMaxSpeed(750)
m.setLimitHardStop(0)
def init_winder():
# Slush Engine init variables
global slush_board
global lateral_motor
global drive_motor
slush_board = Slush.sBoard()
lateral_motor = Slush.Motor(0)
drive_motor = Slush.Motor(1)
# Magnetorqer specific winding variables in mm (can be customized later)
global wire_gauge
global rod_diameter
wire_gauge = 0.02
rod_diameter = 5
def __init__(self):
#setup all of the axis for the SlushEngine
Slush.sBoard()
# steps per revolution obtained by manual obervation of steps to do 90 degree
# multiplying by four to get steps per 360 degree.
self._axis = [
Axis(Slush.Motor(1),
minimum=-8000,
maximum=5500,
speed=10,
current=[65, 65, 65, 65],
steps=32000), #Shoulder
Axis(Slush.Motor(0),
minimum=-4000,
maximum=4000,
speed=10,
current=[65, 70, 60, 70],
steps=16000), #Arm
Axis(Slush.Motor(2),
minimum=-20000,
maximum=20000,
speed=30,
current=[50, 50, 50, 50],
steps=64000),
Axis(Slush.Motor(3),
minimum=-3000,
maximum=3000,
speed=20,
current=[75, 75, 75, 75],
steps=6000),
Axis(Slush.Motor(4),
minimum=-4000,
maximum=4000,
speed=20,
current=[85, 85, 85, 85],
steps=14000),
Axis(Slush.Motor(5),
minimum=-1650,
maximum=1650,
speed=20,
current=[65, 65, 65, 65],
steps=3600)
]
self._gripper = Gripper()
self._target = list(map(lambda a: a.getPositionInRad(), self._axis))
posOfGripper = self._gripper.getPosInRad()
self._target.insert(0, posOfGripper)
self._target.insert(0, posOfGripper)
def __init__(self, config_file):
self.board = Slush.sBoard()
self.loader = config.ConfigLoader()
self.config = config_file
# Create and configure joints and gripper from config file
self.joints = self.loader.create_motors(config_file)
self.pwm = self.loader.create_gripper(config_file)
self.gripper = Gripper(self.pwm)
self.x_history = []
self.history = []
self.fig, self.ax = plt.subplots()
self.tracked_motor = self.motors[2]
import numpy as np
import random
import data_input
import gallery_timings
def cmToStep(cm):
return (round(cm * 200 / 9))
# fresh data
tide.get_tide_data()
wave.get_wave_data()
# initalizes the board and all its functions
SlushEngine = Slush.sBoard()
# initalizes the motor on the board
Motor0 = Slush.Motor(0)
Motor1 = Slush.Motor(3)
def motorReset(Motor_name=Motor0):
Motor_name.resetDev()
Motor_name.setMicroSteps(1)
# Motor_name.free()
def off(Motor_name=Motor0):
import time
import Slush
# initalizes the board and all its functions
board = Slush.sBoard()
print("Initalizing the board and all its functions")
time.sleep(1)
# initalizes the motorOne motor on the board
motorOne = Slush.Motor(0)
motorOne.resetDev()
print("Initalizing motorOne the motor on the board")
time.sleep(1)
# initalizes the motorTwo motor on the board
motorTwo = Slush.Motor(1)
motorTwo.resetDev()
print("Initalizing the motorTwo motor on the board")
time.sleep(1)
# set motorOne motor current hold, run, accel, decel & steps
motorOne.setCurrent(10, 50, 25, 25)
motorOne.setMicroSteps(8)
print("Setting motorOne motor current hold, run, accel, decel & steps")
time.sleep(1)
# set motorTwo motor current hold, run, accel, decel & steps
motorTwo.setCurrent(10, 20, 25, 25)
motorTwo.setMicroSteps(8)
print("Setting motorTwo motor current hold, run, accel, decel & steps")
time.sleep(1)
import Slush
b = Slush.sBoard()
class Stepper(Slush.Motor):
#Stepper constructor, if no setCurrent params are given defaults to 20
def __init__(self, **kwargs):
super().__init__(kwargs.get("port", 0))
self.resetDev()
self.microSteps = kwargs.get("microSteps", 32)
self.setMicroSteps(self.microSteps)
self.setCurrent(kwargs.get("runCurrent", 20),
kwargs.get("accelCurrent", 20),
kwargs.get("deaccelCurrent", 20),
kwargs.get("holdCurrent", 20))
self.stepsPerUnit = kwargs.get("stepsPerUnit", 200 / 25.4)
self.speed = kwargs.get("speed", 10)
self.accel = kwargs.get("accel", 10)
self.setAccel(self.accel)
self.setSpeed(self.speed)
def getMicroSteps(self):
return self.microSteps
def setSpeed(self, speed):
self.speed = speed * self.stepsPerUnit
self.setMaxSpeed(self.speed)
def home(self, direction):
import Slush
# the number of steps we want to be moving
stepmove = 300000
# setup the Slushengine
board = Slush.sBoard()
motor = Slush.Motor(0)
motor.resetDev()
# this isn 't current
motor.setCurrent(20, 20, 20, 20)
# move the motor in one direction and wait for it to finish
while (motor.isBusy()):
continue
motor.move(stepmove)
while (motor.isBusy()):
continue
motor.move(-1 * stepmove)
# when these operations are finished shut off the motor
while (motor.isBusy()):
continue
# release motor
motor.free()
"""
Title: Input Output
Description: This program will read the inputs on the SlushEngine expansion IO. During this program you
may apply 3.3V or ground to any of the pins to observe there change. This program can be modified to
include motor controls based on the inputs and outputs.
"""
#import the required module
import Slush
import time
#initalizes the board and all its functions
SlushEngine = Slush.sBoard()
i = 0
j = 0
label = "A"
while (1):
#reads pin A0 and prints the value
print("Pin " + label + str(i) + ": " +
str(SlushEngine.setIOState(j, i, 1)) + " ON.")
time.sleep(0.1)
print("Pin " + label + str(i) + ": " +
str(SlushEngine.setIOState(j, i, 0)) + " OFF.")
time.sleep(0.1)
i = i + 1
if i == 8:
# This function will send the feeder block back to it's home position import Slush import time board = Slush.sBoard() lat_motor = Slush.Motor(0) lat_motor.run(0, 500) time.sleep(2) lat_motor.run(1, 500) time.sleep(2) lat_motor.run(1, 0)
from inputs import get_gamepad
import RPi.GPIO as GPIO
import Slush
import math
import time
#setup all of the axis for the SlushEngine
b = Slush.sBoard()
joints = [
Slush.Motor(0),
Slush.Motor(1),
Slush.Motor(2),
Slush.Motor(3),
Slush.Motor(4),
Slush.Motor(5)
]
#reset the joints to clear previous errors
for joint in joints:
joint.resetDev()
joint.setMicroSteps(16)
#some initalization stuff that needs cleanup
joints[0].setMaxSpeed(150)
joints[1].setMaxSpeed(150)
joints[2].setMaxSpeed(250)
joints[3].setMaxSpeed(150)
joints[4].setMaxSpeed(150)
joints[5].setMaxSpeed(150)
#joint current limits. Still setting manually becuase testing (hold A, run A, acc A, dec, A)
class RBX1:
# setup all of the axis for the SlushEngine
b = Slush.sBoard()
joints = [
Slush.Motor(0),
Slush.Motor(1),
Slush.Motor(2),
Slush.Motor(3),
Slush.Motor(4),
Slush.Motor(5)
]
pwm = None
gripper = 7
def setup(self):
for joint in self.joints:
joint.resetDev()
joint.setMicroSteps(16)
# some initialization stuff that needs cleanup
self.joints[0].setMaxSpeed(150)
self.joints[1].setMaxSpeed(150)
self.joints[2].setMaxSpeed(150)
self.joints[3].setMaxSpeed(150)
self.joints[4].setMaxSpeed(150)
self.joints[5].setMaxSpeed(150)
# joint current limits. Still setting manually because testing (hold A, run A, acc A, dec, A)
# self.joints[0].setCurrent(75, 85, 75, 70)
curr = 55
self.joints[0].setCurrent(curr, curr, curr, curr)
self.joints[0].setOverCurrent(1500)
self.joints[0].setLowSpeedOpt(0)
# self.joints[1].setCurrent(75, 70, 80, 75)
# self.joints[2].setCurrent(50, 50, 50, 50)
curr_2 = 55
self.joints[2].setCurrent(curr_2, curr_2, curr_2, curr_2)
self.joints[2].setOverCurrent(375)
self.joints[2].setLowSpeedOpt(1)
# self.joints[3].setCurrent(75, 75, 75, 75)
# self.joints[4].setCurrent(85, 85, 85, 85)
# self.joints[5].setCurrent(65, 65, 65, 65)
# setup the gripper
GPIO.setmode(GPIO.BCM)
GPIO.setup(18, GPIO.OUT)
self.pwm = GPIO.PWM(18, 100)
def wait_for_robot(self):
for joint in self.joints:
while joint.isBusy():
continue
def print_positions(self):
pos = []
for joint in self.joints:
pos.append(joint.getPosition())
print("Positions: {}".format(pos))
def get_currents(self, motor):
temp = [
motor.getParam(LReg.KVAL_RUN),
motor.getParam(LReg.KVAL_ACC),
motor.getParam(LReg.KVAL_DEC),
motor.getParam(LReg.KVAL_HOLD)
]
return temp
def print_status(self):
nb = 0
status = self.joints[nb].getStatus()
print("Status: {0:016b}".format(status))
print("ADC_OUT: {:05b}".format(self.joints[nb].getParam(LReg.ADC_OUT)))
print("Config: {:07b}".format(self.joints[nb].getParam(LReg.STALL_TH)))
if not status & LReg.STATUS_STEP_LOSS_A:
print("Step LOSS A")
if not status & LReg.STATUS_STEP_LOSS_B:
print("Step LOSS B")
if not status & LReg.STATUS_UVLO:
print("Under voltage lockout")
if not status & LReg.STATUS_OCD:
print("Overcurrent")
if not status & LReg.STATUS_TH_WRN:
print(" ##### Thermal Warning ##### ")
# raise Exception("THERMAL WARNING")
if not status & LReg.STATUS_BUSY:
print(" Busy ")
# raise Exception("THERMAL WARNING")
def run_robot(self):
# start reading the inputs from the gamepad and putting them out the joints
while 1:
print("Time: {}".format(time.time()))
self.print_positions()
self.print_status()
events = get_gamepad()
for event in events:
if event.code == 'BTN_MODE':
value = event.state
if value == 1:
for joint in self.joints:
joint.free()
if event.code == 'ABS_Y':
value = event.state
self.print_status()
if value < -1500:
if not self.joints[0].isBusy():
self.joints[0].run(0, 35)
elif value > 5000:
if not self.joints[0].isBusy():
self.joints[0].run(1, 35)
else:
if not self.joints[0].isBusy():
self.joints[0].softStop()
if event.code == 'ABS_X':
value = event.state
if value < -1500:
if not self.joints[1].isBusy():
self.joints[1].run(1, 20)
elif value > 5000:
if not self.joints[1].isBusy():
self.joints[1].run(0, 20)
else:
if not self.joints[1].isBusy():
self.joints[1].softStop()
if event.code == 'ABS_RX':
value = event.state
if value < -3500:
if not self.joints[2].isBusy():
self.joints[2].run(1, 100)
elif value > 3500:
if not self.joints[2].isBusy():
self.joints[2].run(0, 100)
else:
if not self.joints[2].isBusy():
self.joints[2].softStop()
if event.code == 'ABS_RY':
value = event.state
if value < -3500:
if not self.joints[3].isBusy():
self.joints[3].run(1, 10)
elif value > 3500:
if not self.joints[3].isBusy():
self.joints[3].run(0, 10)
else:
if not self.joints[3].isBusy():
self.joints[3].softStop()
if event.code == 'ABS_HAT0Y':
value = event.state
if value == 1:
if not self.joints[4].isBusy():
self.joints[4].run(1, 20)
elif value == -1:
if not self.joints[4].isBusy():
self.joints[4].run(0, 20)
else:
if not self.joints[4].isBusy():
self.joints[4].softStop()
if event.code == 'ABS_HAT0X':
value = event.state
if value == 1:
if not self.joints[5].isBusy():
self.joints[5].run(1, 20)
elif value == -1:
if not self.joints[5].isBusy():
self.joints[5].run(0, 20)
else:
if not self.joints[5].isBusy():
self.joints[5].softStop()
# Opens and closes the gripper
if event.code == 'BTN_TL':
if event.state == 1:
self.gripper -= 1
if self.gripper < 7:
self.gripper = 7
self.pwm.start(self.gripper)
if event.code == 'BTN_TR':
if event.state == 1:
self.gripper += 1
if self.gripper > 17:
self.gripper = 17
self.pwm.start(self.gripper)
# calibrate all axis if on point
if event.code == 'BTN_START':
if event.state == 1:
for joint in self.joints:
joint.setAsHome()
def __init__(self, name, number):
self.name = name
self.number = number
self.motor = Slush.Motor(self.number)
from inputs import get_gamepad
import RPi.GPIO as GPIO
import Slush
import math
import time
#setup all of the axis for the SlushEngine
b = Slush.sBoard()
joints = [Slush.Motor(0), Slush.Motor(1), Slush.Motor(2), Slush.Motor(3), Slush.Motor(4), Slush.Motor(5)]
#reset the joints to clear previous errors
for joint in joints:
joint.resetDev()
joint.setMicroSteps(16)
#some initalization stuff that needs cleanup
joints[0].setMaxSpeed(150)
joints[1].setMaxSpeed(150)
joints[2].setMaxSpeed(250)
joints[3].setMaxSpeed(150)
joints[4].setMaxSpeed(150)
joints[5].setMaxSpeed(150)
#joint current limits. Still setting manually becuase testing (hold A, run A, acc A, dec, A)
joints[0].setCurrent(65, 85, 75, 70)
joints[1].setCurrent(65, 85, 85, 65)
joints[2].setCurrent(50, 50, 50, 50)
joints[3].setCurrent(75, 75, 75, 75)
joints[4].setCurrent(85, 85, 85, 85)
""" @file slush_manager.py File to instantiate a Slush Board, to use the same board reference in different files """ import Slush slush_board = Slush.sBoard()
#coding=utf-8
import time
import Slush
import numpy as np
import RPi.GPIO as GPIO
from Slush.Devices import L6470Registers as LReg
GPIO.setwarnings(False)
s = Slush.sBoard()
m = Slush.Motor(0)
m.xfer(LReg.RESET_DEVICE)
# speeds to characterize
speeds = range(100, 1001, 50)
# target current
current = .7
start_k = 10
# this procedure finds the Krun value that just exceeds the stall threshold
def find_kval(start):
krun = start
# now use succesive approximation to find the next 4 bits
while krun < 256:
krun += 1
m.setParam((0x0A, 8), krun)
m.getStatus()
time.sleep(0.5)
status = m.getStatus()
'''
the example uses the move command and the manual limit switch check command. It moves the motor and then checks the limit switch.
this can be used to do fine manual homing
'''
import Slush
#setup the motor
b = Slush.sBoard()
m = Slush.Motor(1)
m.resetDev()
m.setLimitHardStop(0)
m.setCurrent(50, 50, 50, 50)
m.setMaxSpeed(100)
#move until the limit switch is pressed
while m.getSwitch():
m.move(100)
# This script is the hard coded version of the winding algorithm
import sys
import Slush
import time
board = Slush.sBoard()
lateral_motor = Slush.Motor(0)
drive_motor = Slush.Motor(1)
def requiresTurn(number_of_turns):
number_of_turns_required = rod_length/gauge
if(number_of_turns == number_of_turns_required):
return False
return True
#Define constant that describes the amount of horizontal movement that the feeder block moves for every full turn of motor.
lateral_constant = 0.01
#Define parameters for wire and rod
gauge = 0.02
rod_diameter = 5
rod_length = 100
one_full_turn = #number of steps needed for the stepper to do one full rotation
#Wind time, assume is starting from 0 position which is the location of the first wind.
#Initialize variables for winding algorithm
number_of_layers = 0
number_of_turns = 1
while(number_of_layers < 14):
''' This python script will home the stepper motor to a limit switch. It will then set the internal position counter to zero any moves made after this will be in reference to the home position. Things to watch out for - are you limit switches NO or NC? - what direction is your stepper traveling when it homes? ''' import Slush #setup the Slushengine b = Slush.sBoard() axis1 = Slush.Motor(0) axis1.resetDev() axis1.setCurrent(20, 20, 20, 20) #home the motor off a limit switch while(axis1.isBusy()): continue axis1.goUntilPress(0, 1, 1000) #spins until hits a NO limit at speed 1000 and direction 1 while(axis1.isBusy()): continue axis1.setAsHome() #set the position for 0 for all go to commands axis1.goTo(-100000) #move to -100000 while(axis1.isBusy()): continue
Title: Motor Settings
Description: This example will iterate through a number of diffrent stepper motor
settings. This program is intended to show some of the diffrent configurations the
motor is capable of.
Note: This example is dependant on your type of motor and power supply. If you find
you motor skipping steps you may need to slow it down a bit.
"""
# import the required module
import Slush
import time
# initalizes the board and all its functions
SlushEngine = Slush.sBoard()
# initalizes the motor on the board
Motor = Slush.Motor(0)
Motor.resetDev()
Motor.setCurrent(20, 20, 20, 20)
# run the motor with the defulat settings
print("Running the motor at 100 Steps/Second")
Motor.run(1, 100)
time.sleep(2)
# disable motor driving power
print("Drive disabled, motor can now free spin")
Motor.free()
time.sleep(5)
Title: Motor Settings
Description: This example will iterate through a number of diffrent stepper motor
settings. This program is intended to show some of the diffrent configurations the
motor is capable of.
Note: This example is dependant on your type of motor and power supply. If you find
you motor skipping steps you may need to slow it down a bit.
"""
#import the required module
import Slush
import time
#initalizes the board and all its functions
SlushEngine = Slush.sBoard()
#initalizes the motor on the board
Motor = Slush.Motor(0)
Motor.resetDev()
Motor.setCurrent(20, 20, 20, 20)
#run the motor with the defulat settings
print("Running the motor at 100 Steps/Second")
Motor.run(1, 100)
time.sleep(2)
#disable motor driving power
print("Drive disabled, motor can now free spin")
Motor.free()
time.sleep(5)
