prog

def driverProg():
  HOSTNAME = "%(driver_hostname)s"
  PORT = %(driver_reverseport)d
  MSG_OUT = 1
  MSG_QUIT = 2
  MSG_JOINT_STATES = 3
  MSG_MOVEJ = 4
  MSG_WAYPOINT_FINISHED = 5
  MSG_STOPJ = 6
  MSG_SERVOJ = 7
  MSG_SET_PAYLOAD = 8
  MSG_WRENCH = 9
  MSG_SET_DIGITAL_OUT = 10
  MSG_GET_IO = 11
  MSG_SET_FLAG = 12
  MSG_SET_TOOL_VOLTAGE = 13
  MSG_SET_ANALOG_OUT = 14
  MULT_wrench = 10000.0
  MULT_payload = 1000.0
  MULT_jointstate = 10000.0
  MULT_time = 1000000.0
  MULT_blend = 1000.0
  MULT_analog = 1000000.0
  pi = 3.14159265359
  
  def send_out(msg):
    enter_critical
    socket_send_int(MSG_OUT)
    socket_send_string(msg)
    socket_send_string("~")
    exit_critical
  end
  
  def send_waypoint_finished(waypoint_id):
    enter_critical
    socket_send_int(MSG_WAYPOINT_FINISHED)
    socket_send_int(waypoint_id)
    exit_critical
  end
  
  thread statePublisherThread():
    def send_joint_state():
      q = get_joint_positions()
      qdot = get_joint_speeds()
      tau = get_joint_torques()
      enter_critical
      socket_send_int(MSG_JOINT_STATES)
      socket_send_int(floor(MULT_jointstate * q[0]))
      socket_send_int(floor(MULT_jointstate * q[1]))
      socket_send_int(floor(MULT_jointstate * q[2]))
      socket_send_int(floor(MULT_jointstate * q[3]))
      socket_send_int(floor(MULT_jointstate * q[4]))
      socket_send_int(floor(MULT_jointstate * q[5]))
      socket_send_int(floor(MULT_jointstate * qdot[0]))
      socket_send_int(floor(MULT_jointstate * qdot[1]))
      socket_send_int(floor(MULT_jointstate * qdot[2]))
      socket_send_int(floor(MULT_jointstate * qdot[3]))
      socket_send_int(floor(MULT_jointstate * qdot[4]))
      socket_send_int(floor(MULT_jointstate * qdot[5]))
      socket_send_int(floor(MULT_jointstate * tau[0]))
      socket_send_int(floor(MULT_jointstate * tau[1]))
      socket_send_int(floor(MULT_jointstate * tau[2]))
      socket_send_int(floor(MULT_jointstate * tau[3]))
      socket_send_int(floor(MULT_jointstate * tau[4]))
      socket_send_int(floor(MULT_jointstate * tau[5]))
      #socket_send_int(7895160)  # Recognizable  ".xxx" or 00787878
      exit_critical
    end
    #socket_open(HOSTNAME, 50002)
    send_joint_state()
    while True:
      send_joint_state()
      sync()
    end
    sync()
  end

  thread wrench_statePublisherThread():
    def send_wrench_state():
      wrench = get_tcp_force()
      enter_critical
      socket_send_int(MSG_WRENCH)
      socket_send_int(floor(MULT_wrench * wrench[0]))
      socket_send_int(floor(MULT_wrench * wrench[1]))
      socket_send_int(floor(MULT_wrench * wrench[2]))
      socket_send_int(floor(MULT_wrench * wrench[3]))
      socket_send_int(floor(MULT_wrench * wrench[4]))
      socket_send_int(floor(MULT_wrench * wrench[5]))
      exit_critical
    end
    send_wrench_state()
    while True:
      send_wrench_state()
      sync()
    end
    sync()
  end
  
  thread io_statePublisherThread():
    def send_io_state():
      enter_critical
      socket_send_int(MSG_GET_IO)
      socket_send_int(1*get_digital_in(0) + 2*get_digital_in(1) + 4 * get_digital_in(2) + 8 * get_digital_in(3) + 16 * get_digital_in(4) + 32 * get_digital_in(5) + 64 * get_digital_in(6) + 128 * get_digital_in(7) + 256 * get_digital_in(8) + 512 * get_digital_in(9))
      socket_send_int(1*get_digital_out(0) + 2*get_digital_out(1) + 4 * get_digital_out(2) + 8 * get_digital_out(3) + 16 * get_digital_out(4) + 32 * get_digital_out(5) + 64 * get_digital_out(6) + 128 * get_digital_out(7) + 256 * get_digital_out(8) + 512 * get_digital_out(9))
      ####socket_send_int(pow(2,0)*get_flag(0) + pow(2,1)*get_flag(1) + pow(2,2)*get_flag(2) + pow(2,3)*get_flag(3) + pow(2,4)*get_flag(4) + pow(2,5)*get_flag(5) + pow(2,6)*get_flag(6) + pow(2,7)*get_flag(7) + pow(2,8)*get_flag(8) + pow(2,9)*get_flag(9) + pow(2,10)*get_flag(10) + pow(2,11)*get_flag(11) + pow(2,12)*get_flag(12) + pow(2,13)*get_flag(13) + pow(2,14)*get_flag(14) + pow(2,15)*get_flag(15) + pow(2,16)*get_flag(16) + pow(2,17)*get_flag(17) + pow(2,18)*get_flag(18) + pow(2,19)*get_flag(19) + pow(2,20)*get_flag(20) + pow(2,21)*get_flag(21) + pow(2,22)*get_flag(22) + pow(2,23)*get_flag(23) + pow(2,24)*get_flag(24) + pow(2,25)*get_flag(25) + pow(2,26)*get_flag(26) + pow(2,27)*get_flag(27) + pow(2,28)*get_flag(28) + pow(2,29)*get_flag(29) + pow(2,30)*get_flag(30) + pow(2,31)*get_flag(31))
      #socket_send_int(1*get_flag(0) + 2*get_flag(1) + 4*get_flag(2) + 8*get_flag(3) + 16*get_flag(4) + 32*get_flag(5) + 64*get_flag(6) + 128*get_flag(7))
      #socket_send_int(1*get_flag(8) + 2*get_flag(9) + 4*get_flag(10) + 8*get_flag(11) + 16*get_flag(12) + 32*get_flag(13) + 64*get_flag(14) + 128*get_flag(15))
      #socket_send_int(1*get_flag(16) + 2*get_flag(17) + 4*get_flag(18) + 8*get_flag(19) + 16*get_flag(20) + 32*get_flag(21) + 64*get_flag(22) + 128*get_flag(23))
      #socket_send_int(1*get_flag(24) + 2*get_flag(25) + 4*get_flag(26) + 8*get_flag(27) + 16*get_flag(28) + 32*get_flag(29) + 64*get_flag(30) + 128*get_flag(31))
      socket_send_int(get_analog_out(0) * MULT_analog)
      socket_send_int(get_analog_out(1) * MULT_analog)
      socket_send_int(get_analog_in(0) * MULT_analog)
      socket_send_int(get_analog_in(1) * MULT_analog)
      socket_send_int(get_analog_in(2) * MULT_analog)
      socket_send_int(get_analog_in(3) * MULT_analog)
      exit_critical
    end
    
    #temporarily reduce publish rate for io_states
    io_count = 0
    io_slowdown_ratio = 10
    
    send_io_state()
    while True:
      if io_count != io_slowdown_ratio:
        io_count=io_count+1
      else:
        send_io_state()
        io_count=0
      end
      sync()
    end
    sync()
  end
  

  
  SERVO_IDLE = 0
  SERVO_RUNNING = 1
  cmd_servo_state = SERVO_IDLE
  cmd_servo_id = 0  # 0 = idle, -1 = stop
  cmd_servo_q = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
  cmd_servo_dt = 0.0
  def set_servo_setpoint(id, q, dt):
    enter_critical
    cmd_servo_state = SERVO_RUNNING
    cmd_servo_id = id
    cmd_servo_q = q
    cmd_servo_dt = dt
    exit_critical
  end
  thread servoThread():
    state = SERVO_IDLE
    while True:
      # Latches the new command
      enter_critical
      q = cmd_servo_q
      dt = cmd_servo_dt
      id = cmd_servo_id
      do_brake = False
      if (state == SERVO_RUNNING) and (cmd_servo_state == SERVO_IDLE):
        # No command pending
        do_brake = True
      end
      state = cmd_servo_state
      cmd_servo_state = SERVO_IDLE
      exit_critical
      
      #do_brake = False
      
      # Executes the command
      if do_brake:
        #stopj(1.0)  # TODO
        send_out("Braking")
        sync()
      elif state == SERVO_RUNNING:
        servoj(q, 0, 0, dt)
        #send_out("Servoed")
      else:
        #send_out("Idle")
        sync()
      end
    end
  end
  
  socket_open(HOSTNAME, PORT)
  send_out("hello")
  
  thread_state = run statePublisherThread()
  thread_wrench_state = run wrench_statePublisherThread()
  thread_io_state = run io_statePublisherThread()
  thread_servo = run servoThread()
  
  # Servoes in a circle
  #movej([1.5,-0.4,-1.57,0,0,0], 3, 0.75, 1.0)
  #t = 0
  #while True:
  #  q = [1.5,0,-1.57,0,0,0]
  #  q[0] = q[0] + 0.2 * sin(0.25 * t*(2*pi))
  #  q[1] = q[1] - 0.2 + 0.2 * cos(0.25 * t*(2*pi))
  #  #servoj(q, 3, 1, 0.08)
  #  #send_out("servoed")
  #  set_servo_setpoint(t, q, 0.08)
  #  t = t + 0.08
  #  sleep(0.08)
  #end
  
  while True:
    #send_out("Listening")
    ll = socket_read_binary_integer(1)
    
    if ll[0] == 0:
      #send_out("Received nothing")
    elif ll[0] > 1:
      send_out("Received too many things")
    else:
      mtype = ll[1]
      if mtype == MSG_QUIT:
        send_out("Received QUIT")
        break
      elif mtype == MSG_MOVEJ:
        send_out("Received movej")
        params_mult = socket_read_binary_integer(1+6+4)
        if params_mult[0] == 0:
          send_out("Received no parameters for movej message")
        end
        
        # Unpacks the parameters
        waypoint_id = params_mult[1]
        q = [params_mult[2] / MULT_jointstate,
                   params_mult[3] / MULT_jointstate,
                   params_mult[4] / MULT_jointstate,
                   params_mult[5] / MULT_jointstate,
                   params_mult[6] / MULT_jointstate,
                   params_mult[7] / MULT_jointstate]
        a = params_mult[8] / MULT_jointstate
        v = params_mult[9] / MULT_jointstate
        t = params_mult[10] / MULT_time
        r = params_mult[11] / MULT_blend
        
        # Sends the command
        send_out("movej started")
        movej(q, a, v, t, r)
        send_waypoint_finished(waypoint_id)
        send_out("movej finished")
      elif mtype == MSG_SERVOJ:
        # Reads the parameters
        params_mult = socket_read_binary_integer(1+6+1)
        if params_mult[0] == 0:
          send_out("Received no parameters for movej message")
        end
        
        # Unpacks the parameters
        waypoint_id = params_mult[1]
        q = [params_mult[2] / MULT_jointstate,
                   params_mult[3] / MULT_jointstate,
                   params_mult[4] / MULT_jointstate,
                   params_mult[5] / MULT_jointstate,
                   params_mult[6] / MULT_jointstate,
                   params_mult[7] / MULT_jointstate]
        t = params_mult[8] / MULT_time
        
        # Servos
        #servoj(q, 3, 0.1, t)
        #send_waypoint_finished(waypoint_id)
        set_servo_setpoint(waypoint_id, q, t)
      elif mtype == MSG_SET_PAYLOAD:
        params = socket_read_binary_integer(1)
        if params[0] == 0:
          send_out("Received no parameters for setPayload message")
        end
        
        payload = params[1] / MULT_payload
        #send_out(payload)
        send_out("Received new payload")
        set_payload(payload)
      elif mtype == MSG_STOPJ:
        send_out("Received stopj")
        stopj(1.0)
      elif mtype == MSG_SET_DIGITAL_OUT:
        #send_out("Received Digital Out Signal")
        # Reads the parameters
        params_mult = socket_read_binary_integer(2)
        if params_mult[0] == 0:
          send_out("Received no parameters for set_digital_out message")
        end
        if params_mult[2] > 0:
           set_digital_out(params_mult[1], True)
        elif params_mult[2] == 0:
          set_digital_out(params_mult[1], False)
        end
      elif mtype == MSG_SET_FLAG:
        #send_out("Received Set Flag Signal")
        # Reads the parameters
        params_mult = socket_read_binary_integer(2)
        if params_mult[0] == 0:
          send_out("Received no parameters for set_flag message")
        end
        if params_mult[2] != 0:
          set_flag(params_mult[1], True)
        elif params_mult[2] == 0:
          set_flag(params_mult[1], False)
        end
      elif mtype == MSG_SET_ANALOG_OUT:
        #send_out("Received Analog Out Signal")
        # Reads the parameters
        params_mult = socket_read_binary_integer(2)
        if params_mult[0] == 0:
          send_out("Received no parameters for set_analog_out message")
        end
        set_analog_out(params_mult[1], (params_mult[2] / MULT_analog))
      elif mtype == MSG_SET_TOOL_VOLTAGE:
        #send_out("Received Tool Voltage Signal")
        # Reads the parameters (also reads second dummy '0' integer)
        params_mult = socket_read_binary_integer(2)
        if params_mult[0] == 0:
          send_out("Received no parameters for set_tool_voltage message")
        end
        set_tool_voltage(params_mult[1])
      else:
        send_out("Received unknown message type")
      end
    end
  end
  
  #sleep(1)
  kill thread_state
  kill thread_wrench_state
  kill thread_io_state
  socket_send_int(MSG_QUIT)
end
driverProg()