====== Example code ======


===== CAN communication with control boards =====

The applications running in the embedded Linux environment can communicate with the IO boards through the CAN bus protocol. The high level protocol is a reduced version of the CANopen stack.

Here is how to write a Object Dictionary entry:

<code c od_write_master.c>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <stdint.h>
 
#include <net/if.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <sys/ioctl.h>
 
#include <linux/can.h>
#include <linux/can/raw.h>

/**
 * \brief Initializes a raw socket for CAN communication
 * \param ifname the interface, typically can0
 */
int init_can(const char * ifname)
{
  int s;
  struct sockaddr_can addr;
  struct ifreq ifr;
 
  if((s = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
    perror("Error while opening socket");
    return -1;
  }
 
  strcpy(ifr.ifr_name, ifname);
  ioctl(s, SIOCGIFINDEX, &ifr);
 
  addr.can_family  = AF_CAN;
  addr.can_ifindex = ifr.ifr_ifindex; 
 
  printf("%s at index %d\n", ifname, ifr.ifr_ifindex);
 
  if(bind(s, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
    perror("Error in socket bind");
    return -2;
  }  
  
  return s;
}

/**
 * \brief Writes OD entry
 * \param s socket where to write to
 * \param idx OD index
 * \param subidx OD subindex
 * \param val value to write
 */
int od_write(int s, uint16_t idx, uint8_t subidx, int32_t val)
{
  struct can_frame frame;  
  
  frame.can_id  = 0x600;
  frame.can_dlc = 8;
  frame.data[0] = 0x23;
  frame.data[1] = idx&0xFF;
  frame.data[2] = ( idx >> 8 ) & 0xFF ;
  frame.data[3] = subidx ;
  frame.data[4]  = (   val        & 0xFF);
  frame.data[5]  = ( ( val >> 8 ) & 0xFF);
  frame.data[6]  = ( ( val >> 16 ) & 0xFF);
  frame.data[7]  = ( ( val >> 24 ) & 0xFF);
  
  write(s, &frame, sizeof(struct can_frame));
 
  return 0;
}

</code>

===== A simple UObject to control a motor =====

<code c++ UQuimMotor.hpp>
#ifndef UQUIMMOTOR_HPP__
#define UQUIMMOTOR_HPP__
 
// Include the UObject declarations. 
# include <urbi/uobject.hh> 
 
class UQuimMotor
  : public urbi::UObject 
{ 
public: 
  /**
   * \brief C++ contructor.
   *  
   * \param name  name given to the instance
   */ 
  UQuimMotor(const std::string& name); 
 
  /** 
   * \brief Urbi constructor
   *  
   * \param motor_id
   * \return 0 on success
   */ 
  int init(int motor_id); 
  
  /**
   * \brief Function notified when the speed is changed. 
   * \param v the UVar being modified 
   * \return 0  on success
   */ 
  int speed_set(urbi::UVar& v);  
 
private: 
  urbi::UVar speed; 
  int        motorId_m ;
  int        canSocket_m;
}; 
#endif 

</code>

<code c++ UQuimMotor.cpp>
#include "UQuimMotor.hpp" 
#include "od_write_master.h"
#include <stdio.h>
 
// Register the UMachine UObject in the Urbi world. 
UStart(UQuimMotor); 
 
// Bouncing the name to the UObject constructor is mandatory. 
UQuimMotor::UQuimMotor(const std::string& name) 
  : urbi::UObject(name), motorId_m(0) 
{ 
  // Register the Urbi constructor.  This is the only mandatory 
  // part of the C++ constructor. 
  UBindFunction(UQuimMotor, init); 
} 
 
int UQuimMotor::init(int motor_id) 
{ 
  if (! (motor_id == 1 || motor_id == 2) )
  {
    printf("Invalid motor_id\r\n");
    return 1;
  }
  motorId_m = motor_id ;
  
  canSocket_m = init_can("can0");
  if (canSocket_m < 0)
  {
    printf("Failed to init CAN...\r\n");
    return 1;
  }

  // Bind the UVars before using them. 
  UBindVar(UQuimMotor, speed); 

  // Request that speed_set be invoked each time speed is changed.
  UNotifyChange(speed, &UQuimMotor::speed_set); 
 
  // Success. 
  return 0; 
} 

int UQuimMotor::speed_set(urbi::UVar& v) 
{ 
  // Speed entry in QuimMotor entry is at index 0x60FF
  // subidx indicates which motor to set
  od_write(canSocket_m, 0x60FF, motorId_m, int(v));
  return 0 ;
} 

</code>