QuimWiki

User Tools

Site Tools

Trace:
example_code

Differences

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

Link to this comparison view

Both sides previous revisionPrevious revision
example_code [2014/03/07 00:51] – cedric.eloyexample_code [2018/05/22 10:33] (current) – external edit 127.0.0.1
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>