PFMENU DOCS MC

From DIDEAS Wiki
Jump to: navigation, search

PF Users Navigation:

Edit

motor controller

  • 1040 : read MC filter #motor_filter_.28PI_control_loop.29
  • 1041 : set MC filter
  • 1054 : get MR or MILE setting
  • 1055 : set MR or MILE mode
  • 1100 : (REMOVED) set mode
  • 1101 : get mode
  • 1102 : (REMOVED) set torque
  • 1102 :
    • 0 : sensors.raw_mile_position;
    • 1 : sensors.raw_aas_position;
    • 2 : sensors.raw_ballscrew_encoder;
    • 3 : IOENC_GET_COUNT_M(data->param+3); // incremental encoder
  • 1103 : get torque
  • 1104 : set speed
  • 1105 : get speed (and other info)
  • 1106 : get encoder
    • 0 : TKMOT_GET_ENCODER_M()
    • 1 : incremental encoder
    • 2 : ballscrew_encoder
    • 3 : abs_enc_get_encoder()
    • 5 : raw_aas_position
    • 8 : raw_mile_position
    • 7,9 - depends on in_param
  • 1107 : set encoder zero
  • 1108 : set current offsets
  • 1109 : set motor mode
  • 1110 : set motor torque
  • 1111 : getdata
  • 1112 : REMOVED
  • 1113 : enable / disable mode zero current offset calibration (
  • 1114 : set ctrl param vQ and vD
  • 1115 : set ctrl param vQ and vD, and then set motor mode
    • cmd 1115 is the primary to set motor torque and mode.
    • parameters are 1115 <qPq> <qPd> <Mode>
    • Modes are :
      • 0 for shorted motor leads (safety mode)
      • 2 for zero encoder
      • 3 for close current loop torque mode
      • 4 for open loop voltage mode w/constant flux angle (adjust w/ set_position)
      • 6 for open loop voltage mode w/flux angle set by encoder (normal mode)
      • 7 to read and set motor controller current offsets


    • qPq and qPd are q15 format control reference parameters in the Q and D direction.
      • in modes (2?) and 3 - the Q and D parameters represent current with 1amp = ~847 (this number updated 12/11/2012)
      • in mode 4 and 6 - the Q and D parameters represent voltage such that voltage on the motor is : Vq = qPq / 32768 * POWER SUPPLY VOLAGE
      • eg: a parameter of 800 with a supply of 21.6 volts -> vmot = .8/32.7 * 21.6 = 0.5 volts


  • 1116 : read current offsets
    • typically auto set by commanding mode 7.
  • 1117 : set motor filter (both Q and D)
  • 1118 : read ADCs (in_param==16 -> read all 16 channels), else thats the channel offset and read 10 channels

motor filter (PI control loop)

  • 1040 - get PI control parameters

reads back the actual values of Kp, Ki, Kc, and qV max for "D" and for "Q"

    • values 0-3 are for "D"
    • values 4-7 are for "Q"
    • typical values : 750 1500 16000 16000
    • note these are q14 numbers, thus their max values is 16383
  • 1041 - set PI control parameters

Set the values of both the "D" and "Q" PI control loop (using traditional q15 scaling)

    • typical values are twice those read back
    • set defaults with "1041 1500 3000 32000 32000"
    • the final value limits the maximum voltage applied to the motor as follows:
    • PWM duty cycle max = 100% * <final value>/32768
    • in terms of volts: Max motor voltage = Battery voltage * <final value>/32768

force fields

There are two force fields : #1 protects the thrust bearing (motor encoder space), and #2 the K3 (hall space).

capture ballnut motor encoder

To determine the FF motor encoder value for the ballnut, the robot needs to be move to the largest PP angle. Do this as follows:

  • Boot the robot at the K3 engagement angle
  • move to a do nothing state in the SM statemachine w/spec 170 77 17
  • move to a do nothing state in the MC statemachine w/spec 1421 17
  • switch the MC to torque mode w/spec 1115 0 0 3
  • move the robot to the maximum angle
  • apply -1 amp w/spec 1115 -1000 0 3
  • read the motor encoder w/spec 1106. The 'v0' parameter is the motor encoder value. This will be called the "hard stop value"
  • short MC leads : w/spec 1115 0
  • return robot to normal (zero angle) position - this is needed to avoid the jump when enter ES
  • restore the MC and SM robot state to w/spec 1421 30 and spec 170 77 3

simpler version of above

  • Boot the robot at the K3 engagement angle
  • Reduce the ES K and B to near zero
  • manually move the robot to the maximum PP angle
  • read the motor encoder w/spec 1106. The 'v0' parameter is the motor encoder value. This will be called the "hard stop value"


set the ballnut force field

Appx 1/2 motor turn (1000 counts) before the hardstop, is the "activation position".

  • Set the BNFF w/spec 1447 1 <hardlimit> <activation> <mNm torque at stop> <damping> <enable>
  • example for testing FF : 1447 1 15000 14000 500 0 1
  • example for S2  : 1447 1 31000 30000 500 0 1
  • after the value is set, the global FF needs to be enabled w/spec 1445 1. Make certain robot is in a valid position (outside of the FF protected zone), else the robot will violently lurch to escape the forbidden zone.

to test the ballnut FF

The robot will be in ES. Reduce the ES K and B so that the robot can be freely moved to cause a large PP angle. When the FF is enabled, the current will rapidly increase once inside the FF zone.

to capture the K3 protection value

It is actually not possible to move the robot to the value needed for protecting K3. This must be determinate from the calibration graph. However the hall sensor can be read w/spec command 1516. See the value 'v2'.

set the K3 force field

  • an example for TESTING S2 is: 1447 2 -1500 500 -200 0 1
    • this should keep the rollers from making contact when robot is torqued by hand

overload current monitor

  • 1120 - reads overload config and status:

0 to 1 : motor_max_sum, motor_decay 2 to 3 : present motor_phase_A_sum, motor_phase_A_overload_status 4 to 5 : present motor_phase_B_sum, motor_phase_B_overload_status 6 to 7 : battery_max_sum, battery_decay 8 to 9 : present battery_sum, battery_overload_status

  • 1121 <param> - like command 1120, but pass param==1 to reset sums and trips (eg clear overloads). Also needs to reset MC statemachine out of state 70.
  • 1122 <4x params> - use to set the overload config parameters
    • format : 1122 <motor maxsum> <bat maxsum> <motor decay> <bat decay>
  • The computation at 1KHZ, sums the ABS of the the scaled (1/256) Q15 current and subtracts the decay value from the sum. If the sum exceeds the max sum then there is an overload and the MC statemachine jumps to state 70, and various status flags are set (readable with command 1120).
  • As 1 amp is a q15 of 1093 (or 947), then a decay of 10 -> a q15 decay rate of 2560 or about 2.5 amps. Any current value larger than the decay will EVENTUALLY cause an overload.
  • the time in mS given a current for the overload to trip:

t = (1mS) * MAX_SUM / (q15_current/256 - DECAY)

  • for example with max_sum = 21000, decay=20
    • any current less than (20*256)/1093 [4.7amps] never trips
    • 8 amps trips in 1.5 secs: 1e-3 * 21000/((8*1093)/256 - 20)
    • 16 amps trips in 0.43 secs: 1e-3 * 21000/((16*1093)/256 - 20)
    • 24 amps trips in 0.25 secs: 1e-3 * 21000/((24*1093)/256 - 20)
    • 30 amps trips in 0.19 secs: 1e-3 * 21000/((30*1093)/256 - 20)
  • expected currents (based on a typical cycle)
    • 30 amps, 80mS
    • 25 amps, 100mS
    • 20 amps, 132mS
    • 15 amps, 195mS
    • 10 amps, 373mS
  • the decay to zero is also important - this is the time that given a sum "near" overload, it will take for the sum to drop to zero.
    • time_to_zero = 1mS * max_sum/decay
    • clearly if this time is greater than a cycle, then it is possible for the sum to grow cycle to cycle and trip at a later time.

behavior at overload

  • as the overload sum approach the limit :
    • at 80% the MC current is scaled back down at the rate of 1% per MS
    • at 90% the MC statemachine jumps to a safety impedance state (thus the robot servos to the zero position if there is time.)
    • at 100% the MC statemachine jumps to state 70, and shorts the motor leads
  • to see the MC SM state, special command 1420

reset the overload

To clean the overload:

  • first reset the status flags with special command "1121 1"
  • jump to the MC statemachine to a different state "1421 17" is an idle state, "1421 30" is imped
  • jump the SC statemachine to the desired state "170 77 3" is ES, "170 77 17" is do nothing.

encoder

  • 1050 : get 2x / 4x mode
  • 1051 : set 2x / 4x mode (in_param == 2 or 4)
  • 1052 : get direction
  • 1053 : set direction (in_param == +1 or -1)
  • 1055 : config for MR (0) or MILE encoder (1)

instant on

  • 1490
       data->param[0] = sensors.raw_aas_position;
       data->param[1] =  sensors.raw_mile_position;
       data->param[2] = ankle_angle_sensor_rotations * 360;
       data->param[3] = motor_sub_rev * 10.0;
       data->param[4] = (est_motor_rotations - motor_sub_rev) * 10;
       data->param[5] = (est_motor_rotations - motor_sub_rev) * 2048;
       data->param[6] = int_rot;
       data->param[7] = int_rot*2048 + motor_sub_rev*2048;
       data->param[8] = inc_enc*10.0/2048.0;
       data->param[9] = inc_enc;
  • 1498 41 1
UINT16_T magic;    // last word of first long    
UINT16_T mile_encoder_at_true_north;
UINT16_T mile_encoder_comm_ref;
UINT16_T mile_encoder_resolution;
UINT16_T aas_sensor_at_true_north;
UINT16_T aas_encoder_resolution_msb; // upper 16 bits of a 24 bit encoder resolution
FLOAT32_T theta_beta_poly[3];

dataport

  • 1210 - (to be replaced w/ 1228) get baud
  • 1211 - (to be repalced w/ 1229) set baud (1,2,4,8,10)
  • 1228 : set current dataport baudrate (port)
  • 1229 : set current dataport baudrate (port, value) value is actual baud, will return actual baud
  • 1251 - set read back data variable
    • [data_position] [variable_id]
    • the data position is 0 to 4, and variable id is from this list
  • 1257 - dataport_set_select(val)
    • 0 : received values from SC
    • 1 : electronic fuse sums
    • 2 : all 5 SM values

IMPED

  • 1330 - get imp
  • 1331 - set imp
  • 1333 - set mc_sm to allow imped updates
  • 1340 - impdebug -> called imp_debug_params
  param[0] = imp.pos;
  param[1] = imp.rel_pos;
  param[2] = imp.speed;

  param[4] = imp.kp*1e6;
  param[5] = imp.kb*1e6;
  param[6] = imp.kp_enc*1e6;
  param[7] = imp.kb_rpm*1e6;

MC SM

  • 1411 - mc_sm debug
    • 0 mc_sm_debug
    • 1 mc_sm_ctrl_debug
    • 2 mc_imp_debug / mc_imp_debug_param+4
  • 1420 - get motor controller state
  • 1421 - set motor controller state
  • 1422 - set ankle error state
  • 1430 - get max pos, max neg current (mA)
  • 1431 - set max pos, max neg current (mA) (disable torque limit)
  • 1432 - get torque limit : max pos, max neg torque
  • 1433 - set torque limit : max positive, max negative torque
  • 1435 - set max and min position
  • 1437 - set tics timeout (for torque offset)
  • 1439 - set max pos, max neg current (mA) (enable torque limit)
  • 1440 - get ff params
  • 1441 - set ff params
  • 1442 - return ff debug params
  • 1443 - enable/disable ff
  • 1460
  • 1461 - set IMP torque offset
  • 1464 (disabled) - switch to mode4, command a qVq and qVd
  • 1466 (disabled) - switch to mode6, command a qVq and qVd
  • 1468 - get resistance pulse and duration
  • 1469 - set resistance pulse amplitude and duration

debug (1411) options

    • 0 mc_sm_debug
  param[0] = sm->state;
  param[1] = sm->last_state;
  param[2] = sm->tics_in_current_state;
  param[3] = sm->tics_since_last_update;
  param[4] = sm->motor_is_active;
    • 1 mc_sm_ctrl_debug
  param[0] = ctrl->torque * 1000;
  param[1] = ctrl->current * 1000;
  param[2] = ctrl->qIq;
  param[3] = ctrl->qId;
  param[4] = ctrl->max_pos_current;
  param[5] = ctrl->max_neg_current;
  param[6] = ctrl->max_motor_enc_pos;
  param[7] = ctrl->max_motor_enc_pos;
  param[8] = ctrl->torque_offset * 1000;
  param[9] = ctrl->enable_imp_current_update;
    • 2 mc_imp_debug / mc_imp_debug_param+4
  param[0] = imp.pos;
  param[1] = imp.rel_pos;
  param[2] = imp.speed;

  param[4] = imp.kp*1e6;
  param[5] = imp.kb*1e6;
  param[6] = imp.kp_enc*1e6;
  param[7] = imp.kb_rpm*1e6;


    • case 5 : 
	 data->param[0] = ff->hard_limit;                        // CMD_DOC:
	 data->param[1] = ff->activate_pos;                        // CMD_DOC:
	 data->param[2] = ff->torque_at_limit * 1000;                        // CMD_DOC:
	 data->param[3] = ff->zone;                        // CMD_DOC:
	 data->param[4] = ff->pos_scale_factor * 1e6;                        // CMD_DOC:
	 data->param[5] = ff->scale_factor * 1e9;                        // CMD_DOC:
	 data->param[6] = ctrl->torque * 1000;
	 data->param[7] = ctrl->current * 1000;
	 data->param[8] = ctrl->qIq;
	 data->param[9] = enc_pos;
	 data->param[9] = mc_filter.hall;
    • case 6 : 
	 data->param[0] = ff->hard_limit;                        // CMD_DOC:
	 data->param[1] = ff->activate_pos;                        // CMD_DOC:
	 data->param[2] = ff->torque_at_limit * 1000;                        // CMD_DOC:
	 data->param[3] = ff->zone;                        // CMD_DOC:
	 data->param[4] = ff->pos_scale_factor * 1e6;                        // CMD_DOC:
	 data->param[5] = ff->scale_factor * 1e9;                        // CMD_DOC:
	 data->param[6] = ctrl->torque * 1000;
	 data->param[7] = ctrl->current * 1000;
	 data->param[8] = ctrl->qIq;
	 data->param[9] = mc_filter.hall;
    • case 7 : 
	 int i;
	 extern INT32_T debug_ary[10];
	 for(i=0;i<10;i++) data->param[i] = debug_ary[i];

states

(see mc_statemachine_states.h)

  • cmd 1420, 1421 to access, write state respectively
typedef enum {
  MCSM_INIT, 
  MCSM_IDLE, 

  MCSM_MODE7,
  MCSM_MODE2,
  MCSM_MODE4A,
  MCSM_MODE4B,
  MCSM_MODE4C,
  MCSM_MODE4D,

  MCSM_MODE4_FIN,
  MCSM_BOOT_COMPLETE=20, 

  MCSM_IMP_MODE=30,
  MCSM_RESISTANCE_MODE,
  MCSM_ACTIVE, 
  MCSM_OVERLOAD, 
  MCSM_UPDATE_TIMEOUT,
  MCSM_ANKLE_READY,

  MCSM_RELAXED_MODE, // 36

  MCSM_ANKLE_ERROR=40,
  MCSM_ERROR=44,
  MCSM_POS_ERROR_POSITIVE=50,
  MCSM_POS_ERROR_NEGATIVE=52,
  MCSM_ENC_ERROR=60,

} MC_STATE_T;

encoder

  • 1050 : get encoder X mode and QEI register
  • 1051 : send encoder X mode (2 or 4)
  • 1052 : get direction
  • 1053 : set direction (-1 or +1)
  • 1055 : select MR or MILE encoder (0=MR, 1=MILE)
  • 1056 : read abs encoder debug parameters as updated in ISR
 param[0] = abs_enc.enc_position;
 param[1] = abs_enc.num_rotations;
 param[2] = abs_enc.enc_position + (INT32_T) abs_enc.num_rotations * (INT32_T)abs_enc.resolution;
 param[3] = ((long)abs_enc.last_diff*10000L) + abs_enc.sequential_errors;
 param[4] = abs_enc.zero_mech_ang_measured;
 param[5] = abs_enc.zero_mech_ang_stored;
 param[6] = abs_enc.resolution;
 param[7] = abs_enc.half_resolution;
 param[7] = abs_enc.max_encoder_error;
 param[8] = abs_enc.incremental_encoder_offset; 
 param[9] = abs_enc.invalid_count;
  • 1057 : set the present motor position to be the mechanical zero
  • 1059 : set the max encoder error

trajectory mode

The goal position is smoothly adjusted and is used as the set point position for a virtual spring.

  • Entered by setting the MC statemachine to state 41

trajectory-mode commands

  • 1304, 1305 : get/set the tau rescale factor
  • 1306, 1307 : get/set the auto exit tics
  • 1308, 1309 : get/set the swing trajectory mode target position
  • 1310 : get trajectory control debug
  • 1311 : run one time step of the trajectory controller (runs at 1khz when called from MC statemachine)
  • 1312 : get trajectory imp control debug
  • 1313 : appears return the torque that would be commanded, along with debug params
  • 1314, 1315 : get/set tau in 100uS units
  • 1316, 1317 : get/set impedance Kp, Kb
  • 1318 : set the goal position in joint ankle encoder units (typically 0 to 500)
  • 1319 : sub command 0 : set the initial conditions
  • 1319 : sub command 1 : set the positive and negative current limits in MA
  • 1322 : get the joint ankle encoder offset and its present position
  • 1323 : set the joint ankle encoder offset to its present position

example

Python examples

  • First prepare the robot hardware for outside control
  • probe the present state of the trajectory controller
    • read tau with 1314 : units are 100uS
    • get the imped values w/1316
  • set goal position to 100 : 1318 100
  • set current limits to safe values (+2.5, -2.5 amps) : 1319 1 2500 2500
  • set tau to 100 mS : 1315 1000
  • set the imped controller to soft values : 1317 100 10
  • place the MC statemachine into trajectory controller mode : 1421 41
    • return to idle state with 1421 17

look at the source

The best way to understand how these commands work is to look at two files: 

MotorController\tk_cmdset.c          search for "case 1310" to get into the middle of the code.
MotorController\trajectory_ctrl.c    many of the commands directly call functions in this file
Retrieved from "https://mediawiki.dideas.com/index.php?title=PFMENU_DOCS_MC&oldid=7231"