![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
tes ir atas semua
Dependencies: mbed ADS1115 StepperMotor SRF05 TPA81new
AX12/AX12.cpp
- Committer:
- hisyamfs
- Date:
- 2019-03-06
- Revision:
- 49:d23d76689933
- Parent:
- 12:1e3227a6fcd7
File content as of revision 49:d23d76689933:
/* 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) { _ena_tx = 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 } void AX12::SetTorqueEnable(char val) { // write(int ID, int start, int length, char* data, int flag=0); char en = val; write(_ID, AX12_TORQUE_ENABLE, 1, &en, 0); }