Merge pull request #8 from officinerobotiche/feature/message_pin_conf

Feature/message pin conf

src/it/officinerobotiche/serial/frame/AbstractFrame.java

@@ -280,6 +280,29 @@ public static int byteArrayToInt(byte[] b, int offset) {
         return buf.getInt();
     }
 
+    /**
+     * Convert from byte array in boolean variable.
+     *
+     * @param b byte array.
+     * @param offset offset.
+     * @return boolean value associated.
+     */
+    public static boolean byteArrayToBoolean(byte[] b, int offset) {
+        return (byteArrayToInt8(b, offset) == 1);
+    }
+
+    /**
+     * Convert from byte array in boolean variable.
+     *
+     * @param b byte array.
+     * @param offset offset.
+     * @return integer value associated.
+     */
+    public static int byteArrayToInt8(byte[] b, int offset) {
+        ByteBuffer buf = ByteBuffer.wrap(b, offset, 1).order(ByteOrder.LITTLE_ENDIAN);
+        return buf.getInt();
+    }
+
     /**
      * Convert from byte array in float variable.
      *
@@ -308,6 +331,35 @@ public static float[] byteArrayToFloatArray(byte[] b, int offset, int count) {
         return val;
     }
 
+    /**
+     * Convert boolean value and ad in array.
+     *
+     * @param in array to add converted integer value.
+     * @param offset offset to put the variable.
+     * @param value integer variable.
+     * @return the same byte array variable in with boolean value converted.
+     */
+    public static byte[] booleanToByteArray(byte[] in, int offset, boolean value) {
+        byte conv = (byte) (value ? 1 : 0 );
+        byte[] array = ByteBuffer.allocate(1).put((byte) conv).array();
+        System.arraycopy(array, 0, in, offset, array.length);
+        return in;
+    }
+
+        /**
+     * Convert boolean value and ad in array.
+     *
+     * @param in array to add converted integer value.
+     * @param offset offset to put the variable.
+     * @param value integer variable.
+     * @return the same byte array variable in with boolean value converted.
+     */
+    public static byte[] int8ToByteArray(byte[] in, int offset, int value) {
+        byte[] array = ByteBuffer.allocate(1).put((byte) value).array();
+        System.arraycopy(array, 0, in, offset, array.length);
+        return in;
+    }
+
     /**
      * Convert integer value and ad in array.
      *

src/it/officinerobotiche/serial/frame/motion/MotionFrame.java

@@ -59,31 +59,39 @@ public enum Command implements ICommand {
         /**
          * Parameter motors command.
          */
-        PARAM_MOTORS(5, "ParamMotors"),
+        PARAM_UNICYCLE(5, "ParamUnicycle"),
+        /**
+         * Parameter motors command.
+         */
+        PARAM_MOTOR_L(6, "ParamMotor"),
+        /**
+         * Parameter motors command.
+         */
+        PARAM_MOTOR_R(7, "ParamMotor"),
         /**
          * Contraint command.
          */
-        CONSTRAINT(6, "Constraint"),
+        CONSTRAINT(8, "Constraint"),
         /**
          * Velocity reference command.
          */
-        VELOCITY(7, "Velocity"),
+        VELOCITY(9, "Velocity"),
         /**
          * Velocity measured command.
          */
-        VELOCITY_MIS(8, "Velocity"),
+        VELOCITY_MIS(10, "Velocity"),
         /**
          * Enable bridge command.
          */
-        ENABLE(9, "Enable"),
+        ENABLE(11, "Enable"),
         /**
          * Emergency configuration command.
          */
-        EMERGENCY(10, "Emergency"),
+        EMERGENCY(12, "Emergency"),
         /**
          * Relative odometry command.
          */
-        DELTA_ODO(11, "DeltaOdo");
+        DELTA_ODO(13, "DeltaOdo");
 
         /**
          * Number command.

src/it/officinerobotiche/serial/frame/motion/ParamMotor.java

@@ -0,0 +1,275 @@
+/*
+ * Copyright (C) 2014 Officine Robotiche.
+ *
+ * All rights reserved. This program and the accompanying materials
+ * are made available under the terms of the GNU Lesser General Public License
+ * (LGPL) version 2.1 which accompanies this distribution, and is available at
+ * http://www.gnu.org/licenses/lgpl-2.1.html
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * Contributors:
+ *     Raffaello Bonghi - raffaello.bonghi@officinerobotiche.it
+ */
+package it.officinerobotiche.serial.frame.motion;
+
+import it.officinerobotiche.serial.frame.AbstractFrame;
+
+/**
+ * Definition to save or load motor parameter from board. You can set gain to
+ * convert value read from Input capture or QEI. Used IS to configure this data,
+ * in particular meter, radiand, radians to sec.
+ *
+ * @author Raffaello Bonghi
+ */
+public class ParamMotor extends MotionFrame {
+
+    /**
+     * Quick definition for Parameter motor LEFT frame message. This is usefull
+     * on require a information on board.
+     */
+    public static final ParamMotor LEFT = new ParamMotor(Command.PARAM_MOTOR_L);
+
+    /**
+     * Quick definition for Parameter motor RIGHT frame message. This is usefull
+     * on require a information on board.
+     */
+    public static final ParamMotor RIGHT = new ParamMotor(Command.PARAM_MOTOR_R);
+
+    /**
+     * Class definition for Parameter motor LEFT. You can set to particular
+     * Parameter motor configuration.
+     */
+    public static class ParamMotorLeft extends ParamMotor {
+
+        /**
+         * Initialize Parameter motor LEFT message. This Constructor is used for
+         * require data from board.
+         */
+        public ParamMotorLeft() {
+            super(Command.PARAM_MOTOR_L);
+        }
+
+        /**
+         * Initialize Parameter motor LEFT message with K_Ang, K_Vel, default
+         * value enable h-bridge and enableSwap confguration parameters. This
+         * Constructor is used for require data from board.
+         *
+         * @param kAng Gain to convert QEI value to angular position.
+         * @param kVel Gain to convert Input Capture value to angular velocity motor.
+         * @param versus Set default orientation versus motor.
+         * @param enable Set default level configuration h-bridge enable.
+         */
+        public ParamMotorLeft(float kAng, float kVel, int versus, boolean enable) {
+            super(Command.PARAM_MOTOR_L);
+            this.kAng = kAng;
+            this.kVel = kVel;
+            this.versus = versus;
+            this.enable = enable;
+            buildData();
+        }
+    }
+
+    /**
+     * Class definition for PID RIGHT. You can set to particular PID a set
+     * configuration.
+     */
+    public static class ParamMotorRight extends ParamMotor {
+
+        /**
+         * Initialize PID RIGHT message. This Constructor is used for require
+         * data from board.
+         */
+        public ParamMotorRight() {
+            super(Command.PARAM_MOTOR_R);
+        }
+
+        /**
+         * Initialize Parameter motor RIGHT message with K_Ang, K_Vel, default
+         * value enable h-bridge and enableSwap confguration parameters. This
+         * Constructor is used for require data from board.
+         *
+         * @param kAng Gain to convert QEI value to angular position.
+         * @param kVel Gain to convert Input Capture value to angular velocity motor.
+         * @param versus Set default orientation versus motor.
+         * @param enable Set default level configuration h-bridge enable.
+         */
+        public ParamMotorRight(float kAng, float kVel, int versus, boolean enable) {
+            super(Command.PARAM_MOTOR_R);
+            this.kAng = kAng;
+            this.kVel = kVel;
+            this.versus = versus;
+            this.enable = enable;
+            buildData();
+        }
+    }
+
+    /**
+     * Length of PID frame. Tree floats for all data.
+     */
+    private static final int LNG_PARAM_MOTOR = 4 * 2 + 1 * 2;
+
+    /**
+     * Set type of command received.
+     */
+    private final Command comm;
+
+    /**
+     * Proportional gain to convert angular position.
+     */
+    protected float kAng;
+
+    /**
+     * Proportional gain to convert angular velocity.
+     */
+    protected float kVel;
+
+    /**
+     * Set default value bridge in off (Normally high level voltage or
+     * viceversa).
+     */
+    protected boolean enable;
+
+    /**
+     * Set encoder velocity read to swap configuration.
+     */
+    protected int versus;
+
+    /**
+     * Initialize parameter motors message. This Constructor is used for require
+     * data from board.
+     *
+     * @param comm Type of command required.
+     */
+    protected ParamMotor(Command comm) {
+        super();
+        this.comm = comm;
+        this.in = new byte[LNG_PARAM_MOTOR];
+    }
+
+    /**
+     * Initialize message with data received from board. This message is used
+     * normally from parser to set data received, it is a message with data.
+     *
+     * @param sync type of packet received (syncronous or not).
+     * @param command type command received.
+     * @param in byte received.
+     */
+    public ParamMotor(boolean sync, int command, byte[] in) {
+        super(sync, command, in);
+        this.comm = Command.getCommand(command);
+        this.kAng = AbstractFrame.byteArrayToFloat(in, 0);
+        this.kVel = AbstractFrame.byteArrayToFloat(in, 4);
+        this.versus = AbstractFrame.byteArrayToInt8(in, 8);
+        this.enable = AbstractFrame.byteArrayToBoolean(in, 9);
+    }
+
+    /**
+     * Initialize message with ACK, NACK information. This message is used
+     * normally from parser.
+     *
+     * @param sync type of packet received (syncronous or not).
+     * @param command type command received.
+     * @param info Information about message.
+     */
+    public ParamMotor(boolean sync, int command, Information info) {
+        super(sync, command, info);
+        this.comm = Command.getCommand(command);
+    }
+
+    @Override
+    public Command getCommand() {
+        return comm;
+    }
+
+    /**
+     * Construct byte data array with information on object.
+     */
+    @Override
+    protected void buildData() {
+        this.in = AbstractFrame.floatToByteArray(in, 0, kAng);
+        this.in = AbstractFrame.floatToByteArray(in, 4, kVel);
+        this.in = AbstractFrame.int8ToByteArray(in, 8, versus);
+        this.in = AbstractFrame.booleanToByteArray(in, 9, enable);
+    }
+
+    /**
+     * Get the value of versus
+     *
+     * @return the value of versus
+     */
+    public int getVersus() {
+        return versus;
+    }
+
+    /**
+     * Set the value of versus
+     *
+     * @param versus new value of versus
+     */
+    public void setVersus(int versus) {
+        this.versus = versus;
+        buildData();
+    }
+
+    /**
+     * Get the value of enable
+     *
+     * @return the value of enable
+     */
+    public boolean isEnable() {
+        return enable;
+    }
+
+    /**
+     * Set the value of enable
+     *
+     * @param enable new value of enable
+     */
+    public void setEnable(boolean enable) {
+        this.enable = enable;
+        buildData();
+    }
+
+    /**
+     * Get the value of kVel
+     *
+     * @return the value of kVel
+     */
+    public float getkVel() {
+        return kVel;
+    }
+
+    /**
+     * Set the value of kVel
+     *
+     * @param kVel new value of kVel
+     */
+    public void setkVel(float kVel) {
+        this.kVel = kVel;
+        buildData();
+    }
+
+    /**
+     * Get the value of kAng
+     *
+     * @return the value of kAng
+     */
+    public float getkAng() {
+        return kAng;
+    }
+
+    /**
+     * Set the value of kAng
+     *
+     * @param kAng new value of kAng
+     */
+    public void setkAng(float kAng) {
+        this.kAng = kAng;
+        buildData();
+    }
+
+}