User Tools

Site Tools


example_code

Differences

This shows you the differences between two versions of the page.

Link to this comparison view

Both sides previous revision Previous revision
Next revision
Previous revision
example_code [2014/03/07 00:46]
cedric.eloy
example_code [2018/05/22 10:33] (current)
Line 1: Line 1:
 +====== 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>​
example_code.txt ยท Last modified: 2018/05/22 10:33 (external edit)