Diff: MX12.cpp

Revision:

9:67d737d8a349

Child:

11:5f2289e0d0e8

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MX12.cpp	Fri Feb 25 10:13:06 2022 +0000
@@ -0,0 +1,506 @@
+/**
+ * @file MX12.ccp
+ * @brief This file contains all the methods of the MX12 class
+ *        whose prototypes are in the MX12.h header file
+ */
+
+#include "MX12.h"
+#include "math.h"
+
+MX12::MX12(PinName tx, PinName rx, int baud)
+    : _mx12(tx, rx)    // initializes UnbufferedSerial object
+{
+    /*  Serial bus setup
+     */
+    // // Set desired properties (baud-8-N-1)
+    _mx12.baud(baud);     /* modulation speed */
+    _mx12.format(
+        8,                /* bits */
+        SerialBase::None, /* parity */
+        1                 /* stop bit */
+    );
+    // Register a callback to process a Rx (receive) interrupt.
+    _mx12.attach(callback(this, &MX12::_Rx_interrupt), SerialBase::RxIrq);
+
+    // variable used for message reception
+    _status_pck = {.raw = "",
+                   .n_byte = 0,
+                   .servo_id = 0,
+                   .length = 0,
+                   .error = 0,
+                   .n_param = 0,
+                   .param = "",
+                   .received_checksum = 0,
+                   .calculated_checksum = 0,
+                   .parsed = false,
+                   .valid = false
+                  };
+
+    _parser_state = {.expected_field = PacketField::Header1,
+                     .byte_index = 0,
+                     .param_index = 0
+                    };
+
+    // Internal defaults states
+    _bus_state = SerialState::Idle;
+
+}
+
+void MX12::SetSpeed(unsigned char mot_id, float speed)
+{
+    char data[2];
+
+    // Speed absolute value
+    int goal = (0x3ff * abs(speed));
+
+    // Spin direction (CW is negative)
+    if (speed < 0) {
+        goal |= (0x1 << 10);
+    }
+
+    data[0] = goal & 0xff;
+    data[1] = goal >> 8;
+
+    // Send instruction
+    _bus_state = SerialState::Writing;
+    rw(mot_id, CONTROL_TABLE_MOVING_SPEED, 2, data);
+}
+
+void MX12::SetSpeed_rad_s(unsigned char mot_id, float speed)
+{
+    if (speed > MX12_ABSOLUTE_MAX_SPEED_RAD_S) {
+        SetSpeed(mot_id, 1);
+    } else if (speed < -MX12_ABSOLUTE_MAX_SPEED_RAD_S) {
+        SetSpeed(mot_id, -1);
+    } else {
+        SetSpeed(mot_id, speed / MX12_ABSOLUTE_MAX_SPEED_RAD_S);
+    }
+}
+
+char MX12::IsAvailable(void)
+{
+    return (_bus_state == SerialState::Idle);
+}
+
+void MX12::rw(unsigned char mot_id, char address, char len, char *data)
+{
+
+    /* Set variables for reception from servovotor */
+    _answer  = 0;
+    _status_pck = {.raw = "",
+                   .n_byte = 0,
+                   .servo_id = 0,
+                   .length = 0,
+                   .error = 0,
+                   .n_param = 0,
+                   .param = "",
+                   .received_checksum = 0,
+                   .calculated_checksum = 0,
+                   .parsed = false,
+                   .valid = false
+                  };
+    _parser_state = {.expected_field = PacketField::Header1,
+                     .byte_index = 0,
+                     .param_index = 0
+                    };
+
+
+    /* Initialise instruction packet to forge.
+     * Instruction Packet is the command data sent to the servomotor.
+     *
+     * |Header1|Header2|Packet ID|Length|Instruction|Param1...ParamN|Checksum|
+     * |-------|-------|---------|------|-----------|---------------|--------|
+     * |  0xFF |  0xFF |Packet ID|Length|Instruction|Param1...ParamN| CHKSUM |
+     * | cmd[0]| cmd[1]|  cmd[2] |cmd[3]|  cmd[4]   |cmd[5]...      |        |
+     *                  \__  ___/                    \_  _/ \__  __/
+     *                     \/           |              \/      \/            |
+     *                   mot_id         |           address   data           |
+     *                                  |                     (len = N-1)    |
+     *                                  \__________________  ________________/
+     *                                                     \/
+     *                                                  Length ( cmd[3] )
+     */
+    char packet[16];
+    unsigned char packet_length;
+
+    /* Initialise checksum to calculate
+     * It is used to check if packet is damaged during communication.
+     * Status Checksum is calculated according to the following formula:
+     *
+     * Status Checksum = ~( ID + Length + Error + Parameter1 + … Parameter N )
+     */
+    char checksum = 0x00;
+
+    /* header 1 = 0xFF (dynamixel protocol 1.0) */
+    packet[0] = 0xff;
+
+    /* header 2 = 0xFF (dynamixel protocol 1.0) */
+    packet[1] = 0xff;
+
+    /* packet ID i.e. servomotor id (dynamixel protocol 1.0) */
+    packet[2] = mot_id;
+    checksum += packet[2];
+
+    /* Guess instruction type. NULL for read, not NULL for write */
+    if(data == NULL) { // read instruction
+        /* byte length of the instruction: parameter and checksum field.   */
+        /* for read instruction:   1 INSTR +                               */
+        /*       2 PARAM (starting address, length of data) + 1 CHKSUM     */
+        packet[3] = 4;
+        checksum += packet[3];
+
+        /* set write instruction */
+        packet[4] = PROTOCOL_INSTRUCTION_READ;
+        checksum += packet[4];
+
+        /* Param 1: address to read in the Control Table of RAM Area */
+        packet[5] = address;
+        checksum += packet[5];
+
+        /* Param 2: number of bytes to read in the Control Table of RAM Area */
+        packet[6] = len;
+        checksum += packet[6];
+
+        /* Checksum = ~( ID + Length + Instruction + Param1 + … Param N ) */
+        packet[7] = ~checksum;
+
+        packet_length = 8;
+    } else { // write instruction
+        /* byte length of the instruction: parameter and checksum field     */
+        /* For write instruction:   1 INSTR +                               */
+        /*       (1+len)PARAM (starting Address, bytes to write) + 1 CHKSUM */
+        packet[3] = 3 + len;
+        checksum += packet[3];
+
+        /* set read instruction */
+        packet[4] = PROTOCOL_INSTRUCTION_WRITE;
+        checksum += packet[4];
+
+        /* Param 1: address to write in the "Control Table of RAM Area" */
+        packet[5] = address;
+        checksum += packet[5];
+
+        /* Param 2 to N: data to write in the Control Table of RAM Area */
+        for(char i = 0; i < len; i++) {
+            packet[6 + i] = data[i];
+            checksum += data[i];
+        }
+
+        /* Checksum = ~( ID + Length + Instruction + Param1 + … Param N ) */
+        packet[6 + len] = ~checksum;
+
+        packet_length = 7 + len;
+    }
+
+    // Send packet
+    if(mot_id != 0xFE) {
+        for(char i = 0; i < packet_length; i++) {
+            _mx12.write(&packet[i], 1);
+        }
+    }
+}
+
+// Debug function to print Serial read
+void MX12::PrintSerial()
+{
+    for(int i = 0; i < _status_pck.n_byte; i++) {
+        printf("%x ", _status_pck.raw[i]);
+    }
+    printf("\n");
+}
+
+MX12::Status MX12::GetStatus()
+{
+    // Return the corresponding status code
+    switch(_status_pck.error) {
+        case 0:
+            return Ok;
+            break;
+        case 1 << 0:
+            return InputVoltageError;
+            break;
+        case 1 << 1:
+            return AngleLimitError;
+            break;
+        case 1 << 2:
+            return OverheatingError;
+            break;
+        case 1 << 3:
+            return RangeError;
+            break;
+        case 1 << 4:
+            return ChecksumError;
+            break;
+        case 1 << 5:
+            return OverloadError;
+            break;
+        case 1 << 6:
+            return InstructionError;
+            break;
+        default:
+            return Unknown;
+    }
+}
+
+void MX12::_Rx_interrupt()
+{
+
+    char c;
+
+    // Try to read serial
+    if(_mx12.read(&c, 1)) {
+
+        _status_pck.raw[(_parser_state.byte_index)++] = c;
+
+        // State-machine parsing
+        switch(_parser_state.expected_field) {
+
+            /* c char is interpreted as a Header1 field */
+            case PacketField::Header1:
+
+                /* do nothing and set next state to Header2 */
+                _parser_state.expected_field = PacketField::Header2;
+                break;
+
+            /* c char is interpreted as a Header2 field */
+            case PacketField::Header2:
+
+                /* do nothing and set next state to Id */
+                _parser_state.expected_field = PacketField::Id;
+                break;
+
+            /* c char is interpreted as ID field */
+            case PacketField::Id:
+
+                /* store ID, update checksum and set next state to Length */
+                _status_pck.servo_id = c;
+                _status_pck.calculated_checksum += c;
+                _parser_state.expected_field = PacketField::Length;
+                break;
+
+            /* c char is interpreted as length of message data field
+             * Length = number of Parameters + 2
+             * where 2 stands for Length field (1 byte) + Error filed (1 byte)
+             */
+            case PacketField::Length:
+
+                /* store number of param into _status_pck.n_param,
+                 * update calculated_checksum and set next state to Error
+                 */
+                _status_pck.n_param = c - 2;
+                _status_pck.calculated_checksum += c;
+                _parser_state.expected_field = PacketField::Error;
+                break;
+
+            /* c char is interpreted as error status field */
+            case PacketField::Error:
+
+                /* store error status, update checksum
+                 * and set next state to Data
+                 */
+                _status_pck.error = c;
+                _status_pck.calculated_checksum += c;
+                _parser_state.expected_field = PacketField::Data;
+                break;
+
+            /* c char is interpreted as a param field */
+            case PacketField::Data:
+
+                /* store current param, increase param_index
+                 * and update checksum */
+                _status_pck.param[(_parser_state.param_index)++] = c;
+                _status_pck.received_checksum += c;
+
+                /* increase param index (_parser_state.dataCount)
+                 * and test if it is the last param to read
+                 */
+                if(_parser_state.param_index > _status_pck.n_param) {
+                    /* reset param index and set next state to Checksum */
+                    _parser_state.param_index = 0;
+                    _parser_state.expected_field = PacketField::Checksum;
+                }
+                break;
+
+            /* c char is interpreted as Checksum field */
+            case PacketField::Checksum:
+
+                /* store received_checksum, set parsed, store n_byte,
+                 * evalutate valid and set next state to Header1 */
+                _status_pck.received_checksum = c;
+                _status_pck.parsed = true;
+                _status_pck.n_byte = _parser_state.byte_index;
+                _status_pck.valid = (_status_pck.received_checksum == c);
+                _parser_state.expected_field = PacketField::Header1;
+
+                /* set seriel state to Idle */
+                _bus_state = SerialState::Idle;
+                break;
+
+            default:
+
+                /* unexpected case. If it occurs it would be due to a
+                 * code error of this class  */
+                break;
+        }
+    }
+}
+
+/* Code from previous version of the class */
+
+/*
+void MX12::ReadPosition(unsigned char mot_id) {
+    // Make a request, interrupt takes care of everything else
+    _state = State::ReadingPosition;
+    rw(mot_id, 0x24, 2, NULL);
+}
+
+float MX12::GetPosition(unsigned char mot_id) {
+    return _angle[mot_id];
+}
+*/
+
+
+
+
+
+
+void MX12::cmd_moteur(float Vavance, float Vlat, float Wz)
+{
+    // mettre en mode wh
+    char data[2];
+    data[1] = 0;
+    data[0] = 0;
+
+    float W1;
+    float W2;
+    float W3;
+    float Rc;
+    float R;
+    float x1;
+    float x2;
+    float x3;
+    float y1;
+    float y2;
+    float y3;
+    float a1;
+    float a2;
+    float a3;
+    Rc=0.08;  // rayon du chassis*10
+    R=0.019;  // rayon de la roue*10
+    W1=0;
+    W2=0;
+    W3=0;
+    a1 = 4.74 ;
+    a2 = -5.8;
+    a3 = 3.68;
+    x1 = -0.0032;
+    x2 = 0.0616;
+    x3 = -0.0645;
+    y1 = 0.072;
+    y2 = -0.0356;
+    y3 = -0.0376;
+    //Vtm_smax=0.8; //sert pour calculer valeurs -999->999
+    //Vnm_smax=0.9;
+    //Wcrd_smax=2.9;
+    //if (Vtm_s>Vtm_smax)
+    // {Vtm_s=Vtm_smax;}
+    //if (Vnm_s>Vnm_smax)
+    //  {Vnm_s=Vnm_smax;}
+    //if (Wcrd_s>Wcrd_smax)
+    //  {Wcrd_s=Wcrd_smax;}
+    //if (Wcrd_s<-Wcrd_smax)
+    //  {Wcrd_s=-Wcrd_smax;}
+
+    W1=1/R*(-cosf(a1)*Vavance + sinf(a1)*y1*Wz - sinf(a1)*Vlat + cosf(a1)*x1*Wz); //loi de commande moteur 1
+    W2=1/R*(-cosf(a2)*Vavance + sinf(a2)*y2*Wz - sinf(a2)*Vlat + cosf(a2)*x2*Wz);
+    W3=1/R*(-cosf(a3)*Vavance + sinf(a3)*y3*Wz - sinf(a3)*Vlat + cosf(a3)*x3*Wz);
+    printf("%d %d %dn\r",(int)(1000*W1),(int)(1000*W2),(int)(1000*W3));
+
+
+    SetSpeed_rad_s(1,W1);  // impose la vitesse au moteur 1
+    SetSpeed_rad_s(2,W2);
+    SetSpeed_rad_s(3,W3);
+
+
+
+}
+
+
+
+void MX12::eteindre_moteurs()
+{
+    char data[1];
+
+    data[0] = 0;
+
+
+    // Send instruction
+    _bus_state = SerialState::Writing;
+    rw(1, CONTROL_TABLE_TORQUE_ENABLE, 1, data);
+    _bus_state = SerialState::Writing;
+    rw(2, CONTROL_TABLE_TORQUE_ENABLE, 1, data);
+    _bus_state = SerialState::Writing;
+    rw(3, CONTROL_TABLE_TORQUE_ENABLE, 1, data);
+}
+
+
+
+void MX12::cmd_moteur_multiturn(float pos1, float pos2, float pos3)
+{
+    // eteindre les moteurs
+    eteindre_moteurs();
+    // mettre en mode multiturn
+    char data[2];
+    data[1] = 255;
+    data[0] = 15;
+    _bus_state = SerialState::Writing;
+    rw(1, CONTROL_TABLE_CW_ANGLE_LIMIT, 2, data);
+    rw(2, CONTROL_TABLE_CW_ANGLE_LIMIT, 2, data);
+    rw(3, CONTROL_TABLE_CW_ANGLE_LIMIT, 2, data);
+
+
+    _bus_state = SerialState::Writing;
+    rw(1, CONTROL_TABLE_CCW_ANGLE_LIMIT, 2, data);
+    rw(2, CONTROL_TABLE_CCW_ANGLE_LIMIT, 2, data);
+    rw(3, CONTROL_TABLE_CCW_ANGLE_LIMIT, 2, data);
+
+    //relever la position
+    int pas1 = pos1-28672/60;
+    int pas2 = pos2-28672/60;
+    int pas3 = pos3-28672/60;
+
+    for (int i = 0; i < 61; i++) {
+        int goal_position1 = i * pas1 - 28672;
+        if (goal_position1 < 0) {
+            goal_position1 = goal_position1 + 28627 + 36864;
+        }
+        char data1[2];
+        data1[1] = goal_position1%256;
+        data1[0] = goal_position1/256;
+
+        int goal_position2 = i * pas2 - 28672;
+        if (goal_position2 < 0) {
+            goal_position2 = goal_position2 + 28627 + 36864;
+        }
+        char data2[2];
+        data2[1] = goal_position2%256;
+        data2[0] = goal_position2/256;
+
+        int goal_position3 = i * pas3 - 28672;
+        if (goal_position3 < 0) {
+            goal_position3 = goal_position3 + 28627 + 36864;
+        }
+        char data3[2];
+        data3[1] = goal_position3%256;
+        data3[0] = goal_position3/256;
+
+        rw(1, CONTROL_TABLE_GOAL_POSITION, 2, data1);
+        thread_sleep_for(60);
+        rw(2, CONTROL_TABLE_GOAL_POSITION, 2, data2);
+        thread_sleep_for(60);
+        rw(3, CONTROL_TABLE_GOAL_POSITION, 2, data3);
+        thread_sleep_for(60);
+
+        thread_sleep_for(1000);
+    }
+}