MIT SPIne 测试9（失败）

为了进行循环测试，我从网上找了timer函数：

#pragma once

#include <functional>
#include <chrono>
#include <thread>
#include <atomic>
#include <memory>
#include <mutex>
#include <condition_variable>

class Timer
{
public:
	Timer(): _expired(true), _try_to_expire(false)
	{}

	Timer(const Timer& timer)
	{
		_expired = timer._expired.load();
		_try_to_expire = timer._try_to_expire.load();
	}

	~Timer()
	{
		stop();
	}

	void start(int interval, std::function<void()> task)
	{
		// is started, do not start again
		if (_expired == false)
			return;

		// start async timer, launch thread and wait in that thread
		_expired = false;
		std::thread([this, interval, task]() {
			while (!_try_to_expire)
			{
				// sleep every interval and do the task again and again until times up
				std::this_thread::sleep_for(std::chrono::milliseconds(interval));
				task();
			}

			{
				// timer be stopped, update the condition variable expired and wake main thread
				std::lock_guard<std::mutex> locker(_mutex);
				_expired = true;
				_expired_cond.notify_one();
			}
		}).detach();
	}

	void startOnce(int delay, std::function<void()> task)
	{
		std::thread([delay, task]() {
			std::this_thread::sleep_for(std::chrono::milliseconds(delay));
			task();
		}).detach();
	}

	void stop()
	{
		// do not stop again
		if (_expired)
			return;

		if (_try_to_expire)
			return;

		// wait until timer 
		_try_to_expire = true; // change this bool value to make timer while loop stop
		{
			std::unique_lock<std::mutex> locker(_mutex);
			_expired_cond.wait(locker, [this] {return _expired == true; });

			// reset the timer
			if (_expired == true)
				_try_to_expire = false;
		}
	}

private:
	std::atomic<bool> _expired; // timer stopped status
	std::atomic<bool> _try_to_expire; // timer is in stop process
	std::mutex _mutex;
	std::condition_variable _expired_cond;
};

然后将硬件交互部分代码进行更新，

#pragma once
#include "hardware/motor_spi.h"

#include <cstdint>

const uint32_t LEFT = 0;
const uint32_t RIGHT = 1;
const uint32_t FRONT = 0;
const uint32_t REAR = 1;
const uint32_t ABAD = 0;
const uint32_t HIP = 1;
const uint32_t KNEE = 2;

class HardwareBridge
{
    struct convert_motor
    {
        float sign;
        float offset;
    };

    struct convert_leg
    {
        convert_motor motor[3];
    };

    struct convert_spi
    {
        convert_leg leg[2];
    };
public:
    void initialize();    // init communication
    void finalize();

    void update();  // send cmd to motor and get state

    void start();   // enable motor
    void stop();    // disable motor

    void printInfo();

    // i: spi, LEFT, RIGHT, j: leg, FRONT, REAR, k:motor, ABAD, HIP, KNEE, motor cmd
    void setJoint(int i, int j, int k, float p_des, float v_des, float kp, float kd, float t_ff);

public:
    int m_spi_fd[2];

    spine_cmd_t m_cmd[2];
    spine_state_t m_state[2];

    convert_spi m_converter[2];
};

#include "bridge/hardware_bridge.h"
#include "hardware/motor_spi.h"

#include "rt/rt_util.h"
#include "util/crc.h"

#include <iostream>

const char* name[] = {"/dev/spidev0.0", "/dev/spidev0.1"};

const uint32_t spi_count = 2;
const uint32_t leg_count = 2;
const uint32_t motor_count = 3;

// init communication
void HardwareBridge::initialize()
{
    spi_open(m_spi_fd[0], name[0]);
    spi_open(m_spi_fd[1], name[1]);

    // init a very soft behavior
    this->setJoint(LEFT, FRONT, ABAD, 0, 0, 5, 0, 0);
    this->setJoint(LEFT, FRONT, HIP, 0, 0, 5, 0, 0);
    this->setJoint(LEFT, FRONT, KNEE, 0, 0, 5, 0, 0);

    this->setJoint(LEFT, REAR, ABAD, 0, 0, 5, 0, 0);
    this->setJoint(LEFT, REAR, HIP, 0, 0, 5, 0, 0);
    this->setJoint(LEFT, REAR, KNEE, 0, 0, 5, 0, 0);

    this->setJoint(RIGHT, FRONT, ABAD, 0, 0, 5, 0, 0);
    this->setJoint(RIGHT, FRONT, HIP, 0, 0, 5, 0, 0);
    this->setJoint(RIGHT, FRONT, KNEE, 0, 0, 5, 0, 0);

    this->setJoint(RIGHT, REAR, ABAD, 0, 0, 5, 0, 0);
    this->setJoint(RIGHT, REAR, HIP, 0, 0, 5, 0, 0);
    this->setJoint(RIGHT, REAR, KNEE, 0, 0, 5, 0, 0);
}

void HardwareBridge::finalize()
{
    spi_close(m_spi_fd[0]);
    spi_close(m_spi_fd[1]);
}

// send cmd to motor and get state
void HardwareBridge::update()
{
    static uint8_t tx[sizeof(spine_cmd_t)] = {0};
    static uint8_t rx[sizeof(spine_cmd_t)] = {0};

    spine_state_t temp_state;

    for(int i = 0; i < spi_count; ++i)
    {
        m_cmd[i].crc = calculate((uint8_t*)&m_cmd[i], sizeof(spine_cmd_t) - 4);

        memcpy(tx, &m_cmd[i], sizeof(spine_cmd_t));

        int rv = transfer(m_spi_fd[i], tx, rx, sizeof(spine_cmd_t));
        (void)rv;

        memcpy(&temp_state, rx, sizeof(spine_state_t));

        uint32_t crc = calculate((uint8_t*)&temp_state, sizeof(spine_state_t) - 4);
        if(crc == temp_state.crc)
        {
            memcpy(&m_state[i], rx, sizeof(spine_state_t));
            std::cout << "[CRC] crc correct" << std::endl;
        }
        else
        {
            memcpy(&m_state[i], rx, sizeof(spine_state_t));
            std::cout << "[CRC] crc error" << std::endl;
        }
    }
}

// enable motor
void HardwareBridge::start()
{
    for(int i = 0; i < spi_count; ++i)
    {
        for(int j = 0; j < leg_count; ++j)
        {
            m_cmd[i].leg[j].flag = 1;
        }
    }
}

// disable motor
void HardwareBridge::stop()
{
    for(int i = 0; i < spi_count; ++i)
    {
        for(int j = 0; j < leg_count; ++j)
        {
            m_cmd[i].leg[j].flag = 0;
        }
    }
}

void HardwareBridge::printInfo()
{
    std::cout << "[Robot State]" << std::endl;
    for(int i = 0; i < 2; ++i)
    {
        for(int j = 0; j < 2; ++j)
        {
            for(int k = 0; k < 3; ++k)
            {
                motor_data_t& m = m_state[i].leg[j].motor[k];
                std::cout << "spi: " << j << " leg: " << j << " id: " << k << " : " << m.p << ", " << m.v << ", " << m.t << std::endl;
            }
        }
    }
    std::cout << std::endl;
}

// i: spi, LEFT, RIGHT, j: leg, FRONT, REAR, k:motor, ABAD, HIP, KNEE, motor cmd
void HardwareBridge::setJoint(int i, int j, int k, float p_des, float v_des, float kp, float kd, float t_ff)
{
    m_cmd[i].leg[j].motor[k].p_des = p_des;
    m_cmd[i].leg[j].motor[k].v_des = v_des;
    m_cmd[i].leg[j].motor[k].kp = kp;
    m_cmd[i].leg[j].motor[k].kd = kd;
    m_cmd[i].leg[j].motor[k].t_ff = t_ff;
}

测试的循环控制代码如下：

#include "hardware/motor_spi.h"
#include "bridge/hardware_bridge.h"
#include "util/crc.h"
#include "util/timer.h"

#include <cmath>
#include <iostream>
#include <unistd.h>

HardwareBridge bridge;

float angle = 0;

void simple_control()
{
	std::cout << "trigger func1" << std::endl;

    angle += 0.1;

    bridge.setJoint(LEFT, FRONT, ABAD, 0.5 * sin(angle), 0, 5, 0, 0);
    bridge.update();
    bridge.printInfo();
}

int main()
{
    //prefaultStack();
    //setupScheduler();
    create_lookup_table();

    bridge.initialize();

    bridge.start();

    Timer timer;

    std::cout << "--- start period timer ----" << std::endl;
	timer.start(100, std::bind(simple_control));
	std::this_thread::sleep_for(std::chrono::milliseconds(5000));
	timer.stop();
	std::cout << "--- stop period timer ----" << std::endl;

    bridge.finalize();

    return 0;
}

运行代码后一直CRC校验错误，感觉还是STM32部分存在问题，在反复的SPI通信调用中存在bug。