giovanni beri / Mbed OS ROBI_fw

The firmware for robì Nucleo board, used to read data from the IMU, send velocities commands to the wheels, and read actual wheel velocities.

Dependencies:   X_NUCLEO_IKS01A1

Robì board

Firmware overview

The goals of this firmware are:

  • read data from the IMU (the blue shield installed on the ), and send them through ethernet to the PC mounted on Robì
  • receive via ethernet the target velocity of the robot, calculate the required wheels velocities, and send them to the 4 wheels via CAN
  • receive via CAN the actual speeds of the wheels, calculate from them the actual robot velocities, and send it to the PC via ethernet.

For sending and receiving data through ethernet, on the board two UDP sockets are opened, one for outbound communication, and one for inbound communication. A serial communication is also set, that communicates with the PC through the same micro USB cable used to power the Nucleo board, and only trasmits debug info.
The only files you should need to edit/read are main.cpp, with the business logic, and param_config.h file, where you can consult or edit some firmware settings: IP&port for the UDP sockets, size of UDP packet, baudrate of the serial port.

Board setup

The blue shield with the IMU (X-NUCLEO-IKS01A1) must be placed in the connector on the board, the ethernet cable connected to the PC, the micro USB connected to the PC for power supply and debug. NOTE: two micro USB connectors exist, the correct one is the one on the opposite side of the board with respect to the ethernet connector, and not the one adjacent the ethernet connector.

On desktop side, the communication with the board is executed via ROS nodes, that you can find in this repo.

Actual functionalities

This firmware is still very raw, since I only had time to take steps for the implementation of the communication, and not on the logic. As a suggestion, read carefully the current code (it's only 140 LOC), and use it as an example of how the involved parts are going to communicate, rather than a simplified version of the final firmware.

Some final notes

  • learn the structure of the CAN messages to understand the prepareCANMessages procedure
  • for CAN communication you need to connect a CAN transceiver to PD0 and PD1 pins (make reference to this image)
  • the board currently sends raw data from the IMU, and the correction of the IMU bias/distortion is made in the ROS node that receive the data.

Jånzo

main.cpp

Committer:
giovanniberi93
Date:
2018-05-11
Revision:
13:04fa45b2bee1
Parent:
12:101a11e0e37e

File content as of revision 13:04fa45b2bee1:

#include "mbed.h"
#include "x_nucleo_iks01a1.h"
#include "EthernetInterface.h"
#include "UDPSocket.h"
#include "param_config.h"
#include "CAN.h"

// Instantiate the expansion board
static X_NUCLEO_IKS01A1 *mems_expansion_board = X_NUCLEO_IKS01A1::Instance(D14, D15);
// Retrieve the composing elements of the expansion board
static GyroSensor *gyroscope = mems_expansion_board->GetGyroscope();
static MotionSensor *accelerometer = mems_expansion_board->GetAccelerometer();
static MagneticSensor *magnetometer = mems_expansion_board->magnetometer;

// other peripherals
CAN enginesCAN(PD_0, PD_1, 1000000);
EthernetInterface eth;
UDPSocket receive_sock;
UDPSocket send_sock;
Serial pc(USBTX, USBRX);

// CANMessages
CANMessage frontRightWheel;
CANMessage frontLeftWheel;
CANMessage rearRightWheel;
CANMessage rearLeftWheel;
CANMessage receivedCANMsg;

//----------------------------------------------------------
//----------------------------------------------------------

void setupExpansionBoard(){
    uint8_t id;
    magnetometer->read_id(&id);
    if(!id)
        pc.printf("Magnetometer not detected");
    gyroscope->read_id(&id); 
    if(!id)
        pc.printf("Gyroscope not detected");
    accelerometer->read_id(&id);
    if(!id)
        pc.printf("Accelerometer not detected");
}

void setupEthernet(){
    eth.set_network(DEVICE_IP, "255.255.255.0", "192.168.1.11");
    eth.connect();
    /* Open the server on ethernet stack */
    receive_sock.open(&eth);
    send_sock.open(&eth);    
    /* set receiving socket params */
    receive_sock.bind(DEVICE_IP, RECEIVE_PORT);
    receive_sock.set_blocking(false);    
}

void prepareCANMessages(){
    frontRightWheel.format = CANExtended;
    frontLeftWheel.format  = CANExtended;
    rearRightWheel.format  = CANExtended;
    rearLeftWheel.format   = CANExtended;
    // data size is the same (4) for all messages for writing speeds, different for reading speed (9)
    frontRightWheel.len = 4;
    frontLeftWheel.len  = 4;
    rearRightWheel.len  = 4;
    rearLeftWheel.len   = 4;
    // in ID fields, left 8 bits are for the command, right 8 bits for device id
    frontRightWheel.id  = 0x03 << 8 | 0x02;
    frontLeftWheel.id   = 0x03 << 8 | 0x01;
    rearRightWheel.id   = 0x03 << 8 | 0x04;
    rearLeftWheel.id    = 0x03 << 8 | 0x03;            
}

void writeCANMessages(int32_t leftVel, int32_t rightVel){
    // assign data field
    frontRightWheel.data[0] = rightVel;
    rearRightWheel.data[0]  = rightVel;
    frontLeftWheel.data[0]  = leftVel;
    rearLeftWheel.data[0]   = leftVel;
    // send the messages
    enginesCAN.write(frontRightWheel);
    enginesCAN.write(rearRightWheel);
    enginesCAN.write(frontLeftWheel);
    enginesCAN.write(rearLeftWheel);
}
//----------------------------------------------------------
//----------------------------------------------------------


int main()
{
    // ethernet
    SocketAddress dest_addr(PC_IP, DESTINATION_PORT);
    SocketAddress tmp_addr;
    char send_buf[UDP_SEND_PACKET_SIZE];
    char receive_buf[UDP_RECEIVE_PACKET_SIZE];
    // IMU
    int32_t axes_mag[3];
    int32_t axes_gyro[3];
    int32_t axes_acc[3];
    // received velocities
    int32_t* velocities;
    
    pc.baud(SERIAL_BAUD);
    pc.printf("\n\r***** NEW RUN *****\n\r");
    
    setupExpansionBoard();
    setupEthernet();
    prepareCANMessages();
    pc.printf("The board IP address is '%s'\n\r", eth.get_ip_address());
    pc.printf("The destination PC address is '%s'\n\r", PC_IP);

    // main loop
    pc.printf("Streaming data to address %s:%d \n\r", PC_IP, DESTINATION_PORT);
    pc.printf("Structure of data string:\n\r timestamp axes_mag[0] axes_mag[1] axes_mag[2] axes_acc[0] axes_acc[1] axes_acc[2] axes_gyro[0] axes_gyro[1] axes_gyro[2]\n\r");
    while (true) {
        // tmp_addr get filled with the address of the sender of received message
        if(receive_sock.recvfrom(&tmp_addr, receive_buf, UDP_RECEIVE_PACKET_SIZE) > 0) {
            // handle received data
            velocities = reinterpret_cast<int32_t*>(receive_buf);
            pc.printf("Ricevuto da ethernet\n\r");
            pc.printf("%d %d \n\r", velocities[0], velocities[1]);
            writeCANMessages(velocities[0], velocities[1]);
            pc.printf("Scritto su CAN \n\r");
            if(enginesCAN.read(receivedCANMsg)){
                pc.printf("Can loopback va \n\r");
            } else {
                pc.printf("Can loopback NON va \n\r");
            }
            
        }
        // read from sensors
        magnetometer->  get_m_axes(axes_mag);
        accelerometer-> get_x_axes(axes_acc);
        gyroscope->     get_g_axes(axes_gyro);
        sprintf(send_buf, "%d %d %d %d %d %d %d %d %d ", axes_mag[0], axes_mag[1], axes_mag[2], axes_acc[0], axes_acc[1], axes_acc[2], axes_gyro[0], axes_gyro[1], axes_gyro[2]);
        send_sock.sendto(dest_addr, (void *)send_buf, 350);
        
        // Spinning at highest speed. Currently the rate is about 166Hz
        // wait(0.1);
    }
}