motor_driver.h

Go to the documentation of this file.

#ifndef motor_driver_h
#define motor_driver_h
 
#include "motor_constants.h"
#include "config_motor_driver.h"
#include "teensy_can_ports.h"
#include "make_can_msg.h"
#include "FlexCAN_T4.h"
#include <math.h>
#include "utilities.h"
#include "Arduino.h"
 
class MotorControl
{
public:
    MotorControl(){}
 
    MotorControl(uint8_t _id, uint8_t _can_port_id, int _number_of_inner_motor_rotations, double _position_offset);
 
    bool readPIDParameters();
 
    bool writePIDParametersToRAM
    (
        double _kp_pos,
        double _ki_pos,
        double _kp_speed,
        double _ki_speed,
        double _kp_torque,
        double _ki_torque
    ); 
 
    void setPositionReference(double _angle);
 
    void setSpeedReference(double _speed);
 
    void setTorqueReference(double _torque);
 
    void setTorqueCurrent(int _torque_current);
 
    void readMotorControlCommandReply(unsigned char* _can_message);
 
    bool stopMotor();
 
    bool turnOffMotor();
 
    bool readMultiTurnAngle();
 
    bool readMotorStatus();
 
    void requestMotorStatus();
 
    bool readCompleteEncoderPosition();
 
    // TODO change to camelcase - also in related functions
    uint8_t get_id(){return id;}
 
    double getPosition(){return position;}
 
    double getVelocity(){return speed;}
 
    double getTorque(){return torque;}
 
    double getEncoderValue(){return previous_encoder_value;}
 
    double getMultiTurnAngle(){return multi_turn_angle_32_bit;}
 
    double getMultiTurnAngleRaw(){return multi_turn_angle_001lsb;}
 
    void getPIDParameters(
        double &_kp_pos,
        double &_ki_pos,
        double &_kp_speed,
        double &_ki_speed,
        double &_kp_torque,
        double &_ki_torque);  
 
    void printPIDParameters();
 
    void printState();
 
    void printTorqueCurrents();
 
    double raw_position_reference;
private:
    uint8_t id;
    
    uint16_t address;
 
    uint8_t can_port_id;
 
    double kp_pos;
    double ki_pos;
    double kp_speed;
    double ki_speed;
    double kp_torque;
    double ki_torque;
 
    double initial_number_of_inner_motor_rotations;
 
    double number_of_inner_motor_rotations;
 
    uint16_t previous_encoder_value;
 
    int32_t multi_turn_angle_32_bit = 0;
 
    int32_t multi_turn_angle_32_bit_001lsb = 0;
 
    int64_t multi_turn_angle_64_bit = 0;
 
    int64_t multi_turn_angle_001lsb = 0;
 
    // This parameters keeps track of whether or not this is the case.
    double target_position_offset = 0.0;
 
    double position_offset = 0.0;
 
    double position_center_offset = M_PI/6.0; //M_PI/6.0;
 
    double position;
 
    double speed;
 
    double torque;
 
    double temperature;
 
    int16_t torque_current_reference = 0;
    
    int16_t torque_current_measured = 0;
 
    CAN_message_t can_message;
 
    CAN_message_t received_can_message;
 
    // Motor specific parameters
 
    double max_torque;
 
    int max_torque_current;
 
    uint16_t max_encoder_value;
 
    uint16_t encoder_turn_threshold;
 
    double innerMotorTurnCompleted(uint16_t _previous_encoder_value, uint16_t _new_encoder_value);
 
    void sendMessage(CAN_message_t _can_message);
 
    int readMessage(CAN_message_t &_can_message);
 
    // TODO Remove
    void errorMessage();
};
 
#endif