CompatHelper: add new compat entry for IntervalSets at version 0.7, (keep existing compat)

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"

src/TrinamicMotionControl.jl

@@ -9,6 +9,9 @@ module TrinamicMotionControl
 
 using Sockets
 
+using IntervalSets
+using Unitful
+
 const Value = Union{<:Real, Missing}
 
 

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

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" )
+