diff --git a/Project.toml b/Project.toml index e433a76..aec4c2d 100644 --- a/Project.toml +++ b/Project.toml @@ -3,7 +3,10 @@ uuid = "36c24c14-3ff8-4745-ae4c-19a959bc73ea" version = "0.1.0" [deps] +IntervalSets = "8197267c-284f-5f27-9208-e0e47529a953" Sockets = "6462fe0b-24de-5631-8697-dd941f90decc" +Unitful = "1986cc42-f94f-5a68-af5c-568840ba703d" [compat] +IntervalSets = "0.7" julia = "1.6" diff --git a/src/TrinamicMotionControl.jl b/src/TrinamicMotionControl.jl index 6d7361f..5005638 100644 --- a/src/TrinamicMotionControl.jl +++ b/src/TrinamicMotionControl.jl @@ -9,6 +9,9 @@ module TrinamicMotionControl using Sockets +using IntervalSets +using Unitful + const Value = Union{<:Real, Missing} diff --git a/src/motor.jl b/src/motor.jl index 30735e3..1a87c9a 100644 --- a/src/motor.jl +++ b/src/motor.jl @@ -1,3 +1,308 @@ # This file is a part of TrinamicMotionControl.jl, licensed under the MIT License (MIT). +mutable struct Motor + name::String + motor_controller::TMCM3110 + id::Int + axis_type::Symbol #:periodic, :linear + axis_range::Union{Missing, ClosedInterval} + manual_range::Union{Missing, ClosedInterval} + motor_position::Value + encoder_position::Value + calibrated::Bool + safe_to_move::Bool + + function Motor(name::String, + motor_controller::TMCM3110, + id::Int, + axis_type::Symbol, + manual_range::ClosedInterval, + safe_to_move::Bool) + m = new(name, motor_controller, id, axis_type, missing, manual_range, missing, missing, false, safe_to_move) + update_motor!(m) + m + end + + function Motor(name::String, + motor_controller::TMCM3110, + id::Int, + axis_type::Symbol, + safe_to_move::Bool) + m = new(name, motor_controller, id, axis_type, missing, missing, missing, missing, false, safe_to_move) + update_motor!(m) + m + end + + function Base.println(m::Motor) + println() + println("\t--- $(m.name) ---") + println("\tMotor Controller = $(m.motor_controller.name):$(m.id)") + println("\tMotor Position = $(round(m.motor_position, digits = 2))") + println("\tEncoder Position = $(round(m.encoder_position, digits = 2))") + println("\tAxis Range = $(ismissing(m.axis_range) ? missing : 0..round(m.axis_range.right, digits = 2))") + println("\tManual Range = $(m.manual_range)") + println("\tCalibrated = $(m.calibrated)") + println("\tIs safe to move = $(m.safe_to_move)") + println("\tUnits = $(unit(m))") + end + + function Base.show(m::Motor) println(m) end + function Base.display(m::Motor) println(m) end +end + +const relative_motor_encoder_tol = 0.05 +const encoder_factor = 1.024 + +function unit(m::Motor) + if m.axis_type == :linear + return u"mm" + elseif m.axis_type == :periodic + return u"°" + else + @error "Incompatible units for $(m.name)" + end +end + +function direction(m::Motor) + calibration_mode = get_axis_parameter(m.motor_controller, 193, m.id) + if calibration_mode in [65, 2] + return 1 + elseif calibration_mode in [66] + return -1 + else + @error "Calibration Mode not programmed" + end +end + +function convert_unit_to_steps(m::Motor, val::Value)::Union{Int32, Missing} + dir = direction(m) + if m.axis_type == :linear + Int32(dir * round((5000000 / 97.7) * val)) + elseif m.axis_type == :periodic + Int32(dir * round((640000 / 360) * val)) + else + @error "Incompatible units for $(m.name)" + end +end + +function convert_steps_to_unit(m::Motor, steps::Union{Int16, Int32, Int64, Missing})::Value + dir = direction(m) + if m.axis_type == :linear + try + return dir * steps * (97.7 / 5000000) + catch err + @warn err + return missing + end + elseif m.axis_type == :periodic + try + return dir * steps * (360 / 640000) + catch err + @warn err + return missing + end + else + @error "Incorrect Axis Type" + end +end + +function stop(m::Motor)::Nothing + stop(m.motor_controller, n_motor=m.id) + + @info "Check $(m.name) velocity:" + speed = get_axis_parameter( m.motor_controller, 3, m.id ) + @info "Motor speed = $speed" + if speed==0 + @info "$(m.name) stoped!" + else + sleep(0.1) + @warn "$(m.name) still moving." + stop(m) + end + nothing +end + + +function update_motor_position!(m::Motor)::Nothing + try + steps = get_axis_parameter(m.motor_controller, 1, m.id) + pos = convert_steps_to_unit(m, steps) + m.motor_position = pos + catch err + @warn err + m.motor_position = missing + end + nothing +end + +function update_encoder_position!(m::Motor)::Nothing + try + steps = get_axis_parameter(m.motor_controller, 209, m.id) + pos = encoder_factor * convert_steps_to_unit(m, steps) + m.encoder_position = pos + catch err + @warn err + m.encoder_position = missing + end + nothing +end + +function update_axis_range!(m::Motor)::Nothing + try + steps = get_axis_parameter(m.motor_controller, 196, m.id ) + length = convert_steps_to_unit(m, steps) + m.axis_range = 0..length + catch err + @warn err + m.axis_range = missing + end + nothing +end + +function update_motor!(m::Motor)::Nothing + update_motor_position!(m) + update_encoder_position!(m) + update_axis_range!(m) + is_calibrated!(m) +end + +function calibrate_motor(m::Motor; force::Bool = false)::Nothing + if !m.safe_to_move && !force + @warn "$(m.name) is not safe to be moved. Skipping Calibration." + return nothing + else + if m.axis_type == :periodic + update_motor_position!(m) + update_encoder_position!(m) + if !ismissing(m.motor_position) && !ismissing(m.encoder_position) + @warn "Move $(m.name) to -10° to ensure proper cable management." + δ = m.encoder_position - m.motor_position + move_to(m, -10 - δ, force = true) + end + end + start_calibration = 0 + get_status = 2 + fetch(m.motor_controller, 1, 13, start_calibration, m.id, 0) + sleep(0.05) + calibration_status = fetch(m.motor_controller, 1, 13, get_status, m.id, 0) + prog = ProgressUnknown("Calibrating $(m.name)", spinner = true) + while calibration_status != 0 + sleep(1) + try + update_motor_position!(m) + update_encoder_position!(m) + pos = round(m.encoder_position, digits = 2) + next!(prog, spinner = "🐾👣", showvalues = [("Encoder Position", pos * unit(m))]) + sleep(0.05) + calibration_status = fetch(m.motor_controller, 1, 13, get_status, m.id, 0) + catch err + @warn err + end + end + finish!(prog) + @info "\nCalibration finished:" + sleep(0.1) + update_motor!(m) + if m.axis_type == :linear + @info "$(m.name) -> Axis range = $(m.axis_range)" + elseif m.axis_type == :periodic + @info "$(m.name) -> Axis range = Periodic\nManual range = $(m.manual_range)" + end + end +end + +function abort_calibration(m::Motor) + stop_calibration = 1 + fetch(m.motor_controller, 1, 13, stop_calibration, m.id, 0) +end + +function is_calibrated!(m::Motor)::Nothing #use after motor/encoder positions have been updated + try + steps = get_axis_parameter(m.motor_controller, 196, m.id) + if abs(m.encoder_position - m.motor_position) ≤ relative_motor_encoder_tol && steps != 0 + m.calibrated = true + else + m.calibrated = false + end + catch err + @warn err + m.calibrated = false + end + nothing +end + +function print_position(m, pos, alt_units) + print_pos = ismissing(alt_units) ? pos : ustrip(uconvert(alt_units, pos*unit(m))) + round(print_pos, digits = 2) * (ismissing(alt_units) ? unit(m) : alt_units) +end + +#note that is block = false is set, motors will need to be manually updated +function move_to(m::Motor, position::Value; force::Bool = false, alt_units = missing, block = true)::Nothing + update_motor_position!(m) + update_encoder_position!(m) + is_calibrated!(m) + if !force + range = ismissing(m.manual_range) ? m.axis_range : m.manual_range + axorm = ismissing(m.manual_range) ? "axis" : "manual" + if !(position in range) + @warn "New position is not within $axorm range: $range. Ignored move command." + return nothing + elseif !m.safe_to_move + @warn "$(m.name) is not safe to move. Ignored move command." + return nothing + elseif !m.calibrated + @warn "$(m.name) is not calibrated. Ignored move command." + return nothing + end + end + init_pos = force ? m.encoder_position : m.motor_position + move_to(m.motor_controller, m.id, convert_unit_to_steps(m,position)) + if block + prog = Progress(1001, desc = "Moving $(m.name) 🐾 ", barlen = 20)#barglyphs=BarGlyphs(" 🐾 ")) + final_check = 0 + while final_check == 0 + sleep(0.5) + try + force ? update_encoder_position!(m) : update_motor_position!(m) + temp_pos = force ? m.encoder_position : m.motor_position + sleep(0.05) + target_reached = get_axis_parameter(m.motor_controller, 8, m.id) + sleep(0.05) + vel = get_axis_parameter(m.motor_controller, 3, m.id) + update!(prog, position == init_pos ? 1000 : Int(round(1000*(temp_pos - init_pos)/(position-init_pos))); showvalues = [("Motor Position", print_position(m, temp_pos, alt_units))]) + if vel == 0 || target_reached == 1 + final_check = 1 + end + catch err + @warn err + end + end + update_motor_position!(m) + update_encoder_position!(m) + temp_pos = force ? m.encoder_position : m.motor_position + update!(prog, position == init_pos ? 1000 : Int(round(1000*(temp_pos - init_pos)/(position-init_pos))); showvalues = [("Motor Position", print_position(m, temp_pos, alt_units))]) + finish!(prog, desc = "$(m.name) is at: $(print_position(m, m.motor_position, alt_units)) | ") + is_calibrated!(m) + end +end + +move_to(m::Motor, position::Quantity; force::Bool = false) = move_to(m, ustrip(uconvert(unit(m),position)), force = force, alt_units = Unitful.unit(position)) + +function move(m::Motor, δ::Value; force::Bool = false, alt_units = missing) + update_motor_position!(m) + pos = m.motor_position + move_to(m, pos + δ, force = force, alt_units = alt_units) +end + +move(m::Motor, δ::Quantity; force::Bool=false) = move(m, ustrip(uconvert(unit(m),δ)), force = force, alt_units = unit(δ)) + +function target_position_reached(m::Motor)::Bool + flag = false + try + flag = get_axis_parameter(m.motor_controller, 8, m.id) + catch err + @warn err + end + return flag +end diff --git a/src/tmcm3110.jl b/src/tmcm3110.jl index 30735e3..9b5d1ac 100644 --- a/src/tmcm3110.jl +++ b/src/tmcm3110.jl @@ -1,3 +1,319 @@ # This file is a part of TrinamicMotionControl.jl, licensed under the MIT License (MIT). +struct TMCM3110 + name::String + ip::IPv4 + port::Int + + function TMCM3110(name::String, ip::IPv4, port::Int) + device = new(name, ip, port) + return device + end + + function Base.show(io::IO, device::TMCM3110) + for n in fieldnames(typeof(device)) + println(io, "$n: $(getfield(device,n))") + end + end +end + +function encode_command(device::TMCM3110, m_address::Int, n_command::Int, n_type::Int, n_motor::Int, value::Signed) + m_address = UInt8( m_address % (1<<8) ) + n_command = UInt8( n_command % (1<<8) ) + n_type = UInt8( n_type % (1<<8) ) + n_motor = UInt8( n_motor % (1<<8) ) + value = Int32( value ) + values = [ parse(UInt8, bitstring(value)[ 1+(8*(i-1)) : 8+(8*(i-1)) ] , base = 2) for i in 1:4] + + checksum = UInt8( (m_address + n_command + n_type + n_motor + sum(values)) % (1<<8) ) + + tmcl_bytes = [m_address, n_command, n_type, n_motor, values..., checksum] + + return tmcl_bytes +end + +function decode_reply(device::TMCM3110, reply) + r_address = UInt8( reply[1] ) + m_address = UInt8( reply[2] ) + r_status = UInt8( reply[3] ) + Int32(r_status) != 100 ? (@warn TMCM3110_STATUSCODES[Int(r_status)]) : nothing + n_command = UInt8( reply[4] ) + values = [ UInt8(value) for value in reply[5:8] ] + r_checksum = UInt8( reply[9] ) + checksum = UInt8( (r_address + m_address + r_status + n_command + sum(values)) % (1<<8) ) + value = parse( UInt32, "$(bitstring(values[1]))$(bitstring(values[2]))$(bitstring(values[3]))$(bitstring(values[4]))" , base = 2 ) + value = reinterpret(Int32, value) + return value +end + +function fetch(device::TMCM3110, m_address, n_command, n_type, n_motor, value; timeout=3.0) + c = -1 + while c == -1 + try + c = connect(device) + break + catch err + @warn err + end + sleep(0.5) + end + cmd = encode_command(device, m_address, n_command, n_type, n_motor, value) + write(c, cmd ) + t0 = time() + t = 0. + r="" + task = @async read(c, 9) + while t < timeout + if task.state == :done break end + t = time()-t0 + sleep(0.01) + end + close(c) + if t >= timeout + error("Timeout! Device `$(device.name)` did not answer.") + else + r = decode_reply(device, task.result) + return r + end +end + +function get_axis_parameter(device::TMCM3110, n_axisparameter, n_motor) + if in(n_axisparameter, TMCM3110_AXIS_PARAMETER.keys) + if in(n_motor, [0,1,2]) + r = "" + while r == "" + try + r = fetch(device, 1, 6, n_axisparameter, n_motor, 0) + catch err + @warn err + sleep(0.5) + end + end + return r + else + err("$n_motor is no a valid motor id. Nothing was done.") + end + else + err("$n_axisparameter is no a valid axis parameter id. Nothing was done.") + end +end + +function set_axis_parameter(device::TMCM3110, n_axisparameter, n_motor, value) + if in(n_axisparameter, TMCM3110_AXIS_PARAMETER.keys) + if in(n_motor, [0,1,2]) + r = fetch(device, 1, 5, n_axisparameter, n_motor, value) + return r + else + err("$n_motor is no a valid motor id. Nothing was done.") + end + else + err("$n_axisparameter is no a valid axis parameter id. Nothing was done.") + end +end + +function store_axis_parameter_permanent(device::TMCM3110, n_axisparameter, n_motor, value) + if in(n_axisparameter, TMCM3110_AXIS_PARAMETER.keys) + if in(n_motor, [0,1,2]) + set_axis_parameter(device, n_axisparameter, n_motor, value) # first set the parameter to value, + r = fetch(device, 1, 7, n_axisparameter, n_motor, 0) # then store it permanent + else + err("$n_motor is no a valid motor id. Nothing was done.") + end + else + err("$n_axisparameter is no a valid axis parameter id. Nothing was done.") + end +end + +function list_all_axis_parameters(device::TMCM3110; pars = : ) + for key in sort(collect(keys(TMCM3110_AXIS_PARAMETER)))[pars] # wrong oder + axis_parameters = Any[0,0,0] + for i in 1:3 + axis_parameters[i] = get_axis_parameter(device, key, i-1) + end + if length(TMCM3110_AXIS_PARAMETER[key]) <= 12 + @info "$key \t- $(TMCM3110_AXIS_PARAMETER[key]):\t\t\t$(axis_parameters[1])\t\t$(axis_parameters[2])\t\t$(axis_parameters[3])" + elseif 12 < length(TMCM3110_AXIS_PARAMETER[key]) <= 20 + @info "$key \t- $(TMCM3110_AXIS_PARAMETER[key]):\t\t$(axis_parameters[1])\t\t$(axis_parameters[2])\t\t$(axis_parameters[3])" + else + @info "$key \t- $(TMCM3110_AXIS_PARAMETER[key]):\t$(axis_parameters[1])\t\t$(axis_parameters[2])\t\t$(axis_parameters[3])" + end + end + return nothing +end + +list_motion_axis_parameters(device::TMCM3110) = list_all_axis_parameters(device::TMCM3110; pars = 1:4 ) + +function move_to(device::TMCM3110, n_motor, value) + try + position = convert(Int32, value) + catch + err("value must be convertible to Int32") + end + if n_motor in [0,1,2] + r = 0 + t = 0 + while t < 10 + try + r = fetch(device, 1, 4, 0, n_motor, convert(Int,value)) + break + catch err + @warn err + end + sleep(1) + t += 1 + end + if t == 10 + error("Motor controller did not answer to move command.") + else + return r + end + else + error("$n_motor is no a valid motor id. Must be in [0,1,2]. Nothing was done.") + end +end + +function stop(device::TMCM3110; n_motor=-1) + if in(n_motor, [0,1,2]) + r = fetch(device, 1, 3, 0, n_motor, 0) + return r + else + r = Int[] + for m in [0,1,2] + push!(r, fetch(device, 1, 3, 0, m, 0)) + end + return r + end +end + +TMCM3110_STATUSCODES = Dict( 100 => "Succesfully executed, no error", + 101 => "Command loaded into TMCL program EEPROM", + 1 => "Wrong Checksum", + 2 => "Invalid command", + 3 => "Wrong type", + 4 => "Invalid value", + 5 => "Configuration EEPROM locked", + 6 => "Command not available" ) + +TMCM3110_COMMAND_NUMBERS = Dict( 1 => "ROR", + 2 => "ROL", + 3 => "MST", + 4 => "MVP", + 5 => "SAP", + 6 => "GAP", + 7 => "STAP", + 8 => "RSAP", + 9 => "SGP", + 10 => "GGP", + 11 => "STGP", + 12 => "RSGP", + 13 => "RFS", + 14 => "SIO", + 15 => "GIO", + 19 => "CALC", + 20 => "COMP", + 21 => "JC", + 22 => "JA", + 23 => "CSUB", + 24 => "RSUB", + 25 => "EI", + 26 => "DI", + 27 => "WAIT", + 28 => "STOP", + 30 => "SCO", + 31 => "GCO", + 32 => "CCO", + 33 => "CALCX", + 34 => "AAP", + 35 => "AGP", + 37 => "VECT", + 38 => "RETI", + 39 => "ACO" ) + +TMCM3110_AXIS_PARAMETER = Dict( 0 => "target position", + 1 => "actual position", + 2 => "target speed", + 3 => "actual speed", + 4 => "max positioning speed", + 5 => "max acceleration", + 6 => "abs max current", + 7 => "standby current", + 8 => "target pos reached", + 9 => "ref switch status", + 10 => "right limit switch status", + 11 => "left limit switch status", + 12 => "right limit switch disable", + 13 => "left limit switch disable", + 130 => "minimum speed", + 135 => "actual acceleration", + 138 => "ramp mode", + 140 => "microstep resolution", + 141 => "ref switch tolerance", + 149 => "soft stop flag", + 153 => "ramp divisor", + 154 => "pulse divisor", + 160 => "step interpolation enable", + 161 => "double step enable", + 162 => "chopper blank time", + 163 => "chopper mode", + 164 => "chopper hysteresis dec", + 165 => "chopper hysteresis end", + 166 => "chopper hysteresis start", + 167 => "chopper off time", + 168 => "smartEnergy min current", + 169 => "smartEnergy current downstep", + 170 => "smartEnergy hysteresis", + 171 => "smartEnergy current upstep", + 172 => "smartEnergy hysteresis start", + 173 => "stallGuard2 filter enable", + 174 => "stallGuard2 threshold", + 175 => "slope control high side", + 176 => "slope control low side", + 177 => "short protection disable", + 178 => "short detection timer", + 179 => "Vsense", + 180 => "smartEnergy actual current", + 181 => "stop on stall", + 182 => "smartEnergy threshold speed", + 183 => "smartEnergy slow run current", + 193 => "ref. search mode", + 194 => "ref. search speed", + 195 => "ref. switch speed", + 196 => "distance end switches", + 201 => "encoder mode", + 204 => "freewheeling", + 206 => "actual load value", + 208 => "TMC262 errorflags", + 209 => "encoder pos", + 210 => "encoder prescaler", + 212 => "encoder max deviation", + 214 => "power down delay" ) + +TMCM3110_INTERRUPT_VECTORS = Dict( 0 => "Timer 0", + 1 => "Timer 1", + 2 => "Timer 2", + 3 => "Target position 0 reached", + 4 => "Target position 1 reached", + 5 => "Target position 2 reached", + 15 => "stallGuard2 axis 0", + 16 => "stallGuard2 axis 1", + 17 => "stallGuard2 axis 2", + 21 => "Deviation axis 0", + 22 => "Deviation axis 1", + 23 => "Deviation axis 2", + 27 => "Left stop switch 0", + 28 => "Right stop switch 0", + 29 => "Left stop switch 1", + 30 => "Right stop switch 1", + 31 => "Left stop switch 2", + 32 => "Right stop switch 2", + 39 => "Input change 0", + 40 => "Input change 1", + 41 => "Input change 2", + 42 => "Input change 3", + 43 => "Input change 4", + 44 => "Input change 5", + 45 => "Input change 6", + 46 => "Input change 7", + 255 => "Global interrupts" ) + \ No newline at end of file