A Distributed Communication System in a Humanoid Robot

1
A Distributed Communication System in a Humanoid
Robot

• Milton Ruas1
• Filipe M. T. Silva1
• Vítor M. F. Santos2

1 Department of Electronics and
Telecommunications 2 Department of Mechanical
Engineering University of Aveiro, PORTUGAL
2
Overview

• Introduction
• Distributed Control System
• The Communications System Model
• PC-Master Communication
• RS-232 communication
• RS-232 messages and protocol
• Master-Slaves communication
• CAN an introduction
• CAN configuration in a PIC18F258
• CAN messages and protocol
• System variables access
• Conclusions and Future perspectives

3
Introduction

• Complete humanoid model
• 22 degrees of freedom
• Weight - 5 kg
• Height - 60 cm
• Max. width - 25 cm
• Foot print - 20 ? 8 (cm2)
• Actuation
• Servomotors with transmission belts
• Sensors
• A vision Camera
• Servos position (through its internal
  potentiometers)
• Sensitive foot to applied forces
• Accelerometers/Inclinometers
• Gyroscopes

4
Local Control

• Each slave controller is made of a PIC 18F258
  device with I/O interfacing
• All slave units
• Connect up to 3 servomotors
• Have a common base (a piggy-back unit can add I/O
  sensors)

5
Distributed Control System

• A Main Control Unit (PC)
• Exchanges high-level orders and interacts with
  the camera.
• RS-232 communication with the Distributer unit.
• Distributer Unit (Master)
• Interface between the Main and the Local control
  units.
• Adapts the RS-232 to CAN commands (and
  vice-versa).
• 8 Local Control Units (Slaves)
• Control the low-level features of the several
  devices.
• CAN bus to connect them.

6
PC-Master Communication

• Communication system RS-232
• Data to be exchanged
• Actuation
• Final position (3 components)
• Mean velocity for the 3 components
• Type of Controller
• Controller gains (21)
• Sensors information
• Servomotors position (3)
• Servomotors mean velocity (3)
• Servomotors current consumption (3)
• Special sensors data (4)

PC
Master
7
RS-232 Communication

• Exchange Model
• Request/Response from Main Unit to Master
• RS-232 Setup
• Asynchronous flow
• 1 start 8 data 1 parity 1 stop bits
• Baudrate 115K2 bps
• Messages
• Start of Frame (SOF)
• OpCode Operation Code
• ...
• BCC EXOR of Full message

PC
Master
1 byte
1 byte
3/4 bytes
1 byte
SOF OpCode BCC
Start Data (8 bits) Parity Stop
Message Structure (bytes)
RS-232 character (bits)
8
RS-232 Messages

Actuation
byte 1 byte 2 byte 3 byte 4 byte 5
SOF0xFF OpCode Joint 1 Joint 2 Joint 3
byte 1 byte 2 byte 3 byte 4 byte 5 byte 6
0xFD 0
Servos
Sensors Reading
byte 1 byte 2 byte 3 byte 4 byte 5
SOF0xFF OpCode 0 0 0
byte 1 byte 2 byte 3 byte 4 byte 5 byte 6
0xFD Joint 1 Joint 2 Joint 3 Status
Potentiom
byte 1 byte 2 byte 3 byte 4 byte 5 byte 6
0xFD Data1 Data2 Data3 Data4
Special
byte 1 byte 2 byte 3 byte 4 byte 5 byte 6
0xFC 0
Invalid Request
9
RS-232 Messages

bit 7 6 5 4 3 2 1 bit 0
Operation Operation SCU ID SCU ID SCU ID SCU ID Parameter Parameter
OpCode
SCU id Operation Parameter
SCU Address OP_APPLY_JOINT (0b00) Mensagem de actuação sobre as três juntas do SCU alvo (SCU id). PARAM_POSITION (0b00) Posição (em graus) a ser atingida.
SCU Address OP_APPLY_JOINT (0b00) Mensagem de actuação sobre as três juntas do SCU alvo (SCU id). PARAM_VELOCITY (0b01) Velocidade média do movimento.
SCU Address OP_APPLY_JOINT (0b00) Mensagem de actuação sobre as três juntas do SCU alvo (SCU id). PARAM_SPECIAL (0b11) Activação/desactivação do PWM aplicado aos motores.
SCU Address OP_APPLY_CONTROL (0b01) Mensagem de actuação sobre as três juntas do SCU alvo com a definição dos parâmetros do controlador PID. PARAM_KI (0b00) Ganho integral do controlador local.
SCU Address OP_APPLY_CONTROL (0b01) Mensagem de actuação sobre as três juntas do SCU alvo com a definição dos parâmetros do controlador PID. PARAM_KP (0b01) Ganho proporcional do controlador local.
SCU Address OP_APPLY_CONTROL (0b01) Mensagem de actuação sobre as três juntas do SCU alvo com a definição dos parâmetros do controlador PID. PARAM_KD (0b10) Ganho do controlador de 1º nível.
SCU Address OP_APPLY_CONTROL (0b01) Mensagem de actuação sobre as três juntas do SCU alvo com a definição dos parâmetros do controlador PID. PARAM_CONTROLON (0b11) Tipo de controlo a aplicar na junta.
SCU Address OP_READ_SENSORS (0b10) Mensagem de leitura dos sensores adicionais (sensores de força dos pés, giroscópio e inclinómetro). PARAM_POSITION (0b00) Posição actual de cada junta.
SCU Address OP_READ_SENSORS (0b10) Mensagem de leitura dos sensores adicionais (sensores de força dos pés, giroscópio e inclinómetro). PARAM_VELOCITY (0b01) Velocidade média de cada junta.
SCU Address OP_READ_SENSORS (0b10) Mensagem de leitura dos sensores adicionais (sensores de força dos pés, giroscópio e inclinómetro). PARAM_CURRENT (0b10) Corrente drenada por cada servo.
SCU Address OP_READ_SENSORS (0b10) Mensagem de leitura dos sensores adicionais (sensores de força dos pés, giroscópio e inclinómetro). PARAM_SPECIAL (0b11) Saída dos sensores especiais.
10
RS-232 Algorithms
11
RS-232 Commands (examples)

• Communications initialization
• Hinitcom(1,115200) COM1, Baud115k2
• Joints Actuation
• Applyjoint(H,2,0,0 0 70) SCU2, pos0 0 70
• Motion velocity
• Applyjoint(H,2,1,100 100 100) Period2 2 2s
• Joints position reading
• Readjoint(H,2,0) ? pos1 pos2 pos3
• Force sensors reading
• ReadSpecial(H,2) ? data1 data2 data3 data4

12
Master-Slaves Communication

• Communication system CAN
• Data to be exchanged
• Actuation
• Final position to reach (3 components)
• Mean velocity (3 components)
• Type of Controller
• Controller gains (21)
• Sensors information
• Servomotors position (3)
• Servomotors mean velocity (3)
• Status of the slave unit
• Servomotors current consumption (3)
• Special sensors data (4)

CAN
x8
13
Communication

• Developed by Robert Bosch in the 1980s
• Initially applied in the automotive industry, but
  soon has expanded to other fields
• Very reliable and Robust
• Easy to add/remove nodes
• Ideal for Real Time Distributed Control
• CAN
• MAC based on CSMA/CR to control collisions
• Message prioritisation
• Broadcast messages
• Guaranteed data consistency
• Acknowledgment
• Fault tolerance
• Error detection and signalization
• Automatic error recovery

14
Stack

• CAN is based on the traditional OSI model
• Only the Physical and Datalink layers are used
• Physical layer
• Bit encoding/decoding
• Bit timing
• Bit synchronization
• Datalink layers
• Logical Link Control (LLC)
• Reception
• Masking/Filtering
• Recovery management
• Medium Access Control (MAC)
• Data encapsuling
• Frame encoding
• Medium Access management
• Error detection and Signalization

15
Physical Layer

• Bit Encoding
• NRZ Non return to zero
• Each bit is composed by several time quantums
• Synchronization
• Bit stuffing 5 bits
• Synchronization between nodes
• Interrupt generation inside the bit time after
  successful RX/TX
• Maximum nodes
• 32, 64, 128... Depends on the line transceivers
• Transmission rate
• From 5kb/s (10km) to 1Mb/s (40m)
• Depends on the time quantums used.

16
Data Link Layer

• Errors signalled in the CAN data frame.

• MAC CSMA/CR
• Non-destructive arbitration at bit level
• In a collision in the access bus, the highest
  priority message is selected without any delay.

17
Versions

• CAN 2.0A (Basic CAN)
• Partial filtering
• RTRs processing and messages acceptance made at
  the application layer
• Only one Buffer for RX, and one for TX
• High overhead at the application level
• CAN 2.0B (Full CAN)
• Total filtering.
• 16 buffers can be programmed for RX/TX
• Low overhead at the application level
• PIC 18F2x8x implements the full CAN version

18
Protocol

CAN ID
bit 10 9 8 7 6 5 4 3 2 1 bit 0
Source address Source address Source address Source address Destination address Destination address Destination address Destination address Index Operation Operation

• CAN ID information
• Source address transmitter node address
• Destination address receiver node address
• Index Index of message (1st or 2nd)
• Type of operation Actuation/Sensor reading
• 2 types of messages exchanged (operation)
• Actuation from Master to Slaves
• Sensor reading from Slaves to Master
• Each message is composed of 2 CAN packets to
  accommodate all the necessary information.
• Prioritized messages
• Actuation messages (from Master to Slaves) have
  higher priority
• Lower address nodes have higher priority
• First packet of each message have higher priority

19
Protocol


CAN ID
bit 10 9 8 7 6 5 4 3 2 1 bit 0
Source address Source address Source address Source address Destination address Destination address Destination address Destination address Index Operation Operation
Controller Unit Section Address
Master Master unit 0b0000
Slave 1 Right Leg 0b0001
Slave 2 Left Leg 0b0010
Slave 3 Right Hip 0b0011
Slave 4 Left Hip 0b0100
Slave 5 Body 0b0101
Slave 6 Right Arm 0b0110
Slave 7 Left Arm 0b0111
Slave 8 Head 0b1000

• Lower Parts (legs and hips) have higher priority
  than others
• Priority defined according to the part importance

20
Protocol


Actuation Master ? Slaves
byte 1 byte 2 byte 3 byte 4 byte 5 byte 6 byte 7 byte 8
Ref1 Ref2 Ref3 Speed1 Speed2 Speed3 Control K3
Msg1
byte 1 byte 2 byte 3 byte 4 byte 5 byte 6 byte 7 byte 8
KI1 KI2 KI3 KP1 KP2 KP3 K1 K2
Msg2
bit 7 bit 6 bit 5 bit 4 bit 3 bit 2 bit 1 bit 0
PWM Calib Cont3 Cont3 Cont2 Cont2 Cont0 Cont0
Sensor reading Slave ? Master
byte 1 byte 2 byte 3 byte 4 byte 5 byte 6 byte 7 byte 8
S.Flags Pos1 Pos2 Pos3 Speed1 Speed2 Speed3 Status
Msg1
byte 1 byte 2 byte 3 byte 4 byte 5 byte 6 byte 7 byte 8
S.Flags Curr1 Curr2 Curr3 Data1 Data2 Data3 Data4
Msg2
bit 7 6 5 4 3 2 1 bit 0
PWM Calib Deadl. Finall Finone Motion finished Motion finished Motion finished
bit 7 6 5 4 3 2 1 bit 0
Source Address Source Address Source Address Source Address Index 0 0 0
21
in PIC 18F258

• PIC 18F258 basic specifications
• Message bitrates up to 1Mbps
• 11/29-bit ID fields (Normal/Extended CAN)
• Up to 8 bytes of message length
• 3 prioritized TX buffers 2 RX buffers
• 6 full prioritized, 29-bit acceptance filters
• Overflow RX prevention
• Advanced error management features
• Configuration of CAN
• Enter in Configuration Mode avoids accidental
  reconfiguration
• Configure Baudrate
• Configure Mask Filters for message acceptance
• How messages should be transmitted/received
• Enter in Normal Mode ready to exchange messages

22
Initial Configuration Baudrate

• Registers to setup
• BRP2 Quanta duration (depends of the crystal
  frequency)
• PRSEG Propagation Segment (1..8 ? 2)
• SEG1PH Phase Segment 1 (1..8 ? 3)
• SEG2PH Phase Segment 2 (2..8 ? 2)
• SJW Synchronization Jump Width (1..4 ? 2)
• SAM Sample Point (sample once)
• Rules to obey
• (Q Sync SegProp SegPhase Seg 1Phase Seg2)
• 8 Q 25 (Selected Q8)
• Sync Seg 1
• Prop SegPhase Seg 1Phase Seg 2 (23 2)
• Phase Seg 2Sync Jump Width (2 2)

23
Initial Configuration Mask Filters

• 2 CAN messages are always exchanged at once
• Use one RX buffer for each one
• RXB0 First message (Index0)
• RXB1 Second message (Index1)
• Master
• Mask Receive all messages destined to master
• 0b00001111100
• Filter
• To RXB0 0bxxxx00000xx
• To RXB1 0bxxxx00001xx
• Slaves
• Mask Receive messages from Master
• 0b11110000100
• Filter
• To RXB0 0b0000xxxx0xx
• To RXB1 0b0000xxxx1xx

Identifier
10 9 8 7 6 5 4 3 2 1 0
Src addr Src addr Src addr Src addr Dst addr Dst addr Dst addr Dst addr Ind Oper Oper
24
Initial Configuration Message TX/RX

• Reception
• Each message is directed to one RX buffer
• RXB0 ? First message (index0)
• RXB1 ? Second message (index1)
• No RX Buffer 0 overflow to RX Buffer 1
• Receive only valid messages with std ID
• Transmission
• Each message will use one TX buffer
• TXB0 ? First message (index0)
• TXB1 ? Second message (index1)
• Prioritize TX buffers
• TXB0 Highest priority
• TXB1 Lowest priority
• Messages will be transmitted with std ID
• Frame Remote TX will not be requested

25
Low-Level RX/TX

26
High-Level RX/TX in the Master

MASTER
27
CAN Management in the Master

• Full Time-triggered CAN to guarantee BW to CAN
  and RS-232 comm
• RS-232 executed in background
• Master initiates always data exchange for each
  slave, avoiding collisions in responses
• Actuation Master ? Slave
• Sensors Master ? Slave
• Timings
• One slave service 1ms
• Turn-around time 8ms (for 8 slaves)

Timer INT
TX 1st Packet (Actuation)
250µs
RX 1st Packet (Sensors)
Abort 1st packet TX
250µs
TX 2nd Packet (Actuation)
250µs
RX 2nd Packet (Sensors)
Abort 2nd packet TX
250µs
Set Next DST addr
28
High-Level RX/TX in the Slaves

SLAVE
29
CAN Management in the Slaves

• Presented Algorithm is applied to each Packet of
  the message (2)
• Master starts communication with a actuation
  message
• Slave responds with a sensor data message
• PWM control can cause Jitter up to 2ms. In this
  case, response is not sent to avoid collisions
• With a 20ms PWM period, one sensor message is
  guaranteed to be sent.

RXBnIF
x2
Abort previous TX
RX i-th Packet (Actuation)
Yes
PWM interference?
Exit
No
TX i-th Packet (Sensors)
30
Data Access in Master/Slaves

Master
Slave
TX
Timer INT
Actuation 1
RXTX
RXB0 INT
RX
Sensors 1
250µs
TX
Actuation 2
RXTX
RXB1 INT
RX
Sensors 2
Slave i
TX
Actuation 1
RXTX
RX
Sensors 1
TX
Actuation 2
RXTX
RX
Sensors 2
Slave i1
31
Data Access in Master/Slaves

• Building messages for TX requires access to
  actuation/sensorial data
• For slaves
• The actuation message provided by the Master is
  unpredictable, and so the sensorial data for the
  response must be available at any time
• For the Master
• Data must be exchanged between the RS-232 and CAN
  communications with minimal mutual interference
• The best solution is to use a global data base
  where all the system variables are available!
• In Master, RS-232 and CAN communications are
  completely independent
• Slaves have access to actuation/sensorial
  information anytime

32
Data Base in Master

• typedef struct // Estrutura descritiva dos
  sensores
• byte sysStatus // Estado do sistema
• struct_servo servoN_SERVOS
  // Sensores dos servos
• unsigned char specialN_SPECIAL_SENSORS //
  Sensores de força
• struct_sensors
• typedef struct // Valores de referência dos
  controladores
• struct_paramControl servoN_SERVOS
• struct_paramControl copN_SERVOS
• struct_paramControl incN_SERVOS
• struct_paramControl giroN_SERVOS
• struct_refControl
• typedef struct // Estrutura de actuação
• struct // Status para cada SCU
• bool pwm, calib
• sysStatus
• struct // Estrutura de Controlo para
  cada junta
• byte ki, kp, kd

33
Data Base in Slaves

• typedef struct // Estrutura descritiva dos
  sensores
• struct
• bool pwm, calib // Motores
  ligados/desligados Calibração on/off
• bool deadlineError // Violação de
  deadline
• bool motionFinAll // Todos os motores
  terminaram o movimento
• bool motionFinOne // Um dos motores
  terminaram o movimento
• bool motionFinN_SERVOS // Movimento
  terminado
• sysStatus // Estado sensorial do sistema
• struct_servo servoN_SERVOS
  // Sensores dos servos (pos, vel corrente)
• unsigned char specialN_SPECIAL_SENSORS
  // Sensores especiais
• struct_sensors
• typedef struct // Estrutura descritiva dos
  actuadores
• struct // Estrutura de actuação de
  status para cada SCU
• bool pwm, calib
• sysStatus
• struct // Controlador
• byte kp, kd, ki
• byte type

34
Alternatives CAN applications

• Usage of a Industrial Application of CAN
• CANopen
• FTTCan
• DeviceNet
• SDS, J1939, CAN Kingdom,
• Advantages
• Simplicity of implementation
• Some applications are freeware
• Disadvantages
• Low direct control over the physical layer for
  best performance
• Increased overhead for the microcontroller.
• Some applications are not freeware
• Conclusion the usage of the standard CAN, allows
• Control of the performance for the application
  and hardware involved
• But, is required more development time!

35
Hardware Improvements

• To increase performance and provide more BW for
  the control algorithms of the robot, more
  powerfull microcontrollers can be used.
• Adittionally, code compiling reliability depends
  of the architecture.
• Example PIC 18F2580
• Same pin-out
• Fully compatible with PIC 18F258
• C compiler optimized architecture with optional
  extended instruction set
• Enhanced CAN feature (ECAN)
• 3 modes of operation (Legacy, Enhanced Legacy,
  FIFO)
• 6 programmable RX/TX buffers
• 3 full 29-bit acceptance masks
• 16 full 29-bit acceptance filters w/ dynamic
  association
• Advanced error management features

36
Conclusions

• After careful memory management, the system is
  working efficiently during several hours without
  interruption. However, more tests are necessary!
• Due to instability by the Microchip compiler,
  some improvements can be achieved.
• The usage of a C optimized architecture (18F2580)
  can improve the compiler reliability, which is
  worth to experiment.
• If instability persists, the HI-TECH compiler can
  be experimented which offers easy integration
  into MPLAB and reduced source code modifications.