PEAK-System Forum

Code: Select all

bool YloTwoPcanToMoteus::read_moteus_RX_queue(int id, int port, float& position, float& velocity, float& torque, float& voltage, float& temperature, float& fault){
    moteus_rx_msg.ID = 0x8000 | id;

    // Créer une copie du message RX
    moteus_rx_msg_copy = moteus_rx_msg;

    do{ Status = CAN_ReadFD(port,&moteus_rx_msg_copy, NULL); 
        usleep(20);
    }

    while(Status != PCAN_ERROR_OK && Status != PCAN_ERROR_QRCVEMPTY); // is return frame received ?
        usleep(150);
        memcpy(&_position, &moteus_rx_msg_copy.DATA[MSGRX_ADDR_POSITION], sizeof(float));
        memcpy(&_velocity, &moteus_rx_msg_copy.DATA[MSGRX_ADDR_VELOCITY], sizeof(float));
        memcpy(&_torque,   &moteus_rx_msg_copy.DATA[MSGRX_ADDR_TORQUE],   sizeof(float));
        memcpy(&_voltage, &moteus_rx_msg_copy.DATA[MSGRX_ADDR_VOLTAGE], sizeof(float));
        memcpy(&_temperature, &moteus_rx_msg_copy.DATA[MSGRX_ADDR_TEMPERATURE], sizeof(float));
        memcpy(&_fault,   &moteus_rx_msg_copy.DATA[MSGRX_ADDR_FAULT],   sizeof(float));
        position    = _position;   
        velocity    = _velocity;
        torque      = _torque;
        voltage     = _voltage;
        temperature = _temperature;
        fault       = _fault;
        return true;
}