All Downloads are FREE. Search and download functionalities are using the official Maven repository.

org.robokind.impl.motion.openservo.OpenServoCommandSet Maven / Gradle / Ivy

There is a newer version: 0.9.5
Show newest version
/*
 * Copyright 2012 Hanson Robokind LLC.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
package org.robokind.impl.motion.openservo;

import org.robokind.api.common.utils.Utils;

/**
 *
 * @author matt
 */
public class OpenServoCommandSet {    
    public static enum Command{
        RESET(0x80),
        CHECKED_TXN(0x81),
        PWM_ENABLE(0x82),
        PWM_DISABLE(0x83),
        WRITE_ENABLE(0x84),
        WRITE_DISABLE(0x85),
        REGISTERS_SAVE(0x86),
        REGISTERS_RESTORE(0x87),
        REGISTERS_DEFAULT(0x88),
        EEPROM_ERASE(0x89),
        VOLTAGE_READ(0x90),
        CURVE_MOTION_ENABLE(0x91),
        CURVE_MOTION_DISABLE(0x92),
        CURVE_MOTION_RESET(0x93),
        CURVE_MOTION_APPEND(0x94);        
        private byte myCommand;

        private Command(int cmd) {
            myCommand = (byte)cmd;
        }
        
        public byte getCommand(){
            return myCommand;
        }
    }
    
    public static enum Register{
        POSITION_HI(0x08),
        POSITION_LO(0x09),
        VELOCITY_HI(0x0A),
        VELOCITY_LO(0x0B),
        POWER_HI(0x0C),
        POWER_LO(0x0D),
        PWM_CW(0x0E),
        PWM_CCW(0x0F),
        SEEK_HI(0x10),
        SEEK_LO(0x11),
        SEEK_VELOCITY_HI(0x12),
        SEEK_VELOCITY_LO(0x13),
        VOLTAGE_HI(0x14),
        VOLTAGE_LO(0x15),
        PID_DEADBAND(0x21),
        PID_PGAIN_HI(0x22),
        PID_PGAIN_LO(0x23),
        PID_DGAIN_HI(0x24),
        PID_DGAIN_LO(0x25),
        PID_IGAIN_HI(0x26),
        PID_IGAIN_LO(0x27),   
        REVERSE(48),
        PULSE_CONTROL_ENABLED(51);     
        private byte myRegister;

        private Register(int cmd) {
            myRegister = (byte)cmd;
        }
        
        public byte getRegister(){
            return myRegister;
        }
    }
    
    public static byte[] sendCommands(byte rs485Addr, byte i2cAddr, Command...commands) {
        byte[] bytes = new byte[7+commands.length+1];
        bytes[0] = (byte)0xff;
        bytes[1] = (byte)0xff;
        bytes[2] = rs485Addr;
        bytes[3] = (byte)(3+commands.length+1);
        bytes[4] = (byte)(1+commands.length);
        bytes[5] = (byte)0;
        bytes[6] = (byte)(i2cAddr << 1);
        int i=7;
        for(Command cmd : commands){
            bytes[i++] = cmd.getCommand();
        }
        bytes[bytes.length-1] = Utils.checksum(bytes, 2, bytes.length-3, true);
        return bytes;
    }
    
    public static byte[] writeRegisters(byte rs485Addr, byte i2cAddr, Register firstRegister, byte...data){
        byte[] bytes = new byte[8+data.length+1];
        bytes[0] = (byte)0xff;
        bytes[1] = (byte)0xff;
        bytes[2] = rs485Addr;
        bytes[3] = (byte)(4+data.length+1);
        bytes[4] = (byte)(2+data.length);
        bytes[5] = (byte)0;
        bytes[6] = (byte)(i2cAddr << 1);
        bytes[7] = firstRegister.getRegister();
        System.arraycopy(data, 0, bytes, 8, data.length);
        bytes[bytes.length-1] = Utils.checksum(bytes, 2, bytes.length-3, true);
        return bytes;
    }
    
    public static byte[] readRegisters(int rs485Addr, int i2cAddr, Register firstRegister, int readLen){
        int packetLength = 6;
        byte[] bytes = new byte[10];
        bytes[0] = (byte)0xff;
        bytes[1] = (byte)0xff;
        bytes[2] = (byte)rs485Addr;
        bytes[3] = (byte)packetLength;
        bytes[4] = (byte)2;
        bytes[5] = (byte)readLen;
        bytes[6] = (byte)(i2cAddr << 1);
        bytes[7] = firstRegister.getRegister();
        bytes[8] = (byte)((i2cAddr << 1)|1);
        bytes[9] = Utils.checksum(bytes, 2, bytes.length-3, true);
        return bytes;
    }
    
    public static byte[] move(byte rs485Addr, byte i2cAddr, int pos){
        byte[] bytes = new byte[2];
        bytes[0] = (byte)((pos >> 8) & 0xFF);
        bytes[1] = (byte)(pos & 0xFF);
        return writeRegisters(rs485Addr, i2cAddr, Register.SEEK_HI, bytes);
    }
}




© 2015 - 2025 Weber Informatics LLC | Privacy Policy