/** * Constructs the IMU class, overriding the default update rate * with a custom rate which may be from 4 to 100, representing * the number of updates per second sent by the nav6 IMU. * * Note that increasing the update rate may increase the * CPU utilization. * @param serial_port BufferingSerialPort object to use * @param update_rate_hz Custom Update Rate (Hz) */ public IMU(BufferingSerialPort serial_port, byte update_rate_hz) { ypr_update_data = new IMUProtocol.YPRUpdate(); this.update_rate_hz = update_rate_hz; flags = 0; accel_fsr_g = DEFAULT_ACCEL_FSR_G; gyro_fsr_dps = DEFAULT_GYRO_FSR_DPS; this.serial_port = serial_port; yaw_history = new float[YAW_HISTORY_LENGTH]; yaw = (float) 0.0; pitch = (float) 0.0; roll = (float) 0.0; try { serial_port.reset(); } catch (VisaException ex) { ex.printStackTrace(); } initIMU(); m_thread = new Thread(this, "nav6 IMU"); m_thread.start(); }
protected void initIMU() { // The nav6 IMU serial port configuration is 8 data bits, no parity, one stop bit. // No flow control is used. // Conveniently, these are the defaults used by the WPILib's SerialPort class. // // In addition, the WPILib's SerialPort class also defaults to: // // Timeout period of 5 seconds // Termination ('\n' character) // Transmit immediately initializeYawHistory(); user_yaw_offset = 0; // set the nav6 into the desired update mode byte stream_command_buffer[] = new byte[256]; int packet_length = IMUProtocol.encodeStreamCommand( stream_command_buffer, update_type, update_rate_hz ); try { serial_port.write( stream_command_buffer, packet_length ); } catch (VisaException ex) { } }
public Drivetrain() { frontModule = new SwerveModule(RobotMap.frontWheelEncoder, RobotMap.frontModuleEncoder, RobotMap.frontWheelMotor, RobotMap.frontModuleMotor, RobotMap.WHEEL_TOP_ABSOLUTE_SPEED, 0, RobotMap.FRONT_BACK_LENGTH/2, "frontModule"); leftModule = new SwerveModule(RobotMap.leftWheelEncoder, RobotMap.leftModuleEncoder, RobotMap.leftWheelMotor, RobotMap.leftModuleMotor, RobotMap.WHEEL_TOP_ABSOLUTE_SPEED, -RobotMap.LEFT_RIGHT_WIDTH/2, 0, "leftModule"); backModule = new SwerveModule(RobotMap.backWheelEncoder, RobotMap.backModuleEncoder, RobotMap.backWheelMotor, RobotMap.backModuleMotor, RobotMap.WHEEL_TOP_ABSOLUTE_SPEED, 0, -RobotMap.FRONT_BACK_LENGTH/2, "backModule"); rightModule = new SwerveModule(RobotMap.rightWheelEncoder, RobotMap.rightModuleEncoder, RobotMap.rightWheelMotor, RobotMap.rightModuleMotor, RobotMap.WHEEL_TOP_ABSOLUTE_SPEED, RobotMap.LEFT_RIGHT_WIDTH/2, 0, "rightModule"); //We were having occasional errors with the creation of the nav6 object, so we make 5 attempts before allowing the error to go through and being forced to redeploy. int count = 0; int maxTries = 5; while(true) { try { nav6 = new IMUAdvanced(new BufferingSerialPort(57600)); if(nav6 != null) break; } catch (VisaException e) { if (++count == maxTries) { e.printStackTrace(); break; } Timer.delay(.01); } } LiveWindow.addSensor("Drivetrain", "Gyro", nav6); }
/** * Constructs the IMU class, overriding the default update rate * with a custom rate which may be from 4 to 100, representing * the number of updates per second sent by the nav6 IMU. * * Note that increasing the update rate may increase the * CPU utilization. * @param serial_port BufferingSerialPort object to use * @param update_rate_hz Custom Update Rate (Hz) */ public IMU(BufferingSerialPort serial_port, byte update_rate_hz) { ypr_update_data = new IMUProtocol.YPRUpdate(); this.update_rate_hz = update_rate_hz; flags = 0; accel_fsr_g = DEFAULT_ACCEL_FSR_G; gyro_fsr_dps = DEFAULT_GYRO_FSR_DPS; this.serial_port = serial_port; yaw_history = new float[YAW_HISTORY_LENGTH]; yaw = (float) 0.0; pitch = (float) 0.0; roll = (float) 0.0; try { serial_port.reset(); } catch (VisaException ex) { ex.printStackTrace(); } initIMU(); m_thread = new Thread(this); m_thread.start(); }
public void set(double speed) { //Speed should be between -1.0 and 1.0 double spd = Math.abs(speed); if (spd > motorInput_maxSpeed) throw new IllegalArgumentException("Speed must be between -1.0 and 1.0"); _currentSpeed = speed; // Set command byte // byte command; command = SabertoothCommand.MOTOR_ONE_FORWARD.value; if (_motorNumber == Motor.ONE) { if (speed >= motorInput_offSpeed) { command = SabertoothCommand.MOTOR_ONE_FORWARD.value; } else { command = SabertoothCommand.MOTOR_ONE_BACKWARD.value; } } else { if (speed >= motorInput_offSpeed) { command = SabertoothCommand.MOTOR_TWO_FORWARD.value; } else { command = SabertoothCommand.MOTOR_TWO_BACKWARD.value; } } // Set data byte // byte data = (byte) Math.floor(spd * sabertooth_maxSpeed); // Write data // try { writeSerialPacket(command, data); } catch (VisaException e) { throw new RuntimeException("Error writing serial data: " + e.getMessage()); } }
/** * Read a string out of the buffer. Reads the entire contents of the buffer * * @param count the number of characters to read into the string * @return The read string */ public String readString(int count) throws VisaException { byte[] out = Visa.viBufRead(m_portHandle, count); try { return new String(out, 0, count, "US-ASCII"); } catch (UnsupportedEncodingException ex) { ex.printStackTrace(); return new String(); } }
/** * Read a string out of the buffer. Reads the entire contents of the buffer * * @param count the number of characters to read into the string * @return The read string */ public String readString(int count) throws VisaException { byte[] out = Visa.viBufRead(m_portHandle, count); StringBuffer s = new StringBuffer(count + 1); for (int i = 0; i < count; i++) { s.append(out[i]); } return s.toString(); }
private void writeSerialPacket(byte command, byte data) throws VisaException { byte addr = (byte) _address.value; byte checksum = (byte) ((addr + command + data) & 0x7F); //Generate checksum from address, command, and data bytes byte[] packet = { addr, command, data, checksum }; _PORT.write(packet, packet.length); }
/** * Disable termination behavior. */ public void disableTermination() throws VisaException { Visa.viSetAttribute(m_portHandle, Visa.VI_ATTR_TERMCHAR_EN, false); Visa.viSetAttribute(m_portHandle, Visa.VI_ATTR_ASRL_END_IN, Visa.VI_ASRL_END_NONE); }
/** * Create an instance of a Serial Port class. * * @param baudRate The baud rate to configure the serial port. The cRIO-9074 supports up to 230400 Baud. * @param dataBits The number of data bits per transfer. Valid values are between 5 and 8 bits. * @param parity Select the type of parity checking to use. * @param stopBits The number of stop bits to use as defined by the enum StopBits. */ public SerialPort(final int baudRate, final int dataBits, Parity parity, StopBits stopBits) throws VisaException { m_resourceManagerHandle = 0; m_portHandle = 0; m_resourceManagerHandle = Visa.viOpenDefaultRM(); m_portHandle = Visa.viOpen(m_resourceManagerHandle, "ASRL1::INSTR", 0, 0); Visa.viSetAttribute(m_portHandle, Visa.VI_ATTR_ASRL_BAUD, baudRate); Visa.viSetAttribute(m_portHandle, Visa.VI_ATTR_ASRL_DATA_BITS, dataBits); Visa.viSetAttribute(m_portHandle, Visa.VI_ATTR_ASRL_PARITY, parity.value); Visa.viSetAttribute(m_portHandle, Visa.VI_ATTR_ASRL_STOP_BITS, stopBits.value); // Set the default read buffer size to 1 to return bytes immediately setReadBufferSize(1); // Set the default timeout to 5 seconds. setTimeout(5.0f); // Don't wait until the buffer is full to transmit. setWriteBufferMode(WriteBufferMode.kFlushOnAccess); disableTermination(); //viInstallHandler(m_portHandle, VI_EVENT_IO_COMPLETION, ioCompleteHandler, this); //viEnableEvent(m_portHandle, VI_EVENT_IO_COMPLETION, VI_HNDLR, VI_NULL); UsageReporting.report(UsageReporting.kResourceType_SerialPort,0); }
/** * Create an instance of a Serial Port class. * * @param baudRate The baud rate to configure the serial port. The cRIO-9074 supports up to 230400 Baud. * @param dataBits The number of data bits per transfer. Valid values are between 5 and 8 bits. * @param parity Select the type of parity checking to use. * @param stopBits The number of stop bits to use as defined by the enum StopBits. */ public BufferingSerialPort(final int baudRate, final int dataBits, Parity parity, StopBits stopBits) throws VisaException { m_resourceManagerHandle = 0; m_portHandle = 0; m_resourceManagerHandle = Visa.viOpenDefaultRM(); m_portHandle = Visa.viOpen(m_resourceManagerHandle, "ASRL1::INSTR", 0, 0); setFlowControl(BufferingSerialPort.FlowControl.kNone); Visa.viSetAttribute(m_portHandle, Visa.VI_ATTR_ASRL_BAUD, baudRate); Visa.viSetAttribute(m_portHandle, Visa.VI_ATTR_ASRL_DATA_BITS, dataBits); Visa.viSetAttribute(m_portHandle, Visa.VI_ATTR_ASRL_PARITY, parity.value); Visa.viSetAttribute(m_portHandle, Visa.VI_ATTR_ASRL_STOP_BITS, stopBits.value); // Set the default read buffer size to 1 to return bytes immediately setReadBufferSize(1); // Set the default timeout to 5 seconds. setTimeout(5.0f); // Don't wait until the buffer is full to transmit. setWriteBufferMode(WriteBufferMode.kFlushOnAccess); disableTermination(); //viInstallHandler(m_portHandle, VI_EVENT_IO_COMPLETION, ioCompleteHandler, this); //viEnableEvent(m_portHandle, VI_EVENT_IO_COMPLETION, VI_HNDLR, VI_NULL); UsageReporting.report(UsageReporting.kResourceType_SerialPort,0); }
/** * Create an instance of a Serial Port class. * * @param baudRate The baud rate to configure the serial port. The cRIO-9074 supports up to 230400 Baud. * @param dataBits The number of data bits per transfer. Valid values are between 5 and 8 bits. * @param parity Select the type of parity checking to use. * @param stopBits The number of stop bits to use as defined by the enum StopBits. */ public SerialPort(final int baudRate, final int dataBits, Parity parity, StopBits stopBits) throws VisaException { m_resourceManagerHandle = 0; m_portHandle = 0; m_resourceManagerHandle = Visa.viOpenDefaultRM(); m_portHandle = Visa.viOpen(m_resourceManagerHandle, "ASRL1::INSTR", 0, 0); Visa.viSetAttribute(m_portHandle, Visa.VI_ATTR_ASRL_BAUD, baudRate); Visa.viSetAttribute(m_portHandle, Visa.VI_ATTR_ASRL_DATA_BITS, dataBits); Visa.viSetAttribute(m_portHandle, Visa.VI_ATTR_ASRL_PARITY, parity.value); Visa.viSetAttribute(m_portHandle, Visa.VI_ATTR_ASRL_STOP_BITS, stopBits.value); // Set the default timeout to 5 seconds. setTimeout(5.0f); // Don't wait until the buffer is full to transmit. setWriteBufferMode(WriteBufferMode.kFlushOnAccess); disableTermination(); //viInstallHandler(m_portHandle, VI_EVENT_IO_COMPLETION, ioCompleteHandler, this); //viEnableEvent(m_portHandle, VI_EVENT_IO_COMPLETION, VI_HNDLR, VI_NULL); UsageReporting.report(UsageReporting.kResourceType_SerialPort,0); }
/** * Create an instance of a Serial Port class. Defaults to one stop bit. * * @param baudRate The baud rate to configure the serial port. The cRIO-9074 supports up to 230400 Baud. * @param dataBits The number of data bits per transfer. Valid values are between 5 and 8 bits. * @param parity Select the type of parity checking to use. */ public SerialPort(final int baudRate, final int dataBits, Parity parity) throws VisaException { this(baudRate, dataBits, parity, StopBits.kOne); }
/** * Create an instance of a Serial Port class. Defaults to no parity and one * stop bit. * * @param baudRate The baud rate to configure the serial port. The cRIO-9074 supports up to 230400 Baud. * @param dataBits The number of data bits per transfer. Valid values are between 5 and 8 bits. */ public SerialPort(final int baudRate, final int dataBits) throws VisaException { this(baudRate, dataBits, Parity.kNone, StopBits.kOne); }
/** * Create an instance of a Serial Port class. Defaults to 8 databits, * no parity, and one stop bit. * * @param baudRate The baud rate to configure the serial port. The cRIO-9074 supports up to 230400 Baud. */ public SerialPort(final int baudRate) throws VisaException { this(baudRate, 8, Parity.kNone, StopBits.kOne); }
/** * Set the type of flow control to enable on this port. * * By default, flow control is disabled. * @param flowControl */ public void setFlowControl(FlowControl flowControl) throws VisaException { Visa.viSetAttribute(m_portHandle, Visa.VI_ATTR_ASRL_FLOW_CNTRL, flowControl.value); }
/** * Enable termination and specify the termination character. * * Termination is currently only implemented for receive. * When the the terminator is received, the read() or readString() will return * fewer bytes than requested, stopping after the terminator. * * @param terminator The character to use for termination. */ public void enableTermination(char terminator) throws VisaException { Visa.viSetAttribute(m_portHandle, Visa.VI_ATTR_TERMCHAR_EN, true); Visa.viSetAttribute(m_portHandle, Visa.VI_ATTR_TERMCHAR, terminator); Visa.viSetAttribute(m_portHandle, Visa.VI_ATTR_ASRL_END_IN, Visa.VI_ASRL_END_TERMCHAR); }
/** * Enable termination and specify the termination character. * * Termination is currently only implemented for receive. * When the the terminator is received, the read() or readString() will return * fewer bytes than requested, stopping after the terminator. * * The default terminator is '\n' */ public void enableTermination() throws VisaException { this.enableTermination('\n'); }
/** * Get the number of bytes currently available to read from the serial port. * * @return The number of bytes available to read. */ public int getBytesReceived() throws VisaException { return Visa.viGetAttribute(m_portHandle, Visa.VI_ATTR_ASRL_AVAIL_NUM); }
/** * Output formatted text to the serial port. * * @deprecated use write(string.getBytes()) instead * @bug All pointer-based parameters seem to return an error. * * @param write A string to write */ public void print(String write) throws VisaException { Visa.viVPrintf(m_portHandle, write); }
/** * Read a string out of the buffer. Reads the entire contents of the buffer * * @return The read string */ public String readString() throws VisaException { return readString(getBytesReceived()); }
/** * Read raw bytes out of the buffer. * * @param count The maximum number of bytes to read. * @return An array of the read bytes */ public byte[] read(final int count) throws VisaException { return Visa.viBufRead(m_portHandle, count); }
/** * Write raw bytes to the buffer. * * @param buffer the buffer to read the bytes from. * @param count The maximum number of bytes to write. * @return The number of bytes actually written into the port. */ public int write(byte[] buffer, int count) throws VisaException { return Visa.viBufWrite(m_portHandle, buffer, count); }
/** * Configure the timeout of the serial port. * * This defines the timeout for transactions with the hardware. * It will affect reads if less bytes are available than the * read buffer size (defaults to 1) and very large writes. * * @param timeout The number of seconds to to wait for I/O. */ public void setTimeout(double timeout) throws VisaException { Visa.viSetAttribute(m_portHandle, Visa.VI_ATTR_TMO_VALUE, (int) (timeout * 1e3)); }
/** * Specify the size of the input buffer. * * Specify the amount of data that can be stored before data * from the device is returned to Read. If you want * data that is received to be returned immediately, set this to 1. * * It the buffer is not filled before the read timeout expires, all * data that has been received so far will be returned. * * @param size The read buffer size. */ void setReadBufferSize(int size) throws VisaException { Visa.viSetBuf(m_portHandle, Visa.VI_READ_BUF, size); }
/** * Specify the size of the output buffer. * * Specify the amount of data that can be stored before being * transmitted to the device. * * @param size The write buffer size. */ void setWriteBufferSize(int size) throws VisaException { Visa.viSetBuf(m_portHandle, Visa.VI_WRITE_BUF, size); }
/** * Specify the flushing behavior of the output buffer. * * When set to kFlushOnAccess, data is synchronously written to the serial port * after each call to either print() or write(). * * When set to kFlushWhenFull, data will only be written to the serial port when * the buffer is full or when flush() is called. * * @param mode The write buffer mode. */ public void setWriteBufferMode(WriteBufferMode mode) throws VisaException { Visa.viSetAttribute(m_portHandle, Visa.VI_ATTR_WR_BUF_OPER_MODE, mode.value); }
/** * Force the output buffer to be written to the port. * * This is used when setWriteBufferMode() is set to kFlushWhenFull to force a * flush before the buffer is full. */ public void flush() throws VisaException { Visa.viFlush(m_portHandle, Visa.VI_WRITE_BUF); }
/** * Reset the serial port driver to a known state. * * Empty the transmit and receive buffers in the device and formatted I/O. */ public void reset() throws VisaException { Visa.viClear(m_portHandle); }
/** * Create an instance of a Serial Port class. Defaults to one stop bit. * * @param baudRate The baud rate to configure the serial port. The cRIO-9074 supports up to 230400 Baud. * @param dataBits The number of data bits per transfer. Valid values are between 5 and 8 bits. * @param parity Select the type of parity checking to use. */ public BufferingSerialPort(final int baudRate, final int dataBits, Parity parity) throws VisaException { this(baudRate, dataBits, parity, StopBits.kOne); }
/** * Create an instance of a Serial Port class. Defaults to no parity and one * stop bit. * * @param baudRate The baud rate to configure the serial port. The cRIO-9074 supports up to 230400 Baud. * @param dataBits The number of data bits per transfer. Valid values are between 5 and 8 bits. */ public BufferingSerialPort(final int baudRate, final int dataBits) throws VisaException { this(baudRate, dataBits, Parity.kNone, StopBits.kOne); }
/** * Create an instance of a Serial Port class. Defaults to 8 databits, * no parity, and one stop bit. * * @param baudRate The baud rate to configure the serial port. The cRIO-9074 supports up to 230400 Baud. */ public BufferingSerialPort(final int baudRate) throws VisaException { this(baudRate, 8, Parity.kNone, StopBits.kOne); }
