User Tools

Site Tools


example_code

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:

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;
}

A simple UObject to control a motor

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
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 ;
}
example_code.txt · Last modified: 2018/05/22 10:33 (external edit)