T Arai / Futaba_servo140922

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Futaba485.cpp

Committer:
TArai
Date:
2015-12-26
Revision:
0:5387fecc249c

File content as of revision 0:5387fecc249c:

#include "Futaba485.h"
#include "mbed.h"

Futaba::Futaba(PinName tx ,PinName rx, PinName REDE) : _device(tx,rx),_REDE(REDE){
//    tx=p9;
//    rx=p10;
//    REDE = p11;
}

void Futaba::init(void){
    _device.baud(115200);        // baud Rate = 115.2kbps [Futaba default]
    _REDE = 0;                   // RS485 Transmit disable
}


void Futaba::setTmax (unsigned char ID, unsigned char data){
 
    unsigned char TxData[9];    // TransmitByteData [9byte]
    unsigned char CheckSum = 0; // CheckSum calculation
    
    TxData[0] = 0xFA;           // Header
    TxData[1] = 0xAF;           // Header
    TxData[2] = ID;             // ID
    TxData[3] = 0x00;           // Flags
    TxData[4] = 0x23;           // Address
    TxData[5] = 0x01;           // Length
    TxData[6] = 0x01;           // Count
    TxData[7] = data;           // Data
    
    // CheckSum calculation
    for(int i=2; i<=7; i++){
        CheckSum = CheckSum ^ TxData[i];                // XOR from ID to Data
    }
    
    TxData[8] = CheckSum;       // Sum
    
    // Send Packet 
    _REDE = 1;                   // RS485 Transmit Enable
    for(int i=0; i<=8; i++){
        _device.putc(TxData[i]);
    }
    wait_us(250);               // Wait for transmission
    _REDE = 0;                   // RS485 Transmitt disable
}



void Futaba::torqueOn (unsigned char ID){
 
    unsigned char TxData[9];    // TransmitByteData [9byte]
    unsigned char CheckSum = 0; // CheckSum calculation
    
    TxData[0] = 0xFA;           // Header
    TxData[1] = 0xAF;           // Header
    TxData[2] = ID;             // ID
    TxData[3] = 0x00;           // Flags
    TxData[4] = 0x24;           // Address
    TxData[5] = 0x01;           // Length
    TxData[6] = 0x01;           // Count
    TxData[7] = 0x01;           // Data
    
    // CheckSum calculation
    for(int i=2; i<=7; i++){
        CheckSum = CheckSum ^ TxData[i];                // XOR from ID to Data
    }
    
    TxData[8] = CheckSum;       // Sum
    
    // Send Packet 
    _REDE = 1;                   // RS485 Transmit Enable
    for(int i=0; i<=8; i++){
        _device.putc(TxData[i]);
    }
    wait_us(250);               // Wait for transmission
    _REDE = 0;                   // RS485 Transmitt disable
}

void Futaba::torqueOff (unsigned char ID){
 
    unsigned char TxData[9];    // TransmitByteData [9byte]
    unsigned char CheckSum = 0; // CheckSum calculation
    
    TxData[0] = 0xFA;           // Header
    TxData[1] = 0xAF;           // Header
    TxData[2] = ID;             // ID
    TxData[3] = 0x00;           // Flags
    TxData[4] = 0x24;           // Address
    TxData[5] = 0x01;           // Length
    TxData[6] = 0x01;           // Count
    TxData[7] = 0x00;           // Data
    
    // CheckSum calculation
    for(int i=2; i<=7; i++){
        CheckSum = CheckSum ^ TxData[i];                // XOR from ID to Data
    }
    
    TxData[8] = CheckSum;       // Sum
    
    // Send Packet 
    _REDE = 1;                   // RS485 Transmit Enable
    for(int i=0; i<=8; i++){
        _device.putc(TxData[i]);
    }
    wait_us(250);               // Wait for transmission
    _REDE = 0;                   // RS485 Transmitt disable
}




void Futaba::setPosition (unsigned char ID, int data){

    unsigned char TxData[10];   // TransmitByteData [10byte]
    unsigned char CheckSum = 0; // CheckSum calculation
    
    TxData[0] = 0xFA;           // Header
    TxData[1] = 0xAF;           // Header
    TxData[2] = ID;             // ID
    TxData[3] = 0x00;           // Flags
    TxData[4] = 0x1E;           // Address
    TxData[5] = 0x02;           // Length
    TxData[6] = 0x01;           // Count
                                // Data
    TxData[7] = (unsigned char)0x00FF & data;           // Low byte
    TxData[8] = (unsigned char)0x00FF & (data >> 8);    // Hi  byte
    
    // CheckSum calculation
    for(int i=2; i<=8; i++){
        CheckSum = CheckSum ^ TxData[i];                // XOR from ID to Data
    }
    TxData[9] = CheckSum;       // Sum
    // Send Packet
    _REDE = 1;                   // RS485 Transmitt Enable
    for(int i=0; i<=9; i++){
        _device.putc(TxData[i]);
    }
    wait_us(250);               // Wait for transmission
    _REDE = 0;                   // RS485 Transmit disable
}



void Futaba::setPosition_long123 (int data1,int data2, int data3){

    unsigned char TxData[17];   // TransmitByteData [10byte]
    unsigned char CheckSum = 0; // CheckSum calculation
    
    TxData[0] = 0xFA;           // Header
    TxData[1] = 0xAF;           // Header
    TxData[2] = 0x00;             // ID long packet
    TxData[3] = 0x00;           // Flags
    TxData[4] = 0x1E;           // Address
    TxData[5] = 0x03;           // Length
    TxData[6] = 0x03;           // Count
    
    TxData[7] = 0x01;        //Servo ID1
    TxData[8] = (unsigned char)0x00FF & data1;           // Low byte
    TxData[9] = (unsigned char)0x00FF & (data1 >> 8);    // Hi  byte
    
    TxData[10] = 0x02;        //Servo ID2
    TxData[11] = (unsigned char)0x00FF & data2;           // Low byte
    TxData[12] = (unsigned char)0x00FF & (data2 >> 8);    // Hi  byte
    
    TxData[13] = 0x03;        //Servo ID3
    TxData[14] = (unsigned char)0x00FF & data3;           // Low byte
    TxData[15] = (unsigned char)0x00FF & (data3 >> 8);    // Hi  byte
    
    // CheckSum calculation
    for(int i=2; i<=15; i++){
        CheckSum = CheckSum ^ TxData[i];                // XOR from ID to Data
    }
    TxData[16] = CheckSum;       // Sum
    // Send Packet
    _REDE = 1;                   // RS485 Transmitt Enable
    for(int i=0; i<=16; i++){
        _device.putc(TxData[i]);
    }
    wait_us(500);               // Wait for transmission
    _REDE = 0;                   // RS485 Transmit disable
}





void Futaba::setTslope (unsigned char ID, unsigned char CW, unsigned char CCW){
 
    unsigned char TxData[10];    // TransmitByteData [9byte]
    unsigned char CheckSum = 0; // CheckSum calculation
    
    TxData[0] = 0xFA;           // Header
    TxData[1] = 0xAF;           // Header
    TxData[2] = ID;             // ID
    TxData[3] = 0x00;           // Flags
    TxData[4] = 0x1A;           // Address
    TxData[5] = 0x02;           // Length
    TxData[6] = 0x01;           // Count
    TxData[7] = CW;           // Data
    TxData[8] = CCW;           // Data    
    // CheckSum calculation
    for(int i=2; i<=8; i++){
        CheckSum = CheckSum ^ TxData[i];                // XOR from ID to Data
    }
    
    TxData[9] = CheckSum;       // Sum
    
    // Send Packet 
    _REDE = 1;                   // RS485 Transmit Enable
    for(int i=0; i<=9; i++){
        _device.putc(TxData[i]);
    }
    wait_us(250);               // Wait for transmission
    _REDE = 0;                   // RS485 Transmitt disable
}