Diff: AX12/AX12.cpp

Revision:

0:23b97841e8dc

Child:

1:2759fc78aa04

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AX12/AX12.cpp	Wed Aug 29 09:45:32 2018 +0000
@@ -0,0 +1,627 @@
+/* mbed AX-12+ Servo Library
+ *
+ * Copyright (c) 2010, cstyles (http://mbed.org)
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include "AX12.h"
+#include "mbed.h"
+
+AX12::AX12 (PinName tx, PinName rx, PinName tx_ena, int ID, int baud) : _ax12(tx,rx), _ena_tx(tx_ena, 1)
+{
+    _ax12.baud(baud);
+    _baud_bit = 1000000/baud;
+    _wait_sent = 20 * _baud_bit;
+    _timeout_tick = 500 + 12 * _baud_bit;
+    _ID = ID;
+    _error_code = 0;
+    _data[0] = 0;
+    _reg_flag = 0;
+}
+
+// Set the mode of the servo
+//  0 = Positional (0-300 degrees)
+//  1 = Rotational -1 to 1 speed
+int AX12::SetMode(int mode) 
+{
+    if (mode == 1) 
+    { // set CR
+        SetCWLimit(0);
+        SetCCWLimit(0);
+        SetCRSpeed(0.0);
+    } 
+    else 
+    {
+        SetCWLimit(0);
+        SetCCWLimit(300);
+        SetCRSpeed(0.0);
+    }
+    return(0);
+}
+
+// if flag[0] is set, were blocking
+// if flag[1] is set, we're registering
+// they are mutually exclusive operations
+int AX12::SetGoal(int degrees, int flags) 
+{
+    _reg_flag = 0;
+
+    // set the flag is only the register bit is set in the flag
+    if (flags == 0x2) 
+    {
+        _reg_flag = 1;
+    }
+
+    // 1023 / 300 * degrees
+    short goal = (1023 * degrees) / 300;
+
+    _data[0] = goal & 0xff; // bottom 8 bits
+    _data[1] = goal >> 8;   // top 8 bits
+
+    // write the packet, return the error code
+    int rVal = write(_ID, AX12_REG_GOAL_POSITION, 2, _data, _reg_flag);
+
+    if (flags == 1) 
+    {
+        // block until it comes to a halt
+        while (isMoving()) 
+        {
+
+        }
+    }
+    return(rVal);
+}
+
+void AX12::MultSetGoal(int degrees1, float speed1, 
+                        int degrees2, float speed2, 
+                        int degrees3, float speed3, 
+                        int degrees4, float speed4, 
+                        int degrees5, float speed5, 
+                        int degrees6, float speed6, 
+                        int degrees7, float speed7, 
+                        int degrees8, float speed8, 
+                        int degrees9, float speed9, 
+                        int degrees10, float speed10, 
+                        int degrees11, float speed11,
+                        int degrees12, float speed12, 
+                        int degrees13, float speed13, 
+                        int degrees14, float speed14, 
+                        int degrees15, float speed15, 
+                        int degrees16, float speed16, 
+                        int degrees17, float speed17, 
+                        int degrees18, float speed18)
+{
+     char data[(total_ID*length_data) + 1];
+
+    int goal;
+    int goal_speed;
+
+    int alamat;
+
+    int degrees[19];
+    float speed[19];
+   
+    // assignment value from parameter to array
+
+    degrees[1] = degrees1; 
+    degrees[2] = degrees2; 
+    degrees[3] = degrees3; 
+    degrees[4] = degrees4; 
+    degrees[5] = degrees5; 
+    degrees[6] = degrees6; 
+    degrees[7] = degrees7; 
+    degrees[8] = degrees8; 
+    degrees[9] = degrees9; 
+    degrees[10] = degrees10;
+    degrees[11] = degrees11;
+    degrees[12] = degrees12;
+    degrees[13] = degrees13;
+    degrees[14] = degrees14;
+    degrees[15] = degrees15;
+    degrees[16] = degrees16;
+    degrees[17] = degrees17;
+    degrees[18] = degrees18;
+    
+    speed[1] = speed1; 
+    speed[2] = speed2; 
+    speed[3] = speed3; 
+    speed[4] = speed4; 
+    speed[5] = speed5; 
+    speed[6] = speed6; 
+    speed[7] = speed7; 
+    speed[8] = speed8; 
+    speed[9] = speed9; 
+    speed[10] = speed10;
+    speed[11] = speed11;
+    speed[12] = speed12;
+    speed[13] = speed13;
+    speed[14] = speed14;
+    speed[15] = speed15;
+    speed[16] = speed16;
+    speed[17] = speed17;
+    speed[18] = speed18;
+
+/*
+    for (char k=1; k<=total_ID; k++)
+    {
+        degrees[k] = degrees1;
+        speed[k] = speed1;
+    }
+*/
+
+    for (int i=1; i<=total_ID; i++)
+    {
+        alamat = length_data*(i-1);
+
+        goal = (1023 * degrees[i]) / 300;
+
+        data[alamat+1] = goal & 0xff; // bottom 8 bits
+        data[alamat+2] = goal >> 8;   // top 8 bits
+
+        float temp = (speed[i]<0)? (-1)*speed[i]:speed[i];
+        goal_speed = (0x3ff * temp);
+
+        // Set direction CW if we have a negative speed
+        if (speed[i] < 0) 
+        {
+            goal_speed |= (0x1 << 10);
+        }
+
+        data[alamat+3] = goal_speed & 0xff; // bottom 8 bits
+        data[alamat+4] = goal_speed >> 8;   // top 8 bits
+    }
+
+    // Write
+    sync_write(data);
+
+    return;
+}
+
+// Set continuous rotation speed from -1 to 1
+int AX12::SetCRSpeed(float speed) 
+{
+    // bit 10     = direction, 0 = CCW, 1=CW
+    // bits 9-0   = Speed
+    float temp = (speed<0)? speed * (-1): speed;
+    int goal = (0x3ff * temp);
+
+    // Set direction CW if we have a negative speed
+    if (speed < 0) 
+    {
+        goal |= (0x1 << 10);
+    }
+
+    _data[0] = goal & 0xff; // bottom 8 bits
+    _data[1] = goal >> 8;   // top 8 bits
+
+    // write the packet, return the error code
+    int rVal = write(_ID, AX12_REG_MOVING_SPEED, 2, _data);
+
+    return(rVal);
+}
+
+int AX12::SetCWLimit (int degrees) 
+{
+    // 1023 / 300 * degrees
+    short limit = (1023 * degrees) / 300;
+
+    _data[0] = limit & 0xff; // bottom 8 bits
+    _data[1] = limit >> 8;   // top 8 bits
+
+    // write the packet, return the error code
+    return (write(_ID, AX12_REG_CW_LIMIT, 2, _data));
+}
+
+int AX12::SetCCWLimit (int degrees) 
+{
+    // 1023 / 300 * degrees
+    short limit = (1023 * degrees) / 300;
+
+    _data[0] = limit & 0xff; // bottom 8 bits
+    _data[1] = limit >> 8;   // top 8 bits
+
+    // write the packet, return the error code
+    return (write(_ID, AX12_REG_CCW_LIMIT, 2, _data));
+}
+
+void AX12::ControlID (int ID) 
+{
+    _ID = ID;
+
+    return;
+}
+
+int AX12::SetID (int CurrentID, int NewID) 
+{
+    _data[0] = NewID;
+    return (write(CurrentID, AX12_REG_ID, 1, _data));
+}
+
+int AX12::SetBaud (int baud) 
+{
+    _data[0] = baud;
+    return (write(0xFE, AX12_REG_BAUD, 1, _data));
+}
+
+// return 1 is the servo is still in flight
+int AX12::isMoving(void) 
+{
+    read(_ID,AX12_REG_MOVING,1,_data);
+    return(_data[0]);
+}
+
+void AX12::trigger(void) 
+{
+    char TxBuf[16];
+    char sum = 0;
+
+    TxBuf[0] = 0xFF;
+    TxBuf[1] = 0xFF;
+
+    // ID - Broadcast
+    TxBuf[2] = 0xFE;
+    sum += TxBuf[2];
+
+    // Length
+    TxBuf[3] = 0x02;
+    sum += TxBuf[3];
+
+    // Instruction - ACTION
+    TxBuf[4] = 0x04;
+    sum += TxBuf[4];
+
+    // Checksum
+    TxBuf[5] = 0xFF - sum;
+
+    // Transmit the packet in one burst with no pausing
+    for (int i = 0; i < 6 ; i++) {
+        _ax12.putc(TxBuf[i]);
+    }
+
+    // This is a broadcast packet, so there will be no reply
+    return;
+}
+
+float AX12::GetPosition(void) 
+{
+    _error_code = read(_ID, AX12_REG_POSITION, 2, _data);
+    return ((short)(_data[0] + (_data[1] << 8)) * 300.0f / 1024.0f);
+}
+
+float AX12::GetTemp (void) 
+{
+    _error_code = read(_ID, AX12_REG_TEMP, 1, _data);
+    return(_data[0]);
+}
+
+float AX12::GetVolts (void) 
+{
+    _error_code = read(_ID, AX12_REG_VOLTS, 1, _data);
+    return(_data[0]/10.0f);
+}
+
+int AX12::read(int ID, int start, int bytes, char* data) 
+{
+    char PacketLength = 0x4;
+    char TxBuf[8];
+    char sum = 0;
+//    char Status[6 + bytes];
+
+    Status[4] = 0xFE; // return code
+
+    // Build the TxPacket first in RAM, then we'll send in one go
+
+    TxBuf[0] = 0xff;
+    TxBuf[1] = 0xff;
+
+    // ID
+    TxBuf[2] = ID;
+    sum += TxBuf[2];
+
+    // Packet Length
+    TxBuf[3] = PacketLength;    // Length = 4 ; 2 + 1 (start) = 1 (bytes)
+    sum += TxBuf[3];            // Accululate the packet sum
+
+    // Instruction - Read
+    TxBuf[4] = 0x2;
+    sum += TxBuf[4];
+
+    // Start Address
+    TxBuf[5] = start;
+    sum += TxBuf[5];
+
+    // Bytes to read
+    TxBuf[6] = bytes;
+    sum += TxBuf[6];
+
+    // Checksum
+    TxBuf[7] = 0xFF - sum;
+
+    // Transmit the packet in one burst with no pausing
+    _ena_tx = 1;
+    for (int i = 0; i<8 ; i++) 
+    {
+        _ax12.putc(TxBuf[i]);
+    }
+
+    // Wait for the bytes to be transmitted
+    wait_us (_wait_sent);
+    _ena_tx = 0;
+
+    // Skip if the read was to the broadcast address
+    if (_ID != 0xFE) 
+    {
+        // response packet is always 6 + bytes
+        // 0xFF, 0xFF, ID, Length Error, Param(s) Checksum
+        // timeout is a little more than the time to transmit
+        // the packet back, i.e. (6+bytes)*10 bit periods
+        int plen = 0;
+        int timeout = 0;
+        int state = 0;
+        while ((timeout < _timeout_tick) && (plen < (bytes + 3)))
+        {
+            if (_ax12.readable()) 
+            {
+                switch(state)
+                {
+                    case 0:
+                    case 1:
+                        Status[state] = _ax12.getc();
+                        if(Status[state] == 0xFF)
+                        {
+                            state++;
+                            timeout = 0;
+                        }
+                        else
+                            state = 0;
+                        break;
+                    case 2:
+                        Status[state] = _ax12.getc();
+                        if(Status[state] == ID)
+                        {
+                            state++;
+                            timeout = 0;
+                        }
+                        else
+                            state = 0;
+                        break;
+                    case 3:
+                        Status[plen+state] = _ax12.getc();
+                        plen++;
+                        timeout = 0;
+                        break;
+                    default:
+                        state = 0;
+                }
+            }
+
+            // wait for the bit period
+            wait_us(1);
+            timeout++;
+        }
+
+        if (timeout == 550)
+        {
+            return(-1);
+        }
+
+        // Copy the data from Status into data for return
+        for (int i=0; i < Status[3]-2 ; i++) 
+        {
+            data[i] = Status[5+i];
+        }
+    } 
+
+    // if (ID!=0xFE)
+    return(Status[4]);
+}
+
+int AX12::write(int ID, int start, int bytes, char* data, int flag) {
+// 0xff, 0xff, ID, Length, Intruction(write), Address, Param(s), Checksum
+
+//    char TxBuf[7+bytes];
+    char sum = 0;
+    char Status[6];
+
+    // Build the TxPacket first in RAM, then we'll send in one go
+
+    TxBuf[0] = 0xff;
+    TxBuf[1] = 0xff;
+
+    // ID
+    TxBuf[2] = ID;
+    sum += TxBuf[2];
+
+    // packet Length
+    TxBuf[3] = 3+bytes;
+    sum += TxBuf[3];
+
+    // Instruction
+    if (flag == 1) 
+    {
+        TxBuf[4]=0x04;
+        sum += TxBuf[4];
+    } 
+    else 
+    {
+        TxBuf[4]=0x03;
+        sum += TxBuf[4];
+    }
+
+    // Start Address
+    TxBuf[5] = start;
+    sum += TxBuf[5];
+
+    // data
+    for (uint8_t i=0; i<bytes ; i++)
+    {
+        TxBuf[6+i] = data[i];
+        sum += TxBuf[6+i];
+    }
+
+    // checksum
+    TxBuf[6+bytes] = 0xFF - sum;
+
+    // Transmit the packet in one burst with no pausing
+    _ena_tx = 1;
+    for (int i = 0; i < (7 + bytes) ; i++) 
+    {
+        _ax12.putc(TxBuf[i]);
+    }
+
+    // Wait for data to transmit
+    wait_us (_wait_sent);
+    _ena_tx = 0;
+
+    // make sure we have a valid return
+    Status[4]=0x00;
+
+    // we'll only get a reply if it was not broadcast
+    if (_ID!=0xFE) 
+    {
+        // response packet is always 6 bytes
+        // 0xFF, 0xFF, ID, Length Error, Param(s) Checksum
+        // timeout is a little more than the time to transmit
+        // the packet back, i.e. 60 bit periods, round up to 100
+        int timeout = 0;
+        int state = 0;
+        int plen = 0;
+        while ((timeout < _timeout_tick) && (plen<3))
+        {
+            if (_ax12.readable()) 
+            {
+                switch(state)
+                {
+                    case 0:
+                    case 1:
+                        Status[state] = _ax12.getc();
+                        if(Status[state] == 0xFF)
+                        {
+                            state++;
+                            timeout = 0;
+                        }
+                        else
+                            state = 0;
+                        break;
+                    case 2:
+                        Status[state] = _ax12.getc();
+                        if(Status[state] == ID)
+                        {
+                            state++;
+                            timeout = 0;
+                        }
+                        else
+                            state = 0;
+                        break;
+                    case 3:
+                        Status[plen+state] = _ax12.getc();
+                        plen++;
+                        timeout = 0;
+                        break;
+                    default:
+                        state = 0;
+                }
+            }
+
+            // wait for the bit period
+            wait_us (1);
+            timeout++;
+        }
+
+        // Build the TxPacket first in RAM, then we'll send in one go
+    }
+    return(Status[4]); // return error code
+}
+
+int AX12::sync_write(char* data) 
+{
+// 0xff, 0xff, ID, Length, Intruction(write), Address, Param(s), Checksum
+
+    char sum = 0;
+    char Status[6];
+    char TxBuf[(total_ID*(length_data+1)) + 8];
+    
+    int pra_alamat, alamat;
+
+    // Set ID
+    char ID[total_ID+1];
+    
+    for (uint8_t k=1; k<=total_ID; k++)
+    {
+        ID[k] = k;
+    }
+
+    TxBuf[0] = 0xFF;
+    TxBuf[1] = 0xFF;
+
+    // ID
+    TxBuf[2] = 0xFE;
+    sum += TxBuf[2];
+
+    // packet Length
+    TxBuf[3] = ((length_data+1) * total_ID) + 4;
+    sum += TxBuf[3];
+
+    // Instruction
+    TxBuf[4]=0x83;
+    sum += TxBuf[4];
+
+    // Start Address
+    TxBuf[5] = start_reg;
+    sum += TxBuf[5];
+
+    // Length of Data to Write
+    TxBuf[6] = length_data;
+    sum += TxBuf[6];
+
+    for (uint8_t j=1; j<=total_ID; j++)
+    {
+        pra_alamat = length_data*(j-1);
+        alamat = 6 + j + pra_alamat;
+    
+        TxBuf[alamat] = ID[j];
+        sum += TxBuf[alamat];
+
+        for (char i=1; i<=length_data; i++)
+        {
+            TxBuf[alamat+i] = data[pra_alamat+i];
+            sum += TxBuf[alamat+i];
+        }
+    }
+
+    // checksum
+    TxBuf[((total_ID*(length_data+1)) + 7)] = 0xFF - (sum & 0xFF);
+
+    // Transmit the packet in one burst with no pausing
+    _ena_tx = 1;
+    for (int i = 0; i <= ((total_ID*(length_data+1)) + 7) ; i++) 
+    {
+        _ax12.putc(TxBuf[i]);
+    }
+
+    // Wait for data to transmit
+    wait_us (_wait_sent);
+    _ena_tx = 0;
+
+    // make sure we have a valid return
+    Status[4]=0x00;
+
+    return(Status[4]); // return error code
+}