@@ -123,22 +123,14 @@ defmodule Mix.Tasks.So101.Calibrate do
123123 end
124124
125125 defp run_calibration ( pid , dry_run ) do
126- # Verify all servos are present
127126 { found , missing } = check_servos ( pid )
128127
129128 if missing != [ ] do
130129 Mix . shell ( ) . error ( "Missing servos: #{ inspect ( Enum . map ( missing , fn { _ , id , _ } -> id end ) ) } " )
131- Mix . shell ( ) . info ( "Continue anyway? (y/n)" )
132130
133- case IO . gets ( "" ) do
134- data when is_binary ( data ) ->
135- if String . trim ( data ) |> String . downcase ( ) != "y" do
136- Mix . shell ( ) . info ( "Calibration cancelled." )
137- return_early ( )
138- end
139-
140- _ ->
141- return_early ( )
131+ unless confirm? ( "Continue anyway?" ) do
132+ Mix . shell ( ) . info ( "Calibration cancelled." )
133+ return_early ( )
142134 end
143135 end
144136
@@ -151,21 +143,31 @@ defmodule Mix.Tasks.So101.Calibrate do
151143 "Found #{ length ( found ) } servo(s). Press Enter to disable torque and begin..."
152144 )
153145
154- case IO . gets ( "" ) do
155- data when is_binary ( data ) ->
156- if String . trim ( data ) |> String . downcase ( ) == "q" do
157- Mix . shell ( ) . info ( "Calibration cancelled." )
158- else
159- do_calibration ( pid , found , dry_run )
160- end
161-
162- _ ->
163- Mix . shell ( ) . info ( "Calibration cancelled." )
146+ if prompt_quit? ( ) do
147+ Mix . shell ( ) . info ( "Calibration cancelled." )
148+ else
149+ do_calibration ( pid , found , dry_run )
164150 end
165151 end
166152
167153 defp return_early , do: :ok
168154
155+ defp confirm? ( prompt ) do
156+ Mix . shell ( ) . info ( prompt <> " (y/n)" )
157+
158+ case IO . gets ( "" ) do
159+ data when is_binary ( data ) -> String . trim ( data ) |> String . downcase ( ) == "y"
160+ _ -> false
161+ end
162+ end
163+
164+ defp prompt_quit? do
165+ case IO . gets ( "" ) do
166+ data when is_binary ( data ) -> String . trim ( data ) |> String . downcase ( ) == "q"
167+ _ -> true
168+ end
169+ end
170+
169171 defp check_servos ( pid ) do
170172 Enum . split_with ( @ joints , fn { _name , servo_id , _desc } ->
171173 case Feetech . ping ( pid , servo_id ) do
@@ -258,24 +260,7 @@ defmodule Mix.Tasks.So101.Calibrate do
258260 # Read all positions
259261 new_state =
260262 Enum . reduce ( joints , state , fn { _name , servo_id , _desc } , acc ->
261- case Feetech . read_raw ( pid , servo_id , :present_position ) do
262- { :ok , raw_pos } ->
263- update_in ( acc , [ servo_id ] , fn data ->
264- # Unwrap position to handle 0/4095 boundary crossing
265- unwrapped = unwrap_position ( raw_pos , data . raw , data . unwrapped )
266-
267- % {
268- data
269- | raw: raw_pos ,
270- unwrapped: unwrapped ,
271- min_unwrapped: min ( data . min_unwrapped , unwrapped ) ,
272- max_unwrapped: max ( data . max_unwrapped , unwrapped )
273- }
274- end )
275-
276- _ ->
277- acc
278- end
263+ update_servo_tracking ( pid , servo_id , acc )
279264 end )
280265
281266 # Display current state
@@ -285,6 +270,26 @@ defmodule Mix.Tasks.So101.Calibrate do
285270 end
286271 end
287272
273+ defp update_servo_tracking ( pid , servo_id , state ) do
274+ case Feetech . read_raw ( pid , servo_id , :present_position ) do
275+ { :ok , raw_pos } ->
276+ update_in ( state , [ servo_id ] , fn data ->
277+ unwrapped = unwrap_position ( raw_pos , data . raw , data . unwrapped )
278+
279+ % {
280+ data
281+ | raw: raw_pos ,
282+ unwrapped: unwrapped ,
283+ min_unwrapped: min ( data . min_unwrapped , unwrapped ) ,
284+ max_unwrapped: max ( data . max_unwrapped , unwrapped )
285+ }
286+ end )
287+
288+ _ ->
289+ state
290+ end
291+ end
292+
288293 # Handle position wraparound at 0/4095 boundary
289294 defp unwrap_position ( current_raw , last_raw , last_unwrapped ) do
290295 delta = current_raw - last_raw
@@ -350,42 +355,47 @@ defmodule Mix.Tasks.So101.Calibrate do
350355 results =
351356 for { name , servo_id , _desc } <- joints do
352357 data = state [ servo_id ]
353- range = data . max_unwrapped - data . min_unwrapped
354-
355- if range > 10 do
356- # Calculate center in unwrapped space, then convert to raw (0-4095)
357- center_unwrapped = div ( data . min_unwrapped + data . max_unwrapped , 2 )
358- center_raw = Integer . mod ( center_unwrapped , @ steps_per_revolution )
359-
360- # Firmware applies: Present_Position = Actual_Position - Offset
361- # So: 2048 = center_raw - offset, therefore offset = center_raw - 2048
362- # Clamp to ±2047 (sign_magnitude bit 11 limit).
363- offset = center_raw - @ center_position
364- offset = max ( - @ max_offset_magnitude , min ( @ max_offset_magnitude , offset ) )
365-
366- Mix . shell ( ) . info ( """
367- #{ format_joint ( name ) } (ID #{ servo_id } ):
368- Range: #{ range } steps (#{ format_degrees ( steps_to_degrees ( range ) ) } )
369- Center: #{ center_raw } -> Offset: #{ offset }
370- """ )
371-
372- if dry_run do
373- { name , servo_id , { :ok , % { range: range , center: center_raw , offset: offset } } }
374- else
375- case apply_calibration ( pid , servo_id , offset ) do
376- :ok -> { name , servo_id , { :ok , % { offset: offset } } }
377- { :error , reason } -> { name , servo_id , { :error , reason } }
378- end
379- end
380- else
381- Mix . shell ( ) . info ( " #{ format_joint ( name ) } (ID #{ servo_id } ): Skipped (not moved enough)" )
382- { name , servo_id , { :error , :not_moved } }
383- end
358+ process_joint_result ( pid , name , servo_id , data , dry_run )
384359 end
385360
386361 print_summary ( results , dry_run )
387362 end
388363
364+ defp process_joint_result ( _pid , name , servo_id , data , _dry_run )
365+ when data . max_unwrapped - data . min_unwrapped <= 10 do
366+ Mix . shell ( ) . info ( " #{ format_joint ( name ) } (ID #{ servo_id } ): Skipped (not moved enough)" )
367+ { name , servo_id , { :error , :not_moved } }
368+ end
369+
370+ defp process_joint_result ( pid , name , servo_id , data , dry_run ) do
371+ range = data . max_unwrapped - data . min_unwrapped
372+
373+ # Calculate center in unwrapped space, then convert to raw (0-4095)
374+ center_unwrapped = div ( data . min_unwrapped + data . max_unwrapped , 2 )
375+ center_raw = Integer . mod ( center_unwrapped , @ steps_per_revolution )
376+
377+ # Firmware applies: Present_Position = Actual_Position - Offset
378+ # So: 2048 = center_raw - offset, therefore offset = center_raw - 2048
379+ # Clamp to ±2047 (sign_magnitude bit 11 limit).
380+ offset = center_raw - @ center_position
381+ offset = max ( - @ max_offset_magnitude , min ( @ max_offset_magnitude , offset ) )
382+
383+ Mix . shell ( ) . info ( """
384+ #{ format_joint ( name ) } (ID #{ servo_id } ):
385+ Range: #{ range } steps (#{ format_degrees ( steps_to_degrees ( range ) ) } )
386+ Center: #{ center_raw } -> Offset: #{ offset }
387+ """ )
388+
389+ if dry_run do
390+ { name , servo_id , { :ok , % { range: range , center: center_raw , offset: offset } } }
391+ else
392+ case apply_calibration ( pid , servo_id , offset ) do
393+ :ok -> { name , servo_id , { :ok , % { offset: offset } } }
394+ { :error , reason } -> { name , servo_id , { :error , reason } }
395+ end
396+ end
397+ end
398+
389399 defp reset_position_offset ( pid , servo_id ) do
390400 unlock_eeprom ( pid , servo_id )
391401
0 commit comments