example_code
Table of Contents
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 by 127.0.0.1