y2018/control_loops/python/arm_proximal.py

#!/usr/bin/python3

# This code was used to select the gear ratio for the proximal arm.
# Run it from the command line and it displays the time required
# to move the proximal arm 180 degrees from straight down to straight up.
# 
# Michael Schuh
# January 20, 2018

import math
import numpy
import scipy.integrate

# apt-get install python-scipy python3-scipy python-numpy python3-numpy

pi = math.pi
pi2 = 2.0*pi
rad_to_deg = 180.0/pi
inches_to_meters = 0.0254
lbs_to_kg = 1.0/2.2
newton_to_lbf = 0.224809
newton_meters_to_ft_lbs = 0.73756
run_count = 0
theta_travel = 0.0

def to_deg(angle):
  return (angle*rad_to_deg)

def to_rad(angle):
  return (angle/rad_to_deg)

def to_rotations(angle):
  return (angle/pi2)

def time_derivative(x, t, voltage, c1, c2, c3):
  global run_count
  theta, omega = x
  dxdt = [omega, -c1*omega + c3*math.sin(theta) + c2*voltage]
  run_count = run_count + 1

  #print ('dxdt = ',dxdt,' repr(dxdt) = ', repr(dxdt))
  return dxdt

def get_distal_angle(theta_proximal):
  # For the proximal angle = -50 degrees, the distal angle is -180 degrees
  # For the proximal angle =  10 degrees, the distal angle is  -90 degrees
  distal_angle = to_rad(-180.0 - (-50.0-to_deg(theta_proximal))*(180.0-90.0)/(50.0+10.0))
  return distal_angle

def get_distal_omega(omega_proximal):
  # For the proximal angle = -50 degrees, the distal angle is -180 degrees
  # For the proximal angle =  10 degrees, the distal angle is  -90 degrees
  distal_angle = omega_proximal*( (180.0-90.0) / (50.0+10.0) )
  return distal_angle
  

def get_180_degree_time(c1,c2,c3,voltage,gear_ratio,motor_free_speed):
  #print ("# step     time    theta    angular_speed   angular_acceleration  theta   angular_speed  motor_speed motor_speed_fraction")
  #print ("#          (sec)   (rad)      (rad/sec)        (rad/sec^2)      (rotations) (rotations/sec)    (rpm)   (fraction)")
  global run_count
  global theta_travel

  if ( False ):
    # Gravity is assisting the motion.
    theta_start = 0.0
    theta_target = pi
  elif ( False ):
    # Gravity is assisting the motion.
    theta_start = 0.0
    theta_target = -pi
  elif ( False ):
    # Gravity is slowing the motion.
    theta_start = pi
    theta_target = 0.0
  elif ( False ):
    # Gravity is slowing the motion.
    theta_start = -pi
    theta_target = 0.0
  elif ( True ):
    # This is for the proximal arm motion.
    theta_start = to_rad(-50.0)
    theta_target = to_rad(10.0)

  theta_half = 0.5*(theta_start + theta_target)
  if (theta_start > theta_target):
    voltage = -voltage
  theta = theta_start
  theta_travel = theta_start - theta_target 
  if ( run_count == 0 ):
    print ("# Theta Start = %.2f radians End = %.2f Theta travel %.2f Theta half = %.2f Voltage = %.2f" % (theta_start,theta_target,theta_travel,theta_half, voltage))
    print ("# Theta Start = %.2f degrees End = %.2f Theta travel %.2f Theta half = %.2f Voltage = %.2f" % (to_deg(theta_start),to_deg(theta_target),to_deg(theta_travel),to_deg(theta_half), voltage))
  omega = 0.0
  time = 0.0
  delta_time = 0.01 # time step in seconds
  for step in range(1, 5000):
     t = numpy.array([time, time + delta_time])
     time = time + delta_time
     x = [theta, omega]
     angular_acceleration = -c1*omega + c2*voltage
     x_n_plus_1 = scipy.integrate.odeint(time_derivative,x,t,args=(voltage,c1,c2,c3))
     #print ('x_n_plus_1 = ',x_n_plus_1)
     #print ('repr(x_n_plus_1) = ',repr(x_n_plus_1))
     theta, omega = x_n_plus_1[1]
     #theta= x_n_plus_1[0]
     #omega = x_n_plus_1[1]
     if ( False ):
       print ("%4d  %8.4f %8.2f          %8.4f          %8.4f    %8.3f    %8.3f     %8.3f      %8.3f" % \
         (step, time, theta, omega, angular_acceleration, to_rotations(theta), \
         to_rotations(omega), omega*gear_ratio*60.0/pi2, omega*gear_ratio/motor_free_speed ))
     if (theta_start < theta_target):
       # Angle is increasing through the motion.
       if (theta > theta_half):
         break
     else:
       # Angle is decreasing through the motion.
       if (theta < theta_half):
         break
       
  #print ("# step     time    theta    angular_speed   angular_acceleration  theta   angular_speed  motor_speed motor_speed_fraction")
  #print ("#          (sec)   (rad)      (rad/sec)        (rad/sec^2)      (rotations) (rotations/sec)    (rpm)   (fraction)")
  #print ("# Total time for 1/2 rotation of arm is %0.2f" % (time*2))
  return (2.0*time)

def main():
  global run_count
  gravity = 9.8 # m/sec^2 Gravity Constant
  voltage_nominal = 12 # Volts
  
  # Vex 775 Pro motor specs from http://banebots.com/p/M2-RS550-120
  motor_name = "Vex 775 Pro motor specs from http://banebots.com/p/M2-RS550-120"
  current_stall = 134 # amps stall current
  current_no_load = 0.7 # amps no load current
  torque_stall = 710/1000.0 # N-m Stall Torque
  speed_no_load_rpm = 18730 # RPM no load speed
  
  if ( False ):
    # Bag motor from https://www.vexrobotics.com/217-3351.html
    motor_name = "Bag motor from https://www.vexrobotics.com/217-3351.html"
    current_stall = 53.0 # amps stall current
    current_no_load = 1.8 # amps no load current
    torque_stall = 0.4 # N-m Stall Torque
    speed_no_load_rpm = 13180.0 # RPM no load speed
  
  if ( True ):
    # Mini CIM motor from https://www.vexrobotics.com/217-3371.html
    motor_name = "Mini CIM motor from https://www.vexrobotics.com/217-3371.html"
    current_stall = 89.0 # amps stall current
    current_no_load = 3.0 # amps no load current
    torque_stall = 1.4 # N-m Stall Torque
    speed_no_load_rpm = 5840.0 # RPM no load speed

  # How many motors are we using?
  num_motors = 1

  # Motor values
  print ("# Motor: %s" % (motor_name))
  print ("# Number of motors: %d" % (num_motors))
  print ("# Stall torque: %.1f n-m" % (torque_stall))
  print ("# Stall current: %.1f amps" % (current_stall))
  print ("# No load current: %.1f amps" % (current_no_load))
  print ("# No load speed: %.0f rpm" % (speed_no_load_rpm))
  
  # Constants from motor values
  resistance_motor = voltage_nominal/current_stall 
  speed_no_load_rps = speed_no_load_rpm/60.0 # Revolutions per second no load speed
  speed_no_load = speed_no_load_rps*2.0*pi
  Kt = num_motors*torque_stall/current_stall # N-m/A torque constant
  Kv_rpm = speed_no_load_rpm /(voltage_nominal - resistance_motor*current_no_load)  # rpm/V
  Kv = Kv_rpm*2.0*pi/60.0 # rpm/V
  
  # Robot Geometry and physics
  length_proximal_arm = inches_to_meters*47.34 # m Length of arm connected to the robot base
  length_distal_arm = inches_to_meters*44.0 # m Length of arm that holds the cube
  mass_cube = 6.0*lbs_to_kg  # Weight of the cube in Kgrams
  mass_proximal_arm = 5.5*lbs_to_kg # Weight of proximal arm
  mass_distal_arm = 3.5*lbs_to_kg # Weight of distal arm
  mass_distal = mass_cube + mass_distal_arm
  mass_proximal = mass_proximal_arm + mass_distal
  radius_to_proximal_arm_cg = 22.0*inches_to_meters # m Length from arm pivot point to arm CG
  radius_to_distal_arm_cg = 10.0*inches_to_meters # m Length from arm pivot point to arm CG

  radius_to_distal_cg = ( length_distal_arm*mass_cube + radius_to_distal_arm_cg*mass_distal_arm)/mass_distal
  radius_to_proximal_cg = ( length_proximal_arm*mass_distal + radius_to_proximal_arm_cg*mass_proximal_arm)/mass_proximal
  J_cube = length_distal_arm*length_distal_arm*mass_cube 
  # Kg m^2 Moment of inertia of the proximal arm
  J_proximal_arm = radius_to_proximal_arm_cg*radius_to_proximal_arm_cg*mass_distal_arm 
  # Kg m^2 Moment of inertia distal arm and cube at end of proximal arm.
  J_distal_arm_and_cube_at_end_of_proximal_arm = length_proximal_arm*length_proximal_arm*mass_distal 
  J_distal_arm = radius_to_distal_arm_cg*radius_to_distal_arm_cg*mass_distal_arm # Kg m^2 Moment of inertia of the distal arm
  J = J_distal_arm_and_cube_at_end_of_proximal_arm + J_proximal_arm # Moment of inertia of the arm with the cube on the end

  error_margine = 1.0
  voltage = 10.0 # voltage for the motor.  Assuming a loaded robot so not using 12 V.
  # It might make sense to use a lower motor frees peed when the voltage is not a full 12 Volts.
  # motor_free_speed = Kv*voltage
  motor_free_speed = speed_no_load
  
  print ("# Kt = %f N-m/A\n# Kv_rpm = %f rpm/V\n# Kv = %f radians/V" % (Kt, Kv_rpm, Kv))
  print ("# %.2f Ohms Resistance of the motor " % (resistance_motor))
  print ("# %.2f kg Cube weight" % (mass_cube))
  print ("# %.2f kg Proximal Arm mass" % (mass_proximal_arm))
  print ("# %.2f kg Distal Arm mass" % (mass_distal_arm))
  print ("# %.2f kg Distal Arm and Cube weight" % (mass_distal))
  print ("# %.2f m Length from distal arm pivot point to arm CG" % (radius_to_distal_arm_cg))
  print ("# %.2f m Length from distal arm pivot point to arm and cube cg" % (radius_to_distal_cg))
  print ("# %.2f kg-m^2 Moment of inertia of the cube about the arm pivot point" % (J_cube))
  print ("# %.2f m Length from proximal arm pivot point to arm CG" % (radius_to_proximal_arm_cg))
  print ("# %.2f m Length from proximal arm pivot point to arm and cube cg" % (radius_to_proximal_cg))
  print ("# %.2f m  Proximal arm length" % (length_proximal_arm))
  print ("# %.2f m  Distal arm length" % (length_distal_arm))

  print ("# %.2f kg-m^2 Moment of inertia of the distal arm about the arm pivot point" % (J_distal_arm))
  print ("# %.2f kg-m^2 Moment of inertia of the proximal arm about the arm pivot point" % (J_proximal_arm))
  print ("# %.2f kg-m^2 Moment of inertia of the distal arm and cube mass about the proximal arm pivot point" % (J_distal_arm_and_cube_at_end_of_proximal_arm))
  print ("# %.2f kg-m^2 Moment of inertia of the proximal arm and distal arm and cube about the arm pivot point" % (J))
  print ("# %d Number of motors" % (num_motors))
  
  print ("# %.2f V Motor voltage" % (voltage))

  print ("\n# Min time is for proximal arm without any forces from distal arm.  Max time puts all distal arm mass at the end of proximal arm.")

  for gear_ratio in range(60, 241, 10):
    c1 = Kt*gear_ratio*gear_ratio/(Kv*resistance_motor*J)
    c2 = gear_ratio*Kt/(J*resistance_motor)
    c3 = radius_to_proximal_cg*mass_proximal*gravity/J
    c1_proximal_only = Kt*gear_ratio*gear_ratio/(Kv*resistance_motor*J_proximal_arm)
    c2_proximal_only = gear_ratio*Kt/(J_proximal_arm*resistance_motor)
    c3_proximal_only = radius_to_proximal_arm_cg*mass_proximal_arm*gravity/J_proximal_arm
  
    if ( False and run_count < 1 ):
      print ("# %.8f 1/sec C1 constant" % (c1))
      print ("# %.2f 1/sec C2 constant" % (c2))
      print ("# %.2f 1/(V sec^2) C3 constant" % (c3))
      print ("# %.8f 1/sec C1 proximal only constant" % (c1_proximal_only))
      print ("# %.2f 1/sec C2 proximal only constant" % (c2_proximal_only))
      print ("# %.2f 1/(V sec^2) C3 proximal only constant" % (c3_proximal_only))
      print ("# %.2f RPM Free speed at motor voltage" % (voltage*Kv_rpm))
  
    torque_90_degrees = radius_to_proximal_cg*mass_proximal*gravity
    voltage_90_degrees = resistance_motor*torque_90_degrees/(gear_ratio*Kt)
    torque_peak = gear_ratio*num_motors*torque_stall
    torque_peak_ft_lbs = torque_peak * newton_meters_to_ft_lbs
    normal_force = torque_peak/length_proximal_arm
    normal_force_lbf = newton_to_lbf*normal_force 
    normal_distal_arm_end_force = torque_peak/(length_proximal_arm + length_distal_arm)
    normal_distal_arm_end_force_lbf = newton_to_lbf*normal_distal_arm_end_force 
    time_required = get_180_degree_time(c1,c2,c3,voltage,gear_ratio,motor_free_speed)
    time_required_proximal_only = get_180_degree_time(c1_proximal_only,c2_proximal_only,c3_proximal_only,voltage,gear_ratio,motor_free_speed)
    print ("Time for %.1f degrees for gear ratio %3.0f is %.2f (min) %.2f (max) seconds.  90 degree torque %.1f N-m and voltage %.1f Volts. Peak torque %3.0f nm %3.0f ft-lb Normal force at proximal end %3.0f N %2.0f lbf distal end %3.0f N %2.0f lbf" % \
      (to_deg(theta_travel),gear_ratio,time_required_proximal_only,time_required,torque_90_degrees,voltage_90_degrees,
       torque_peak,torque_peak_ft_lbs,normal_force,normal_force_lbf,normal_distal_arm_end_force,normal_distal_arm_end_force_lbf))
  
if __name__ == '__main__':
   main()