├── .gitignore ├── README.md ├── buffpy ├── buff.bash ├── data │ ├── build │ │ ├── crosshair.yaml │ │ ├── cv-benchmarking.yaml │ │ ├── deploy.yaml │ │ ├── edge2-npu.yaml │ │ ├── efficientdet-d0-trainer.yaml │ │ ├── fw-release.yaml │ │ ├── rust-debug.yaml │ │ ├── rust-docs.yaml │ │ ├── rust-release.yaml │ │ └── sys_id.yaml │ ├── install │ │ ├── dependencies.txt │ │ ├── docker_requirements.txt │ │ ├── python3_requirements.txt │ │ └── ros_dependencies.txt │ └── robots │ │ ├── infantry-sim │ │ ├── buffbot.xacro │ │ ├── buffbot_control.yaml │ │ └── nodes.yaml │ │ ├── infantry │ │ ├── buffbot.xacro │ │ └── nodes.yaml │ │ ├── penguin │ │ ├── buffbot.xacro │ │ └── nodes.yaml │ │ ├── robots.yaml │ │ ├── self.txt │ │ └── sys_id │ │ ├── buffbot.xacro │ │ └── nodes.yaml ├── package.xml └── scripts │ ├── buff_entrypoint.sh │ ├── buffbot.service │ ├── buffpy │ ├── effdet_trainer.bash │ ├── install.bash │ ├── install_buffpy.bash │ ├── install_docker.bash │ ├── install_jetson_inference.bash │ ├── install_macos.bash │ ├── install_ros.bash │ ├── install_ros2.bash │ ├── install_ros_source.bash │ ├── install_sublime.bash │ ├── install_tytools.bash │ ├── onnx2blob.py │ ├── openvino_setup.bash │ ├── pt2blob.bash │ ├── purge.bash │ ├── robot_setup.bash │ ├── run │ ├── wsl_gpu_drivers.bash │ └── yolov5_setup.bash ├── containers ├── base │ ├── .dockerignore │ └── Dockerfile ├── dev │ └── Dockerfile └── docker-compose.yaml ├── src ├── buff_rust │ ├── .cargo │ │ └── config.toml │ ├── Cargo.toml │ ├── src │ │ ├── lib.rs │ │ ├── localization │ │ │ ├── data_structures.rs │ │ │ ├── estimator_node.rs │ │ │ ├── estimators.rs │ │ │ ├── mod.rs │ │ │ ├── quadtree.rs │ │ │ ├── swerve.rs │ │ │ └── tests.rs │ │ ├── locomotion │ │ │ ├── controllers.rs │ │ │ ├── locomotion.rs │ │ │ ├── locomotion_node.rs │ │ │ ├── mod.rs │ │ │ └── test.rs │ │ ├── teensy_comms │ │ │ ├── buff_hid.rs │ │ │ ├── comms_node.rs │ │ │ ├── data_structures.rs │ │ │ ├── mod.rs │ │ │ └── test.rs │ │ ├── utilities │ │ │ ├── buffers.rs │ │ │ ├── loaders.rs │ │ │ ├── mod.rs │ │ │ ├── test.rs │ │ │ └── viz.rs │ │ └── vision │ │ │ ├── image_fps.rs │ │ │ └── mod.rs │ └── tests │ │ ├── live_estimator_test.rs │ │ └── live_motor_test.rs ├── crosshair │ ├── CMakeLists.txt │ ├── package.xml │ ├── scripts │ │ ├── buffnet.py │ │ ├── image_generator.py │ │ ├── interactive_thresholder.py │ │ ├── luxnet.py │ │ ├── mds_detector.py │ │ ├── ml_detector.py │ │ ├── tracker_demo_detector.py │ │ ├── trainer.py │ │ └── xml2txt.py │ └── src │ │ └── depthai-depth.cpp ├── cv_benchmarking │ ├── luxonis_depth_colormap.py │ ├── luxonis_rgb_mobilenet.py │ ├── luxonis_spatial_object_tracker.py │ ├── mobilenet-tf.py │ └── mobilenet-torch.py ├── firmware │ ├── Makefile │ ├── examples │ │ ├── blink │ │ │ └── main.cpp │ │ ├── can │ │ │ ├── basic_can_read.cpp │ │ │ └── test_rm_ux.cpp │ │ ├── comms │ │ │ └── main.cpp │ │ ├── controllers │ │ │ ├── main.cpp │ │ │ └── test_controllers.cpp │ │ ├── dr16 │ │ │ └── main.cpp │ │ ├── hid_debug │ │ │ ├── input_dump.cpp │ │ │ └── output_dump.cpp │ │ ├── icm20649 │ │ │ └── main.cpp │ │ ├── iw_sandbox │ │ │ └── main.cpp │ │ ├── lsm6dsox │ │ │ └── main.cpp │ │ └── scheduling │ │ │ └── pipeline_timers.cpp │ ├── libraries │ │ ├── .DS_Store │ │ ├── Adafruit_BusIO │ │ │ ├── .DS_Store │ │ │ ├── Adafruit_BusIO_Register.cpp │ │ │ ├── Adafruit_BusIO_Register.h │ │ │ ├── Adafruit_I2CDevice.cpp │ │ │ ├── Adafruit_I2CDevice.h │ │ │ ├── Adafruit_I2CRegister.h │ │ │ ├── Adafruit_SPIDevice.cpp │ │ │ ├── Adafruit_SPIDevice.h │ │ │ └── examples │ │ │ │ ├── i2c_address_detect │ │ │ │ └── i2c_address_detect.ino │ │ │ │ ├── i2c_readwrite │ │ │ │ └── i2c_readwrite.ino │ │ │ │ ├── i2c_registers │ │ │ │ └── i2c_registers.ino │ │ │ │ ├── i2corspi_register │ │ │ │ └── i2corspi_register.ino │ │ │ │ ├── spi_modetest │ │ │ │ └── spi_modetest.ino │ │ │ │ ├── spi_readwrite │ │ │ │ └── spi_readwrite.ino │ │ │ │ ├── spi_register_bits │ │ │ │ └── spi_register_bits.ino │ │ │ │ └── spi_registers │ │ │ │ └── spi_registers.ino │ │ ├── Adafruit_ICM20X │ │ │ ├── .DS_Store │ │ │ ├── Adafruit_ICM20649.cpp │ │ │ ├── Adafruit_ICM20649.h │ │ │ ├── Adafruit_ICM20948.cpp │ │ │ ├── Adafruit_ICM20948.h │ │ │ ├── Adafruit_ICM20X.cpp │ │ │ ├── Adafruit_ICM20X.h │ │ │ ├── README.md │ │ │ ├── assets │ │ │ │ ├── 649.jpg │ │ │ │ ├── 649.png │ │ │ │ └── 948.png │ │ │ ├── code-of-conduct.md │ │ │ ├── examples │ │ │ │ ├── adafruit_icm20649_test │ │ │ │ │ └── adafruit_icm20649_test.ino │ │ │ │ ├── adafruit_icm20948_test │ │ │ │ │ └── adafruit_icm20948_test.ino │ │ │ │ ├── adafruit_icm20x_accel_filter │ │ │ │ │ └── adafruit_icm20x_accel_filter.ino │ │ │ │ ├── icm20649_unifiedsensors │ │ │ │ │ └── icm20649_unifiedsensors.ino │ │ │ │ ├── icm20948_mag_rate_test │ │ │ │ │ └── icm20948_mag_rate_test.ino │ │ │ │ └── icm20948_unifiedsensors │ │ │ │ │ └── icm20948_unifiedsensors.ino │ │ │ └── license.txt │ │ ├── Adafruit_LIS3MDL │ │ │ ├── Adafruit_LIS3MDL.cpp │ │ │ ├── Adafruit_LIS3MDL.h │ │ │ └── examples │ │ │ │ └── lis3mdl_demo.cpp │ │ ├── Adafruit_LSM6DS │ │ │ ├── .DS_Store │ │ │ ├── Adafruit_ISM330DHCX.cpp │ │ │ ├── Adafruit_ISM330DHCX.h │ │ │ ├── Adafruit_LSM6DS.cpp │ │ │ ├── Adafruit_LSM6DS.h │ │ │ ├── Adafruit_LSM6DS3.cpp │ │ │ ├── Adafruit_LSM6DS3.h │ │ │ ├── Adafruit_LSM6DS33.cpp │ │ │ ├── Adafruit_LSM6DS33.h │ │ │ ├── Adafruit_LSM6DS3TRC.cpp │ │ │ ├── Adafruit_LSM6DS3TRC.h │ │ │ ├── Adafruit_LSM6DSL.cpp │ │ │ ├── Adafruit_LSM6DSL.h │ │ │ ├── Adafruit_LSM6DSO32.cpp │ │ │ ├── Adafruit_LSM6DSO32.h │ │ │ ├── Adafruit_LSM6DSOX.cpp │ │ │ ├── Adafruit_LSM6DSOX.h │ │ │ ├── assets │ │ │ │ ├── board.jpg │ │ │ │ └── board.png │ │ │ ├── examples │ │ │ │ ├── adafruit_ism330dhcx_test │ │ │ │ │ └── adafruit_ism330dhcx_test.ino │ │ │ │ ├── adafruit_lsm6ds33_test │ │ │ │ │ └── adafruit_lsm6ds33_test.ino │ │ │ │ ├── adafruit_lsm6ds3trc_test │ │ │ │ │ └── adafruit_lsm6ds3trc_test.ino │ │ │ │ ├── adafruit_lsm6ds_pedometer │ │ │ │ │ └── adafruit_lsm6ds_pedometer.ino │ │ │ │ ├── adafruit_lsm6ds_shake │ │ │ │ │ └── adafruit_lsm6ds_shake.ino │ │ │ │ ├── adafruit_lsm6ds_unifiedsensors │ │ │ │ │ └── adafruit_lsm6ds_unifiedsensors.ino │ │ │ │ ├── adafruit_lsm6dso32_test │ │ │ │ │ └── adafruit_lsm6dso32_test.ino │ │ │ │ └── adafruit_lsm6dsox_test │ │ │ │ │ └── adafruit_lsm6dsox_test.ino │ │ │ └── license.txt │ │ ├── Adafruit_Sensor │ │ │ ├── .DS_Store │ │ │ ├── ._.DS_Store │ │ │ ├── Adafruit_Sensor.cpp │ │ │ ├── Adafruit_Sensor.h │ │ │ ├── LICENSE.txt │ │ │ └── examples │ │ │ │ └── sensortest │ │ │ │ └── sensortest.ino │ │ ├── FlexCAN_T4 │ │ │ ├── .DS_Store │ │ │ ├── ._.DS_Store │ │ │ ├── FlexCAN_T4.h │ │ │ ├── FlexCAN_T4.tpp │ │ │ ├── FlexCAN_T4Beta1BoardArchive.zip │ │ │ ├── FlexCAN_T4FD.tpp │ │ │ ├── FlexCAN_T4FDTimings.tpp │ │ │ ├── LICENSE │ │ │ ├── README.md │ │ │ ├── circular_buffer.h │ │ │ ├── examples │ │ │ │ ├── BiDirectionalForward │ │ │ │ │ └── BiDirectionalForward.ino │ │ │ │ ├── CAN2.0_example_FIFO_with_interrupts.ino │ │ │ │ ├── CAN2.0_example_FIFO_with_interrupts │ │ │ │ │ └── CAN2.0_example_FIFO_with_interrupts.ino │ │ │ │ ├── beta_sample │ │ │ │ │ └── beta_sample.ino │ │ │ │ ├── isotp_example_send_receive │ │ │ │ │ └── isotp_example_send_receive.ino │ │ │ │ ├── isotp_server_example │ │ │ │ │ └── isotp_server_example.ino │ │ │ │ ├── isotp_server_multiple_buffers_to_individual_busses_example │ │ │ │ │ └── isotp_server_multiple_buffers_to_individual_busses_example.ino │ │ │ │ ├── mailbox_filtering_example_with_interrupts │ │ │ │ │ └── mailbox_filtering_example_with_interrupts.ino │ │ │ │ └── read_two_channels │ │ │ │ │ └── read_two_channels.ino │ │ │ ├── imxrt_flexcan.h │ │ │ ├── isotp.h │ │ │ ├── isotp.tpp │ │ │ ├── isotp_server.h │ │ │ ├── isotp_server.tpp │ │ │ └── kinetis_flexcan.h │ │ ├── FreqMeasureMulti │ │ │ ├── FreqMeasureMulti.cpp │ │ │ ├── FreqMeasureMulti.h │ │ │ ├── FreqMeasureMultiIMXRT.cpp │ │ │ ├── FreqMeasureMultiIMXRT.h │ │ │ ├── README.md │ │ │ ├── docs │ │ │ │ └── issue_template.md │ │ │ ├── examples │ │ │ │ ├── Serial_Output │ │ │ │ │ └── Serial_Output.ino │ │ │ │ ├── Single_PWM_in_Serial_Output │ │ │ │ │ └── Single_PWM_in_Serial_Output.ino │ │ │ │ └── Three_PWM_in_Serial_Output │ │ │ │ │ └── Three_PWM_in_Serial_Output.ino │ │ │ └── keywords.txt │ │ ├── SPI │ │ │ ├── .DS_Store │ │ │ ├── ._.DS_Store │ │ │ ├── SPI.cpp │ │ │ ├── SPI.h │ │ │ └── examples │ │ │ │ ├── BarometricPressureSensor │ │ │ │ └── BarometricPressureSensor.ino │ │ │ │ └── DigitalPotControl │ │ │ │ └── DigitalPotControl.ino │ │ ├── Wire │ │ │ ├── .DS_Store │ │ │ ├── ._.DS_Store │ │ │ ├── Wire.cpp │ │ │ ├── Wire.h │ │ │ ├── WireIMXRT.cpp │ │ │ ├── WireIMXRT.h │ │ │ ├── WireKinetis.cpp │ │ │ ├── WireKinetis.h │ │ │ ├── examples │ │ │ │ ├── SFRRanger_reader │ │ │ │ │ └── SFRRanger_reader.ino │ │ │ │ ├── Scanner │ │ │ │ │ └── Scanner.ino │ │ │ │ ├── digital_potentiometer │ │ │ │ │ └── digital_potentiometer.ino │ │ │ │ ├── master_reader │ │ │ │ │ └── master_reader.ino │ │ │ │ ├── master_writer │ │ │ │ │ └── master_writer.ino │ │ │ │ ├── slave_receiver │ │ │ │ │ └── slave_receiver.ino │ │ │ │ └── slave_sender │ │ │ │ │ └── slave_sender.ino │ │ │ ├── keywords.txt │ │ │ ├── library.properties │ │ │ └── utility │ │ │ │ ├── twi.c │ │ │ │ └── twi.h │ │ └── unity │ │ │ ├── unity.c │ │ │ ├── unity.h │ │ │ ├── unity_config.h │ │ │ └── unity_internals.h │ ├── src │ │ ├── .DS_Store │ │ ├── algorithms │ │ │ ├── lowpass_filter.h │ │ │ ├── pid_filter.h │ │ │ └── rolling_average_filter.h │ │ ├── buff_cpp │ │ │ ├── blink.cpp │ │ │ ├── blink.h │ │ │ ├── controllers.cpp │ │ │ ├── controllers.h │ │ │ ├── device_manager.cpp │ │ │ ├── device_manager.h │ │ │ ├── global_robot_state.cpp │ │ │ ├── global_robot_state.h │ │ │ ├── loggers.cpp │ │ │ ├── loggers.h │ │ │ ├── teensy_io.cpp │ │ │ ├── teensy_io.h │ │ │ ├── timing.cpp │ │ │ └── timing.h │ │ ├── main.cpp │ │ ├── motor_drivers │ │ │ ├── rm_can_interface.cpp │ │ │ └── rm_can_interface.h │ │ ├── robot_comms │ │ │ ├── hid_report.cpp │ │ │ └── hid_report.h │ │ └── sensors │ │ │ ├── dr16.cpp │ │ │ ├── dr16.h │ │ │ ├── icm20649.cpp │ │ │ ├── icm20649.h │ │ │ ├── lsm6dsox.cpp │ │ │ ├── lsm6dsox.h │ │ │ ├── refSystem.cpp │ │ │ ├── refSystem.h │ │ │ ├── revEnc.cpp │ │ │ └── revEnc.h │ └── teensy4 │ │ ├── Arduino.h │ │ ├── AudioStream.cpp │ │ ├── AudioStream.h │ │ ├── Blink.cc │ │ ├── Client.h │ │ ├── CrashReport.cpp │ │ ├── CrashReport.h │ │ ├── DMAChannel.cpp │ │ ├── DMAChannel.h │ │ ├── EventResponder.cpp │ │ ├── EventResponder.h │ │ ├── FS.h │ │ ├── HardwareSerial.cpp │ │ ├── HardwareSerial.h │ │ ├── HardwareSerial1.cpp │ │ ├── HardwareSerial2.cpp │ │ ├── HardwareSerial3.cpp │ │ ├── HardwareSerial4.cpp │ │ ├── HardwareSerial5.cpp │ │ ├── HardwareSerial6.cpp │ │ ├── HardwareSerial7.cpp │ │ ├── HardwareSerial8.cpp │ │ ├── IPAddress.cpp │ │ ├── IPAddress.h │ │ ├── IntervalTimer.cpp │ │ ├── IntervalTimer.h │ │ ├── Keyboard.h │ │ ├── MIDIUSB.h │ │ ├── Mouse.h │ │ ├── Print.cpp │ │ ├── Print.h │ │ ├── Printable.h │ │ ├── Server.h │ │ ├── Stream.cpp │ │ ├── Stream.h │ │ ├── Time.cpp │ │ ├── Tone.cpp │ │ ├── Udp.h │ │ ├── WCharacter.h │ │ ├── WMath.cpp │ │ ├── WProgram.h │ │ ├── WString.cpp │ │ ├── WString.h │ │ ├── analog.c │ │ ├── arm_common_tables.h │ │ ├── arm_const_structs.h │ │ ├── arm_math.h │ │ ├── avr │ │ ├── eeprom.h │ │ ├── interrupt.h │ │ ├── io.h │ │ ├── pgmspace.h │ │ ├── power.h │ │ ├── sleep.h │ │ └── wdt.h │ │ ├── avr_emulation.h │ │ ├── avr_functions.h │ │ ├── binary.h │ │ ├── bootdata.c │ │ ├── clockspeed.c │ │ ├── cmsis_gcc.h │ │ ├── core_cm7.h │ │ ├── core_cmInstr.h │ │ ├── core_id.h │ │ ├── core_pins.h │ │ ├── debug │ │ └── printf.h │ │ ├── debugprintf.c │ │ ├── delay.c │ │ ├── digital.c │ │ ├── eeprom.c │ │ ├── elapsedMillis.h │ │ ├── extmem.c │ │ ├── fuse.c │ │ ├── imxrt.h │ │ ├── imxrt1062.ld │ │ ├── imxrt1062_mm.ld │ │ ├── imxrt1062_t41.ld │ │ ├── interrupt.c │ │ ├── keylayouts.c │ │ ├── keylayouts.h │ │ ├── libc.c │ │ ├── math_helper.h │ │ ├── memcpy-armv7m.S │ │ ├── memset.S │ │ ├── new.cpp │ │ ├── nonstd.c │ │ ├── pgmspace.h │ │ ├── pins_arduino.h │ │ ├── pwm.c │ │ ├── rtc.c │ │ ├── serialEvent.cpp │ │ ├── serialEvent1.cpp │ │ ├── serialEvent2.cpp │ │ ├── serialEvent3.cpp │ │ ├── serialEvent4.cpp │ │ ├── serialEvent5.cpp │ │ ├── serialEvent6.cpp │ │ ├── serialEvent7.cpp │ │ ├── serialEvent8.cpp │ │ ├── serialEventUSB1.cpp │ │ ├── serialEventUSB2.cpp │ │ ├── sm_alloc_valid.c │ │ ├── sm_calloc.c │ │ ├── sm_free.c │ │ ├── sm_hash.c │ │ ├── sm_malloc.c │ │ ├── sm_malloc_stats.c │ │ ├── sm_pool.c │ │ ├── sm_realloc.c │ │ ├── sm_realloc_i.c │ │ ├── sm_realloc_move.c │ │ ├── sm_szalloc.c │ │ ├── sm_util.c │ │ ├── sm_zalloc.c │ │ ├── smalloc.h │ │ ├── smalloc_i.h │ │ ├── startup.c │ │ ├── tempmon.c │ │ ├── usb.c │ │ ├── usb_audio.cpp │ │ ├── usb_audio.h │ │ ├── usb_desc.c │ │ ├── usb_desc.h │ │ ├── usb_dev.h │ │ ├── usb_flightsim.cpp │ │ ├── usb_flightsim.h │ │ ├── usb_inst.cpp │ │ ├── usb_joystick.c │ │ ├── usb_joystick.h │ │ ├── usb_keyboard.c │ │ ├── usb_keyboard.h │ │ ├── usb_midi.c │ │ ├── usb_midi.h │ │ ├── usb_mouse.c │ │ ├── usb_mouse.h │ │ ├── usb_mtp.c │ │ ├── usb_mtp.h │ │ ├── usb_names.h │ │ ├── usb_rawhid.c │ │ ├── usb_rawhid.h │ │ ├── usb_seremu.c │ │ ├── usb_seremu.h │ │ ├── usb_serial.c │ │ ├── usb_serial.h │ │ ├── usb_serial2.c │ │ ├── usb_serial3.c │ │ ├── usb_touch.c │ │ ├── usb_touch.h │ │ ├── util │ │ ├── atomic.h │ │ ├── crc16.h │ │ ├── delay.h │ │ └── parity.h │ │ ├── wiring.h │ │ ├── wiring_private.h │ │ └── yield.cpp ├── hunter │ ├── CMakeLists.txt │ ├── package.xml │ └── scripts │ │ ├── deadreckon_tracer.py │ │ ├── mds_tracker.py │ │ ├── projector.py │ │ ├── target_map.py │ │ └── tracker.py ├── system_identification │ ├── camera_calibration.py │ ├── chassis_identification.py │ └── motor_identification.py └── wrecker │ └── src │ ├── hero.cppd │ └── test.cpp └── title-card.png /.gitignore: -------------------------------------------------------------------------------- 1 | **/.pio 2 | **/.idea 3 | **/.piopm 4 | **/.vscode 5 | **/.DS_store 6 | **/Cargo.lock 7 | **/__pycache__ 8 | **/.catkin_tools 9 | 10 | 11 | data/* 12 | buffpy/lib/* 13 | documentation 14 | buffpy/bin/tycmd 15 | buffpy/data/models/* 16 | src/firmware/build/* 17 | src/buff_rust/target/* 18 | src/efficientdet-d0-trainer/* -------------------------------------------------------------------------------- /buffpy/data/build/crosshair.yaml: -------------------------------------------------------------------------------- 1 | project: 'crosshair' 2 | install: ['scripts/luxnet.py'] -------------------------------------------------------------------------------- /buffpy/data/build/cv-benchmarking.yaml: -------------------------------------------------------------------------------- 1 | project: 'cv_benchmarking' 2 | install: [''] -------------------------------------------------------------------------------- /buffpy/data/build/deploy.yaml: -------------------------------------------------------------------------------- 1 | include: ['fw-release.yaml', 'rust-release.yaml'] 2 | 3 | 4 | -------------------------------------------------------------------------------- /buffpy/data/build/edge2-npu.yaml: -------------------------------------------------------------------------------- 1 | project: 'edge2-npu' 2 | install: ['Python/ssd', 'Python/inceptionv3'] -------------------------------------------------------------------------------- /buffpy/data/build/efficientdet-d0-trainer.yaml: -------------------------------------------------------------------------------- 1 | project: 'efficientdet-d0-trainer' 2 | build: 'chmod +x scripts/* && ./scripts/start_training.bash' 3 | -------------------------------------------------------------------------------- /buffpy/data/build/fw-release.yaml: -------------------------------------------------------------------------------- 1 | project: 'firmware' 2 | build: 'pio run -e release' 3 | bin: ['.pio/build/release/firmware.hex'] 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /buffpy/data/build/rust-debug.yaml: -------------------------------------------------------------------------------- 1 | project: 'buff_rust' 2 | build: 'cargo build && cargo fmt' 3 | install: ['target/debug/comms_node'] -------------------------------------------------------------------------------- /buffpy/data/build/rust-docs.yaml: -------------------------------------------------------------------------------- 1 | project: 'buff_rust' 2 | build: 'cargo fmt && cargo doc --no-deps --open' 3 | docs: ['target/doc'] -------------------------------------------------------------------------------- /buffpy/data/build/rust-release.yaml: -------------------------------------------------------------------------------- 1 | project: 'buff_rust' 2 | build: 'cargo build --release && cargo fmt' 3 | install: ['target/release/comms_node', 'target/release/estimator_node'] 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /buffpy/data/build/sys_id.yaml: -------------------------------------------------------------------------------- 1 | project: 'system_identification' 2 | install: ['motor_identification.py'] 3 | 4 | 5 | -------------------------------------------------------------------------------- /buffpy/data/install/dependencies.txt: -------------------------------------------------------------------------------- 1 | git 2 | vim 3 | htop 4 | tmux 5 | wget 6 | curl 7 | gnupg2 8 | libudev-dev 9 | lsb-release 10 | python3-dev 11 | iputils-ping 12 | openssh-client 13 | openssh-server 14 | build-essential 15 | ca-certificates 16 | -------------------------------------------------------------------------------- /buffpy/data/install/docker_requirements.txt: -------------------------------------------------------------------------------- 1 | qemu 2 | binfmt-support 3 | qemu-user-static -------------------------------------------------------------------------------- /buffpy/data/install/python3_requirements.txt: -------------------------------------------------------------------------------- 1 | numpy 2 | pyyaml 3 | pandas 4 | pillow 5 | cython 6 | seaborn 7 | requests 8 | protobuf 9 | setuptools 10 | platformio==6.0.1 11 | opencv-python 12 | -------------------------------------------------------------------------------- /buffpy/data/install/ros_dependencies.txt: -------------------------------------------------------------------------------- 1 | python3-wstool 2 | python3-rosdep2 3 | python3-rosinstall 4 | python3-rosinstall-generator 5 | -------------------------------------------------------------------------------- /buffpy/data/robots/infantry-sim/buffbot_control.yaml: -------------------------------------------------------------------------------- 1 | buffbot: 2 | 3 | # Position Controllers --------------------------------------- 4 | fr_steer_controller: 5 | type: effort_controllers/JointPositionController 6 | joint: fr_swerve 7 | pid: {p: 100.0, i: 0.01, d: 10.0} 8 | fr_drive_controller: 9 | type: effort_controllers/JointEffortController 10 | joint: fr_wheel 11 | pid: {p: 100.0, i: 0.01, d: 10.0} -------------------------------------------------------------------------------- /buffpy/data/robots/infantry-sim/nodes.yaml: -------------------------------------------------------------------------------- 1 | buff_rust: 2 | debug: True 3 | files: ['buff_rust'] 4 | 5 | gazebo: 6 | files: ['gazebo'] 7 | package: gazebo_ros 8 | args: ["--verbose"] 9 | 10 | spawn_urdf: 11 | files: ['spawn_model'] 12 | package: gazebo_ros 13 | args: ["-param robot_description -urdf -model buffbot -z 0.5"] 14 | 15 | spawn_ros_control: 16 | files: ['spawner'] 17 | package: controller_manager 18 | args: ["--namespace buffbot --timeout 10 19 | joint_state_controller 20 | fls_controller 21 | fld_controller 22 | frs_controller 23 | frd_controller 24 | rrs_controller 25 | rrd_controller 26 | rls_controller 27 | rld_controller 28 | "] 29 | 30 | # Controllers --------------------------------------- 31 | 32 | joint_state_controller: 33 | type: joint_state_controller/JointStateController 34 | publish_rate: 50 35 | 36 | fls_controller: 37 | type: effort_controllers/JointPositionController 38 | joint: fl_steer 39 | pid: {p: 100.0, i: 0.01, d: 10.0} 40 | fld_controller: 41 | type: effort_controllers/JointEffortController 42 | joint: fl_drive 43 | pid: {p: 100.0, i: 0.01, d: 10.0} 44 | 45 | frs_controller: 46 | type: effort_controllers/JointPositionController 47 | joint: fr_steer 48 | pid: {p: 100.0, i: 0.01, d: 10.0} 49 | frd_controller: 50 | type: effort_controllers/JointEffortController 51 | joint: fr_drive 52 | pid: {p: 100.0, i: 0.01, d: 10.0} 53 | 54 | rrs_controller: 55 | type: effort_controllers/JointPositionController 56 | joint: rr_steer 57 | pid: {p: 100.0, i: 0.01, d: 10.0} 58 | rrd_controller: 59 | type: effort_controllers/JointEffortController 60 | joint: rr_drive 61 | pid: {p: 100.0, i: 0.01, d: 10.0} 62 | 63 | rls_controller: 64 | type: effort_controllers/JointPositionController 65 | joint: rl_steer 66 | pid: {p: 100.0, i: 0.01, d: 10.0} 67 | rld_controller: 68 | type: effort_controllers/JointEffortController 69 | joint: rl_drive 70 | pid: {p: 100.0, i: 0.01, d: 10.0} 71 | 72 | robot: 'infantry' 73 | 74 | -------------------------------------------------------------------------------- /buffpy/data/robots/robots.yaml: -------------------------------------------------------------------------------- 1 | edge-k.local: 2 | 'penguin' 3 | edge0.local: 4 | 'penguin' 5 | edge1.local: 6 | 'sentry' 7 | edge2.local: 8 | 'hero' 9 | edge3.local: 10 | 'infantry' -------------------------------------------------------------------------------- /buffpy/data/robots/self.txt: -------------------------------------------------------------------------------- 1 | penguin -------------------------------------------------------------------------------- /buffpy/scripts/buff_entrypoint.sh: -------------------------------------------------------------------------------- 1 | #! /bin/bash 2 | 3 | #!/bin/bash 4 | set -e 5 | 6 | buffbash="/home/cu-robotics/buff-code/buffpy/buff.bash" 7 | echo "sourcing $buffbash" 8 | source "$buffbash" 9 | 10 | echo "Host $HOSTNAME" 11 | echo "Project Root $PROJECT_ROOT" 12 | 13 | exec "$@" -------------------------------------------------------------------------------- /buffpy/scripts/buffbot.service: -------------------------------------------------------------------------------- 1 | [Unit] 2 | Description=CU Robotics Startup Service 3 | ConditionPathExists=/home/cu-robotics/buff-code/buffpy 4 | 5 | [Service] 6 | Type=forking 7 | Restart=always 8 | User=cu-robotics 9 | TimeoutStartSec=900 10 | WorkingDirectory=/home/cu-robotics/buff-code 11 | ExecStart=/bin/bash -c 'source /home/cu-robotics/buff-code/buffpy/buff.bash && run self.yaml' 12 | 13 | [Install] 14 | WantedBy=multi-user.target -------------------------------------------------------------------------------- /buffpy/scripts/effdet_trainer.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | EFFDET_SRC="${PROJECT_ROOT}/src/efficientdet-d0-trainer" 4 | RESEARCH_DIR="${EFFDET_SRC}/models/research" 5 | PROTO_DIR="${EFFDET_SRC}/software/protobuf-21.12" 6 | 7 | echo ${EFFDET_SRC} 8 | # Rebuild everytime this is run 9 | if [[ -d "${EFFDET_SRC}" ]]; then 10 | rm -rf ${EFFDET_SRC} 11 | fi 12 | 13 | # Clones repo into buff-code/src/ 14 | cd "${PROJECT_ROOT}/src" && \ 15 | git clone https://github.com/ethan-wst/efficientdet-d0-trainer.git 16 | 17 | # add protodir to the path 18 | export PATH="${PATH}:${PROTO_DIR}/bin" 19 | 20 | # Installs tensorflow & numpy 21 | pip install --ignore-installed --upgrade tensorflow #==2.11.0 22 | # pip install numpy==1.22.0 23 | 24 | # Appends Protobuf to .bashrc (don't mess with bashrc leave that up to users) 25 | # echo $bashrcExport >> ~/.bashrc 26 | # reset 27 | 28 | cd "${RESEARCH_DIR}" && \ 29 | protoc object_detection/protos/*.proto --python_out=. 30 | 31 | # Clones COCO repo and makes pythonAPI 32 | # Copies pycocotools in /efficientdet-d0-trainer/models/research/ 33 | cd "${EFFDET_SRC}/software" && \ 34 | git clone https://github.com/cocodataset/cocoapi.git 35 | cd "cocoapi/PythonAPI" && \ 36 | make && \ 37 | cp -r pycocotools $RESEARCH_DIR 38 | 39 | # Installs Object Detection API 40 | cd "${RESEARCH_DIR}" && \ 41 | cp object_detection/packages/tf2/setup.py . && \ 42 | python3 -m pip install --use-pep517 . 43 | 44 | # Installs Panda Package 45 | # pip install pandas 46 | 47 | echo -e "\n" 48 | echo "Recommended that you test the instalations of TensorFlow and the Object" 49 | echo "Detection API with the installation verification instruction at the github:" 50 | echo "https://github.com/ethan-wst/efficientdet-d0-trainer.git" 51 | echo -e "\n" 52 | 53 | cd ${PROJECT_ROOT} 54 | -------------------------------------------------------------------------------- /buffpy/scripts/install.bash: -------------------------------------------------------------------------------- 1 | #! /bin/bash 2 | 3 | 4 | # 5 | # Export some variables 6 | # 7 | 8 | export UBUNTU_VERSION=$(cut -f2 <<< $(lsb_release -r)) 9 | export DEBIAN_FRONTEND=noninteractive # prevent prompts in docker and everywhere else 10 | 11 | # Setup robot params 12 | export DOCKER=False 13 | export PROJECT_ROOT=${PWD} 14 | export HOSTNAME=$HOSTNAME 15 | export SUDO='sudo' 16 | 17 | if [[ -f /.dockerenv ]]; then 18 | SUDO='' 19 | DOCKER=True 20 | PROJECT_ROOT=/home/cu-robotics/buff-code 21 | fi 22 | 23 | if [[ "${UBUNTU_VERSION}" == "22.04" ]]; then 24 | echo "WARNING: You are using Ubuntu 22.04. ros noetic is not supported, but we will attempt installation from source anyway... (^C to cancel)" 25 | sleep 10 26 | export ROS_DISTRO=noetic 27 | elif [[ "${UBUNTU_VERSION}" == "20.04" ]]; then 28 | export ROS_DISTRO=noetic # ROS for Ubuntu18 29 | elif [[ "${UBUNTU_VERSION}" == "18.04" ]]; then 30 | export ROS_DISTRO=melodic 31 | fi 32 | 33 | export ROS_PKG=desktop 34 | 35 | 36 | # 37 | # Update the apt package manager 38 | # 39 | 40 | echo -e "\n\tapt updating...\n" 41 | 42 | $SUDO apt update 43 | 44 | # Make some space if on Jetson 45 | if [[ "${HOSTNAME}" == "edge"* ]]; then 46 | $SUDO apt purge -y thunderbird libreoffice-* 47 | fi 48 | 49 | $SUDO apt upgrade -y 50 | 51 | 52 | # 53 | # Install BuffCode 54 | # 55 | 56 | source ${PROJECT_ROOT}/buffpy/scripts/install_buffpy.bash 57 | 58 | $SUDO apt autoremove -y 59 | $SUDO apt clean 60 | $SUDO apt update 61 | 62 | # 63 | # Check for ROS install (installs if none) 64 | # 65 | 66 | if [[ ! -d /opt/ros/${ROS_DISTRO} ]]; then 67 | if [[ "${UBUNTU_VERSION}" == "22.04" ]]; then 68 | # source ${PROJECT_ROOT}/buffpy/scripts/install_ros2.bash 69 | source ${PROJECT_ROOT}/buffpy/scripts/install_ros_source.bash 70 | else 71 | source ${PROJECT_ROOT}/buffpy/scripts/install_ros.bash 72 | fi 73 | 74 | fi 75 | 76 | $SUDO apt autoremove -y 77 | $SUDO apt clean 78 | $SUDO apt update 79 | 80 | 81 | # 82 | # Install Utilities 83 | # 84 | 85 | source ${PROJECT_ROOT}/buffpy/scripts/install_tytools.bash 86 | 87 | curl --proto '=https' --tlsv1.2 -sSf https://sh.rustup.rs -sSf | sh -s -- -y 88 | 89 | if [[ "${HOSTNAME}" == "edge"* ]]; then 90 | $SUDO cp ${PROJECT_ROOT}/buffpy/scripts/buffbot.service /etc/systemd/system 91 | elif [[ "${DOCKER}" == "False" ]]; then 92 | source ${PROJECT_ROOT}/buffpy/scripts/install_docker.bash 93 | curl -sSL http://get.gazebosim.org | sh 94 | fi 95 | 96 | 97 | -------------------------------------------------------------------------------- /buffpy/scripts/install_buffpy.bash: -------------------------------------------------------------------------------- 1 | #! /bin/bash 2 | 3 | # 4 | # First install system dependencies through apt 5 | # 6 | 7 | echo -e "\n\tInstalling BuffCode Dependencies...\n" 8 | 9 | $SUDO xargs apt install -y --no-install-recommends <${PROJECT_ROOT}/buffpy/data/install/dependencies.txt 10 | 11 | # 12 | # Install pip with get-pip 13 | # the system pip (from apt) is custom and 14 | # we would rather use vanilla pip (basic). 15 | 16 | echo -e "\n\tInstalling pip3\n" 17 | 18 | curl https://bootstrap.pypa.io/get-pip.py -o get-pip.py 19 | 20 | $SUDO python3 get-pip.py 21 | 22 | rm get-pip.py 23 | 24 | # 25 | # Install python requirements with pip3 26 | # 27 | 28 | echo -e "\n\tInstalling BuffCode python3 requirements\n" 29 | 30 | pip3 install -r ${PROJECT_ROOT}/buffpy/data/install/python3_requirements.txt 31 | -------------------------------------------------------------------------------- /buffpy/scripts/install_docker.bash: -------------------------------------------------------------------------------- 1 | #! /bin/bash 2 | 3 | sudo apt-get remove docker docker-engine docker.io containerd runc 4 | 5 | sudo apt update 6 | 7 | sudo apt -y install \ 8 | ca-certificates \ 9 | curl \ 10 | gnupg \ 11 | lsb-release \ 12 | apt-transport-https \ 13 | software-properties-common 14 | 15 | curl -fsSL https://download.docker.com/linux/ubuntu/gpg | sudo apt-key add - 16 | sudo add-apt-repository "deb [arch=amd64] https://download.docker.com/linux/ubuntu `lsb_release -cs` test" 17 | sudo apt update 18 | sudo apt -y install docker-ce 19 | 20 | 21 | sudo apt update 22 | 23 | sudo apt -y install docker-ce docker-ce-cli containerd.io docker-compose-plugin 24 | 25 | docker run hello-world 26 | 27 | sudo apt -y install qemu binfmt-support qemu-user-static 28 | 29 | docker run --rm --privileged multiarch/qemu-user-static --reset -p yes 30 | -------------------------------------------------------------------------------- /buffpy/scripts/install_jetson_inference.bash: -------------------------------------------------------------------------------- 1 | #! /bin/bash 2 | 3 | # switch to the home directory 4 | cd $HOME 5 | 6 | # Clone the jetson inference repo 7 | git clone --recursive https://github.com/dusty-nv/jetson-inference 8 | 9 | # Switch to the jetson inference repo 10 | cd jetson-inference 11 | 12 | # Make the build dir and switch into it 13 | mkdir build 14 | cd build 15 | 16 | # Build it using CMake 17 | cmake ../ 18 | make -j$(nproc) 19 | 20 | # Install the repo 21 | sudo make install 22 | 23 | # Setup links 24 | sudo ldconfig -------------------------------------------------------------------------------- /buffpy/scripts/install_macos.bash: -------------------------------------------------------------------------------- 1 | #! /bin/bash 2 | 3 | # run this script with 'source scripts/install.bash' 4 | ##### DEPRECATED ###### 5 | PROJECT_ROOT=$PWD 6 | 7 | echo -e "Running Install from ${PWD}" 8 | 9 | if [[ $PROJECT_ROOT != *"/buff-code" ]]; then 10 | echo -e "Run this script from the project root" 11 | exit 12 | fi 13 | 14 | echo -e "\n\tapt updating...\n" 15 | # update the apt package manager 16 | sudo apt update 17 | 18 | echo -e "Installing python3 pip" 19 | sudo apt install -y python3 python3-dev python3-pip build-essential 20 | 21 | echo -e "\n\tInstalling Dependencies...\n" 22 | # Using apt and pip install all the dependencies for the project 23 | xargs sudo apt install -y <${PROJECT_ROOT}/config/install/dependencies.txt 24 | 25 | echo -e "\n\tUpgrading pip3\n" 26 | # upgrade pip before installing dependencies 27 | python3 -m pip install --upgrade pip==21.3.1 28 | 29 | echo -e "\n\tInstalling python3 requirements\n" 30 | python3 -m pip install -r ${PROJECT_ROOT}/config/install/python3_requirements.txt 31 | 32 | 33 | 34 | # If no ROS, install it 35 | if [[ $ROS_DISTRO == "" ]]; then 36 | source "${PROJECT_ROOT}/scripts/install_ros_melodic.bash" melodic desktop 37 | fi 38 | 39 | # Also install Sublime Text-editor 40 | source "${PROJECT_ROOT}/scripts/install_sublime.bash" 41 | 42 | 43 | if [[ "${HOSTNAME}" != "edge"* ]]; then 44 | echo -e "\n\tInstalling Teensy loader...\n" 45 | 46 | # Pull teensy files from pjrc.com 47 | # teensy binary and objects 48 | curl https://www.pjrc.com/teensy/teensy_linux64.tar.gz -O 49 | # teensy rules file 50 | curl https://www.pjrc.com/teensy/00-teensy.rules -O 51 | 52 | # mv rules into rules.d and set the proper file permissions 53 | sudo mv 00-teensy.rules /etc/udev/rules.d/00-teensy.rules 54 | sudo chmod 0644 /etc/udev/rules.d/00-teensy.rules 55 | 56 | # extract the tar to buffpy/bin 57 | tar -xvsf teensy_linux64.tar.gz -C ${PROJECT_ROOT}/buffpy/bin 58 | # remove unecessary tar.gz 59 | rm teensy_linux64.tar.gz 60 | fi 61 | -------------------------------------------------------------------------------- /buffpy/scripts/install_ros.bash: -------------------------------------------------------------------------------- 1 | #! /bin/bash 2 | 3 | 4 | # 5 | # Setup ROS repositories 6 | # 7 | 8 | echo -e "\n\tSetting up ROS ${ROS_DISTRO} ${ROS_PKG}\n" 9 | 10 | $SUDO sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' 11 | 12 | 13 | # 14 | # Setup ROS keys 15 | # 16 | 17 | curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | $SUDO apt-key add - 18 | 19 | $SUDO apt update 20 | 21 | 22 | # 23 | # Install ROS 24 | # 25 | 26 | echo -e "\n\tInstalling ros-${ROS_DISTRO}-${ROS_PKG}\n" 27 | 28 | $SUDO apt install -y --no-install-recommends ros-${ROS_DISTRO}-${ROS_PKG} ros-${ROS_DISTRO}-rqt ros-${ROS_DISTRO}-rqt-common-plugins ros-${ROS_DISTRO}-catkin ros-${ROS_DISTRO}-xacro ros-${ROS_DISTRO}-rqt-robot-plugins python3-catkin-tools 29 | 30 | $SUDO apt update 31 | 32 | 33 | # 34 | # Init rosdep 35 | # 36 | 37 | if [[ "${HOSTNAME}" != "edge"* ]]; then 38 | echo -e "\n\tSetting up rosdep\n" 39 | 40 | $SUDO apt update 41 | $SUDO apt install python3-rosdep2 42 | 43 | cd /opt/ros/${ROS_DISTRO} 44 | 45 | $SUDO rosdep init 46 | rosdep update 47 | fi 48 | 49 | $SUDO apt update 50 | 51 | # 52 | # Return to project 53 | # 54 | cd ${PROJECT_ROOT} 55 | 56 | 57 | 58 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /buffpy/scripts/install_ros2.bash: -------------------------------------------------------------------------------- 1 | #! /bin/bash 2 | 3 | 4 | # 5 | # Setup ROS repositories 6 | # 7 | 8 | sudo apt install software-properties-common 9 | sudo add-apt-repository universe 10 | 11 | # 12 | # Setup ROS keys 13 | # 14 | 15 | sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg 16 | echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null 17 | 18 | # 19 | # Install ROS 20 | # 21 | 22 | echo -e "\n\tInstalling ros-${ROS_DISTRO}-${ROS_PKG}\n" 23 | 24 | $SUDO apt update 25 | $SUDO apt upgrade 26 | 27 | $SUDO apt install -y --no-install-recommends ros-${ROS_DISTRO}-${ROS_PKG} 28 | 29 | 30 | # 31 | # Return to project 32 | # 33 | cd ${PROJECT_ROOT} 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /buffpy/scripts/install_ros_source.bash: -------------------------------------------------------------------------------- 1 | # Ubuntu/Debian steps from https://wiki.ros.org/noetic/Installation/Source 2 | # 1. Prerequisites 3 | sudo apt-get install python3-rosdep python3-rosinstall-generator python3-vcstools python3-vcstool build-essential 4 | 5 | # 1.2 initialize rosdep 6 | sudo rosdep init 7 | rosdep update 8 | 9 | 10 | # 2. Installation 11 | # 2.1 Create a catkin Workspace 12 | mkdir ~/ros_catkin_ws 13 | cd ~/ros_catkin_ws 14 | 15 | # download ROS source packages 16 | rosinstall_generator desktop --rosdistro noetic --deps --tar > noetic-desktop.rosinstall 17 | mkdir ./src 18 | vcs import --input noetic-desktop.rosinstall ./src 19 | 20 | # 2.1.1 Resolving Dependencies 21 | rosdep install --from-paths ./src --ignore-packages-from-source --rosdistro noetic -y 22 | 23 | # 2.1.2 Building the catkin Workspace 24 | ## ensure this uses python3. Append to following command if python is broken, updating the path accordingly: -DPYTHON_EXECUTABLE=/usr/bin/python3 25 | ./src/catkin/bin/catkin_make_isolated --install -DCMAKE_BUILD_TYPE=Release\ 26 | source ~/ros_catkin_ws/install_isolated/setup.bash 27 | 28 | 29 | # 3. Maintaining a Source Checkout 30 | # 3.1 Update your rosinstall file 31 | mv -i noetic-desktop.rosinstall noetic-desktop.rosinstall.old 32 | rosinstall_generator desktop --rosdistro noetic --deps --tar > noetic-desktop.rosinstall 33 | diff -u noetic-desktop.rosinstall noetic-desktop.rosinstall.old # this might not be required for automated install 34 | 35 | # 3.2 Download the latest sources 36 | vcs import --input noetic-desktop.rosinstall ./src 37 | 38 | # 3.3 Rebuild your workspace 39 | ./src/catkin/bin/catkin_make_isolated --install 40 | source ~/ros_catkin_ws/install_isolated/setup.bash -------------------------------------------------------------------------------- /buffpy/scripts/install_sublime.bash: -------------------------------------------------------------------------------- 1 | #! /bin/bash 2 | 3 | # This install was taken from 4 | # https://www.sublimetext.com/docs/linux_repositories.html 5 | 6 | # Install GPG key 7 | wget -qO - https://download.sublimetext.com/sublimehq-pub.gpg | sudo apt-key add - 8 | 9 | # Select the channel to use, this script uses stable 10 | echo "deb https://download.sublimetext.com/ apt/stable/" | sudo tee /etc/apt/sources.list.d/sublime-text.list 11 | 12 | # Update Apt 13 | sudo apt update 14 | 15 | # Install dependencies 16 | sudo apt install -y apt-transport-https 17 | 18 | # Update apt again 19 | sudo apt update 20 | 21 | # Install Sublime 22 | sudo apt install -y sublime-text -------------------------------------------------------------------------------- /buffpy/scripts/install_tytools.bash: -------------------------------------------------------------------------------- 1 | #! /bin/bash 2 | 3 | # 4 | # Setup Teensy Rules 5 | # 6 | 7 | echo -e "\n\tInstalling Teensy Rules...\n" 8 | 9 | curl https://www.pjrc.com/teensy/00-teensy.rules -O 10 | 11 | # mv rules into rules.d and set the proper file permissions 12 | $SUDO mv 00-teensy.rules /etc/udev/rules.d/00-teensy.rules 13 | $SUDO chmod 0644 /etc/udev/rules.d/00-teensy.rules 14 | 15 | # Recommended 16 | curl -fsSL https://raw.githubusercontent.com/platformio/platformio-core/master/scripts/99-platformio-udev.rules | sudo tee /etc/udev/rules.d/99-platformio-udev.rules 17 | 18 | # OR, manually download and copy this file to destination folder 19 | sudo cp 99-platformio-udev.rules /etc/udev/rules.d/99-platformio-udev.rules 20 | 21 | echo -e "\n\tInstalling TYcmd...\n" 22 | 23 | cd "${PROJECT_ROOT}/.." 24 | 25 | $SUDO apt -y install build-essential cmake libudev-dev qtbase5-dev pkg-config 26 | 27 | wget https://github.com/Koromix/tytools/archive/refs/tags/v0.9.8.tar.gz 28 | 29 | tar -xf v0.9.8.tar.gz 30 | 31 | cd tytools-0.9.8 32 | 33 | mkdir -p build/linux && cd build/linux 34 | cmake -DCMAKE_INSTALL_PREFIX=/usr/local ../.. 35 | make 36 | 37 | $SUDO make install 38 | 39 | cd "${PROJECT_ROOT}" 40 | 41 | rm -rf ../tytools-0.9.8 -------------------------------------------------------------------------------- /buffpy/scripts/onnx2blob.py: -------------------------------------------------------------------------------- 1 | #! /bin/env python3 2 | 3 | import os 4 | import sys 5 | import blobconverter 6 | 7 | if len(sys.argv) > 1: 8 | print(f'Converting blob with blobconverter') 9 | 10 | project_root = os.getenv('PROJECT_ROOT') 11 | xmlfile = f"{project_root}/buffpy/models/{sys.argv[1]}.xml" 12 | binfile = f"{project_root}/buffpy/models/{sys.argv[1]}.bin" 13 | 14 | blob_path = blobconverter.from_openvino( 15 | xml=xmlfile, 16 | bin=binfile, 17 | data_type="FP16", 18 | output_dir=os.path.join(project_root, 'buffpy', 'models'), 19 | shaves=6, 20 | ) 21 | 22 | print(f'Done') 23 | -------------------------------------------------------------------------------- /buffpy/scripts/openvino_setup.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | cd ${PROJECT_ROOT}/.. 4 | if [[ ! -d "l_openvino_toolkit_p_2021.4.582" ]]; then 5 | sudo apt-get install -y pciutils cpio 6 | sudo apt autoremove 7 | 8 | wget https://github.com/PINTO0309/tflite2tensorflow/releases/download/v1.10.4/l_openvino_toolkit_p_2021.4.582.tgz 9 | 10 | ## install openvino 11 | tar xf "l_openvino_toolkit_p_2021.4.582.tgz" 12 | cd l_openvino_toolkit_p_2021.4.582/ 13 | 14 | sudo ./install_openvino_dependencies.sh && \ 15 | sed -i 's/decline/accept/g' silent.cfg && \ 16 | sudo ./install.sh --silent silent.cfg 17 | 18 | rm -rf "../l_openvino_toolkit_p_2021.4.582.tgz" 19 | 20 | /opt/intel/openvino_2021/deployment_tools/model_optimizer/install_prerequisites/install_prerequisites.sh 21 | python3 -m pip install boto3==1.17.39 blobconverter==1.3.0 22 | fi 23 | 24 | cd ${PROJECT_ROOT} -------------------------------------------------------------------------------- /buffpy/scripts/pt2blob.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | source ${PROJECT_ROOT}/buffpy/scripts/yolov5_setup.bash 4 | source ${PROJECT_ROOT}/buffpy/scripts/openvino_setup.bash 5 | 6 | cd ${PROJECT_ROOT}/../yolov5 7 | 8 | FILENAME=$1 9 | PT_FILE="${PROJECT_ROOT}/buffpy/models/${FILENAME}.pt" 10 | 11 | python3 export.py --weights "${PT_FILE}" \ 12 | --img 320 \ 13 | --batch 1 \ 14 | --device cpu \ 15 | --include "onnx" \ 16 | --simplify 17 | 18 | cd ${PROJECT_ROOT} 19 | 20 | 21 | source /opt/intel/openvino_2021/bin/setupvars.sh 22 | python3 /opt/intel/openvino_2021/deployment_tools/model_optimizer/mo.py \ 23 | --input_model "${PROJECT_ROOT}/buffpy/models/${FILENAME}.onnx" \ 24 | --model_name "${FILENAME}" \ 25 | --data_type FP16 \ 26 | --output_dir "${PROJECT_ROOT}/buffpy/models/" \ 27 | --input_shape [1,3,320,320] \ 28 | --reverse_input_channel \ 29 | --scale 255 30 | 31 | 32 | python3 ${PROJECT_ROOT}/buffpy/scripts/onnx2blob.py ${FILENAME} 33 | 34 | mv "${PROJECT_ROOT}/buffpy/models/${FILENAME}_openvino_2021.4_6shave.blob" "${PROJECT_ROOT}/buffpy/models/${FILENAME}.blob" -------------------------------------------------------------------------------- /buffpy/scripts/purge.bash: -------------------------------------------------------------------------------- 1 | #! /bin/bash 2 | 3 | 4 | # Doesn't Actually work :/ 5 | # MEANING THIS MAY PURGE YOUR ENTIRE FS 6 | # DO NOT RUN 7 | 8 | exit 9 | 10 | # 11 | # Export some variables 12 | # 13 | 14 | export ROS_PKG=ros-base # Basic ROS (haha only communication and services) 15 | export ROS_DISTRO=melodic # ROS for Ubuntu18 16 | 17 | export PROJECT_ROOT=$PWD # Path to buff-code 18 | export DEBIAN_FRONTEND=noninteractive # prevent prompts in docker and everywhere else 19 | 20 | 21 | # 22 | # Assert start-up directory 23 | # 24 | 25 | echo -e "Running purge from ${PWD}" 26 | 27 | if [[ $PROJECT_ROOT != *"/buff-code" ]]; then 28 | echo -e "source buffpy/buff.bash and run this script" 29 | exit 30 | fi 31 | 32 | 33 | # 34 | # Source buff.bash 35 | # 36 | 37 | source ${PROJECT_ROOT}/buffpy/buff.bash 38 | 39 | 40 | # 41 | # Uninstall BuffCode Python requirments 42 | # 43 | 44 | pip uninstall -r ${PROJECT_ROOT}/buffpy/config/install/ros_python_requirements.txt 45 | pip uninstall -r ${PROJECT_ROOT}/buffpy/config/install/python3_requirements.txt 46 | pip uninstall pip 47 | 48 | # 49 | # Uninstall BuffCode Dependencies 50 | # 51 | 52 | sudo xargs apt purge -y <${PROJECT_ROOT}/buffpy/config/install/dependencies.txt 53 | sudo apt purge -y ros-${ROS_DISTRO}-${ROS_PKG} ros-${ROS_DISTRO}-rqt ros-${ROS_DISTRO}-rqt-common-plugins 54 | 55 | if [[ "${HOSTNAME}" != "edge"* ]]; then 56 | sudo apt purge -y ros-${ROS_DISTRO}-robot-plugins 57 | fi 58 | 59 | sudo rm -rf /home/cu-robotics/opencv_ws 60 | 61 | sudo apt autoremove -y 62 | sudo apt clean 63 | sudo apt update 64 | 65 | 66 | # 67 | # Install Utilities 68 | # 69 | 70 | if [[ "${HOSTNAME}" != "edge"* ]]; then 71 | echo -e "\n\tPurge Purge Purge...\n" 72 | 73 | # # Pull teensy files from pjrc.com 74 | # # teensy binary and objects 75 | # curl https://www.pjrc.com/teensy/teensy_linux64.tar.gz -O 76 | # # teensy rules file 77 | # curl https://www.pjrc.com/teensy/00-teensy.rules -O 78 | 79 | # # mv rules into rules.d and set the proper file permissions 80 | # mv 00-teensy.rules /etc/udev/rules.d/00-teensy.rules 81 | # chmod 0644 /etc/udev/rules.d/00-teensy.rules 82 | 83 | # # extract the tar to buffpy/bin 84 | # tar -xvsf teensy_linux64.tar.gz -C ${PROJECT_ROOT}/buffpy/bin 85 | # # remove unecessary tar.gz 86 | # rm teensy_linux64.tar.gz 87 | 88 | else 89 | # Copy our startup service to the system units directory 90 | sudo rm -rf /etc/systemd/system/buffbot.service 91 | 92 | fi 93 | 94 | 95 | -------------------------------------------------------------------------------- /buffpy/scripts/robot_setup.bash: -------------------------------------------------------------------------------- 1 | #! /bin/bash 2 | 3 | sudo cp /home/cu-robotics/buff-code/buffpy/scripts/buffbot.service /etc/systemd/system 4 | 5 | sudo systemctl enable buffbot.service 6 | -------------------------------------------------------------------------------- /buffpy/scripts/wsl_gpu_drivers.bash: -------------------------------------------------------------------------------- 1 | #! /bin/bash 2 | 3 | 4 | wget https://developer.download.nvidia.com/compute/cuda/repos/wsl-ubuntu/x86_64/cuda-wsl-ubuntu.pin 5 | sudo mv cuda-wsl-ubuntu.pin /etc/apt/preferences.d/cuda-repository-pin-600 6 | wget https://developer.download.nvidia.com/compute/cuda/11.7.0/local_installers/cuda-repo-wsl-ubuntu-11-7-local_11.7.0-1_amd64.deb 7 | sudo dpkg -i cuda-repo-wsl-ubuntu-11-7-local_11.7.0-1_amd64.deb 8 | sudo cp /var/cuda-repo-wsl-ubuntu-11-7-local/cuda-*-keyring.gpg /usr/share/keyrings/ 9 | sudo apt-get update 10 | sudo apt-get -y install cuda 11 | rm -rf cuda-repo-wsl-ubuntu-11-7-local_11.7.0-1_amd64.deb 12 | -------------------------------------------------------------------------------- /buffpy/scripts/yolov5_setup.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | 4 | 5 | cd ${PROJECT_ROOT}/.. 6 | if [[ ! -d "yolov5" ]]; then 7 | git clone https://github.com/ultralytics/yolov5 # clone repo next to buff-code 8 | pip install torch==1.9.0+cu111 torchvision==0.10.0+cu111 tensorboard -f https://download.pytorch.org/whl/torch_stable.html 9 | fi 10 | 11 | cd ${PROJECT_ROOT} 12 | -------------------------------------------------------------------------------- /containers/base/.dockerignore: -------------------------------------------------------------------------------- 1 | src 2 | data 3 | containers 4 | buffpy/bin 5 | buffpy/lib 6 | buffpy/models 7 | buffpy/data/robots 8 | buffpy/data/build -------------------------------------------------------------------------------- /containers/base/Dockerfile: -------------------------------------------------------------------------------- 1 | # 2 | # this dockerfile roughly follows the 'Ubuntu install of ROS Melodic' from: 3 | # http://wiki.ros.org/melodic/Installation/Ubuntu 4 | # 5 | 6 | ARG IMAGE_NAME 7 | 8 | FROM ${IMAGE_NAME} 9 | 10 | ENV ROS_PKG=ros-base 11 | ENV ROS_DISTRO=noetic 12 | ENV UBUNTU_VERSION=20.04 13 | ENV DEBIAN_FRONTEND=noninteractive 14 | ENV ROS_ROOT=/opt/ros/${ROS_DISTRO} 15 | ENV PROJECT_ROOT=/home/cu-robotics/buff-code 16 | 17 | # 18 | # Create a new user 19 | # 20 | 21 | RUN useradd -ms /bin/bash cu-robotics 22 | 23 | # 24 | # Install Dependencies 25 | # 26 | 27 | USER cu-robotics 28 | COPY buffpy/ /home/cu-robotics/buff-code/buffpy 29 | 30 | # 31 | # Install Buffpy 32 | # 33 | 34 | USER root 35 | RUN apt update && \ 36 | xargs apt install -y --no-install-recommends <${PROJECT_ROOT}/buffpy/data/install/dependencies.txt && \ 37 | curl https://bootstrap.pypa.io/get-pip.py -o get-pip.py && \ 38 | python3 get-pip.py && rm -rf get-pip.py && \ 39 | pip install -r ${PROJECT_ROOT}/buffpy/data/install/python3_requirements.txt 40 | 41 | # 42 | # Install ROS Noetic 43 | # 44 | 45 | RUN sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' && \ 46 | curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | apt-key add - && \ 47 | apt update && \ 48 | apt install -y --no-install-recommends ros-${ROS_DISTRO}-${ROS_PKG} ros-${ROS_DISTRO}-rqt ros-${ROS_DISTRO}-rqt-common-plugins ros-${ROS_DISTRO}-catkin ros-${ROS_DISTRO}-xacro ros-${ROS_DISTRO}-rqt-robot-plugins 49 | 50 | # 51 | # Install Rosdep 52 | # (idk why, thats just what people do) 53 | 54 | RUN apt update && apt install -y python3-rosdep && cd /opt/ros/${ROS_DISTRO} && rosdep init && rosdep update 55 | 56 | # 57 | # Install Rust 58 | # 59 | 60 | RUN curl --proto '=https' --tlsv1.2 -sSf https://sh.rustup.rs -sSf | sh -s -- -y 61 | 62 | 63 | # 64 | # Clean apt 65 | # 66 | 67 | RUN apt update && \ 68 | apt clean && \ 69 | apt autoremove -y && \ 70 | apt upgrade -y 71 | 72 | # 73 | # setup entrypoint 74 | # 75 | 76 | RUN echo "source /home/cu-robotics/buff-code/buffpy/buff.bash" >> ~/.bashrc 77 | 78 | CMD ["bash"] 79 | ENTRYPOINT ["/home/cu-robotics/buff-code/buffpy/scripts/buff_entrypoint.sh"] 80 | WORKDIR /home/cu-robotics/buff-code 81 | -------------------------------------------------------------------------------- /containers/dev/Dockerfile: -------------------------------------------------------------------------------- 1 | # 2 | # this dockerfile roughly follows the 'Ubuntu install of ROS Melodic' from: 3 | # http://wiki.ros.org/melodic/Installation/Ubuntu 4 | # 5 | 6 | # Select base image here (ARM64 or AMD64) 7 | 8 | ARG IMAGE_NAME 9 | FROM ${IMAGE_NAME} 10 | 11 | ENV PATH="/root/.cargo/bin:${PATH}" 12 | ENV PROJECT_ROOT=/home/cu-robotics/buff-code 13 | ENV DEBIAN_FRONTEND=noninteractive 14 | 15 | # 16 | # Add your code 17 | # 18 | USER cu-robotics 19 | COPY src /home/cu-robotics/buff-code/src 20 | COPY buffpy /home/cu-robotics/buff-code/buffpy 21 | 22 | 23 | # 24 | # Build Buff-Code 25 | # 26 | 27 | USER root 28 | #RUN /home/cu-robotics/buff-code/buffpy/scripts/buffpy -b rust-debug && \ 29 | # rm -rf /home/cu-robotics/buff-code/src 30 | 31 | 32 | # 33 | # Set up the environment 34 | # 35 | 36 | CMD ["/bin/bash"] 37 | ENTRYPOINT ["/home/cu-robotics/buff-code/buffpy/scripts/buff_entrypoint.sh"] 38 | WORKDIR /home/cu-robotics/buff-code 39 | 40 | -------------------------------------------------------------------------------- /containers/docker-compose.yaml: -------------------------------------------------------------------------------- 1 | version: "3.9" 2 | services: 3 | aarch64-base: 4 | build: 5 | context: ../ 6 | dockerfile: containers/base/Dockerfile 7 | args: 8 | IMAGE_NAME: arm64v8/ubuntu:20.04 9 | network_mode: host 10 | image: curobotics/buffbox:aarch64-base 11 | 12 | x86_64-base: 13 | build: 14 | context: ../ 15 | dockerfile: containers/base/Dockerfile 16 | args: 17 | IMAGE_NAME: ubuntu:20.04 18 | network_mode: host 19 | image: curobotics/buffbox:x86_64-base 20 | 21 | aarch64-dev: 22 | build: 23 | context: ../ 24 | dockerfile: containers/dev/Dockerfile 25 | args: 26 | IMAGE_NAME: curobotics/buffbox:aarch64-base 27 | network_mode: host 28 | volumes: 29 | - ../:/home/cu-robotics/buff-code 30 | - /tmp/.X11-unix 31 | environment: 32 | - DISPLAY 33 | image: curobotics/buffbox:aarch64-dev 34 | 35 | x86_64-dev: 36 | build: 37 | context: ../ 38 | dockerfile: containers/dev/Dockerfile 39 | args: 40 | IMAGE_NAME: curobotics/buffbox:x86_64-base 41 | network_mode: host 42 | volumes: 43 | - ../:/home/cu-robotics/buff-code 44 | - /tmp/.X11-unix 45 | environment: 46 | - DISPLAY 47 | image: curobotics/buffbox:x86_64-dev 48 | 49 | 50 | -------------------------------------------------------------------------------- /src/buff_rust/.cargo/config.toml: -------------------------------------------------------------------------------- 1 | [net] 2 | git-fetch-with-cli = true -------------------------------------------------------------------------------- /src/buff_rust/Cargo.toml: -------------------------------------------------------------------------------- 1 | [package] 2 | name = "buff_rust" 3 | version = "0.2.0" 4 | edition = "2021" 5 | 6 | # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html 7 | 8 | [dependencies] 9 | # eye = "0.4.1" 10 | rand = "0.8.5" 11 | rosrust = "0.9" 12 | hidapi = "2.0.2" 13 | image = "0.24.5" 14 | # glium = "0.31.0" 15 | yaml-rust = "0.4" 16 | # nalgebra = "0.31.1" 17 | rosrust_msg = "0.1" 18 | env_logger = "0.9.0" 19 | chrono = "0.4.23" 20 | 21 | [[bin]] 22 | name = "comms_node" 23 | path = "src/teensy_comms/comms_node.rs" 24 | 25 | -------------------------------------------------------------------------------- /src/buff_rust/src/lib.rs: -------------------------------------------------------------------------------- 1 | pub mod localization; 2 | pub mod locomotion; 3 | pub mod teensy_comms; 4 | pub mod utilities; 5 | pub mod vision; 6 | -------------------------------------------------------------------------------- /src/buff_rust/src/localization/estimator_node.rs: -------------------------------------------------------------------------------- 1 | use buff_rust::localization::estimators::*; 2 | 3 | fn main() { 4 | env_logger::init(); 5 | 6 | rosrust::init("buff_estimation"); 7 | 8 | let mut rse = RobotStateEstimator::new(); 9 | 10 | rse.spin(); 11 | } 12 | -------------------------------------------------------------------------------- /src/buff_rust/src/localization/mod.rs: -------------------------------------------------------------------------------- 1 | pub mod data_structures; 2 | pub mod estimators; 3 | pub mod quadtree; 4 | pub mod tests; 5 | -------------------------------------------------------------------------------- /src/buff_rust/src/locomotion/locomotion_node.rs: -------------------------------------------------------------------------------- 1 | use buff_rust::locomotion::locomotion::*; 2 | 3 | fn main() { 4 | env_logger::init(); 5 | 6 | rosrust::init("buff_locomotion"); 7 | 8 | let mut controller = BuffLocomotion::new(); 9 | 10 | controller.spin(); 11 | } 12 | -------------------------------------------------------------------------------- /src/buff_rust/src/locomotion/mod.rs: -------------------------------------------------------------------------------- 1 | pub mod controllers; 2 | pub mod locomotion; 3 | pub mod test; 4 | -------------------------------------------------------------------------------- /src/buff_rust/src/teensy_comms/comms_node.rs: -------------------------------------------------------------------------------- 1 | use buff_rust::teensy_comms::buff_hid::*; 2 | 3 | fn main() { 4 | HidLayer::pipeline(); 5 | } 6 | -------------------------------------------------------------------------------- /src/buff_rust/src/teensy_comms/mod.rs: -------------------------------------------------------------------------------- 1 | pub mod buff_hid; 2 | pub mod data_structures; 3 | pub mod test; 4 | -------------------------------------------------------------------------------- /src/buff_rust/src/utilities/mod.rs: -------------------------------------------------------------------------------- 1 | pub mod buffers; 2 | pub mod loaders; 3 | pub mod test; 4 | pub mod viz; 5 | -------------------------------------------------------------------------------- /src/buff_rust/src/vision/mod.rs: -------------------------------------------------------------------------------- 1 | pub mod image_fps; 2 | -------------------------------------------------------------------------------- /src/crosshair/scripts/interactive_thresholder.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | import os 3 | import cv2 4 | import sys 5 | import glob 6 | import numpy as np 7 | import buffvision as bv 8 | 9 | # Someday will become its own project 10 | 11 | pixels = [] 12 | 13 | 14 | def click_event(event, x, y, flags, param): 15 | global pixels 16 | # on press mark position 17 | 18 | if event == cv2.EVENT_LBUTTONUP: 19 | print('Click detected at ({},{})'.format(x,y)) 20 | pixels.append((x,y)) 21 | 22 | def run_stats(pixels): 23 | std = [0,0,0] 24 | n = len(pixels) 25 | if n == 0: 26 | return [0,0], [0,0] 27 | 28 | average = np.mean(pixels, axis=0) 29 | for pixel in pixels: 30 | for i in range(len(pixel)): 31 | std[i] += (pixel[i] - average[i])**2 / n 32 | 33 | return average, std 34 | 35 | def main(data_dir): 36 | global pixels 37 | 38 | all_values = [] 39 | value_high = [0,0,0] 40 | value_low = [180,255,255] 41 | 42 | cv2.namedWindow("image") 43 | cv2.setMouseCallback("image", click_event) 44 | 45 | project_root = os.getenv('PROJECT_ROOT') 46 | data_path = os.path.join(project_root, 'data', data_dir) 47 | 48 | m = len(data_path) + len('labels') + 2 49 | label_paths = glob.glob(os.path.join(data_path, 'labels', '*.txt')) 50 | 51 | for i,labelf in enumerate(label_paths): 52 | imfile = os.path.join(data_path, 'images', labelf[m:-4] + '.jpg') 53 | if os.path.exists(imfile): 54 | image = cv2.imread(imfile) 55 | label = bv.load_label(labelf) 56 | 57 | print(f'{i}: image shape: {image.shape} ') 58 | hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV) 59 | 60 | #bv.buffshow('image', image) 61 | bv.display_annotated(image, label) 62 | 63 | for x, y in pixels: 64 | h, s, v = hsv[y,x] 65 | 66 | all_values.append([h,s,v]) 67 | 68 | value_low[0] = min(value_low[0], h) 69 | value_low[1] = min(value_low[1], s) 70 | value_low[2] = min(value_low[2], v) 71 | 72 | value_high[0] = max(value_high[0], h) 73 | value_high[1] = max(value_high[1], s) 74 | value_high[2] = max(value_high[2], v) 75 | 76 | pixels = [] 77 | 78 | avg, std = run_stats(all_values) 79 | 80 | print('Low: {}'.format(value_low)) 81 | print('High: {}'.format(value_high)) 82 | print('Average: {}'.format(avg)) 83 | print('STD: {}'.format(std)) 84 | 85 | 86 | 87 | if __name__ == '__main__': 88 | if len(sys.argv) > 1: 89 | main(sys.argv[1]) 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | -------------------------------------------------------------------------------- /src/cv_benchmarking/mobilenet-tf.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/python3 2 | 3 | import cv2 4 | import time 5 | import numpy as np 6 | import tensorflow as tf 7 | import matplotlib.pyplot as plt 8 | 9 | from tensorflow.keras.preprocessing import image 10 | from tensorflow.keras.applications import imagenet_utils 11 | 12 | def process_image(img, model, preprocess): 13 | #initializing the model to predict the image details using predefined models. 14 | finalimg = preprocess([img]) 15 | predictions = model.predict(finalimg) 16 | # To predict and decode the image details 17 | results = imagenet_utils.decode_predictions(predictions) 18 | return results 19 | 20 | def main(): 21 | 22 | cam_port = 0 23 | duration = 10 24 | prediction_count = 0 25 | 26 | cam = cv2.VideoCapture(cam_port, cv2.CAP_V4L2) 27 | cam.set(cv2.CAP_PROP_FRAME_WIDTH, 224) 28 | cam.set(cv2.CAP_PROP_FRAME_HEIGHT, 224) 29 | cam.set(cv2.CAP_PROP_FPS, 36) 30 | 31 | # model = tf.keras.applications.mobilenet_v2.MobileNetV2() 32 | 33 | start = time.time() 34 | 35 | while time.time() - start < duration: 36 | # reading the input using the camera 37 | result, image = cam.read() 38 | 39 | # If image will detected without any error, 40 | # show result 41 | if result: 42 | prediction_count += 1 43 | # predictions = process_image(image, model, tf.keras.applications.mobilenet_v2.preprocess_input) 44 | 45 | # If captured image is corrupted, moving to else part 46 | else: 47 | print("No image detected.") 48 | 49 | print(f"Model PPS: {prediction_count / duration}") 50 | 51 | 52 | 53 | 54 | if __name__ =='__main__': 55 | main() -------------------------------------------------------------------------------- /src/cv_benchmarking/mobilenet-torch.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/python3 2 | 3 | import time 4 | 5 | import torch 6 | import numpy as np 7 | from torchvision import models, transforms 8 | 9 | import cv2 10 | from PIL import Image 11 | 12 | torch.backends.quantized.engine = 'qnnpack' 13 | 14 | cap = cv2.VideoCapture(0, cv2.CAP_V4L2) 15 | cap.set(cv2.CAP_PROP_FRAME_WIDTH, 224) 16 | cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 224) 17 | cap.set(cv2.CAP_PROP_FPS, 36) 18 | 19 | preprocess = transforms.Compose([ 20 | transforms.ToTensor(), 21 | transforms.Normalize(mean=[0.485, 0.456, 0.406], std=[0.229, 0.224, 0.225]), 22 | ]) 23 | 24 | net = models.quantization.mobilenet_v2(pretrained=True, quantize=True) 25 | # jit model to take it from ~20fps to ~30fps 26 | net = torch.jit.script(net) 27 | 28 | duration = 10 29 | started = time.time() 30 | last_logged = time.time() 31 | frame_count = 0 32 | 33 | with torch.no_grad(): 34 | while time.time() - started < duration: 35 | # read frame 36 | ret, image = cap.read() 37 | if not ret: 38 | raise RuntimeError("failed to read frame") 39 | 40 | # convert opencv output from BGR to RGB 41 | image = image[:, :, [2, 1, 0]] 42 | permuted = image 43 | 44 | # preprocess 45 | input_tensor = preprocess(image) 46 | 47 | # create a mini-batch as expected by the model 48 | input_batch = input_tensor.unsqueeze(0) 49 | 50 | # run model 51 | output = net(input_batch) 52 | # do something with output ... 53 | 54 | # log model performance 55 | frame_count += 1 56 | now = time.time() 57 | if now - last_logged > 1: 58 | print(f"{frame_count / (now-last_logged)} fps") 59 | last_logged = now 60 | frame_count = 0 61 | 62 | print(f"{frame_count / duration} fps") -------------------------------------------------------------------------------- /src/firmware/examples/blink/main.cpp: -------------------------------------------------------------------------------- 1 | // #include "unity.h" 2 | #include "buff_cpp/timing.h" 3 | #include "buff_cpp/blink.h" 4 | 5 | int test_setup_blink(void) { 6 | // test stuff 7 | setup_blink(); 8 | return 0; 9 | } 10 | 11 | int test_blink(void) { 12 | // more test stuff 13 | blink(); 14 | blinker_status = false; 15 | bool init_status = blinker_status; 16 | blinker_timer_mark = ARM_DWT_CYCCNT; 17 | 18 | timer_set(0); 19 | while (init_status == blinker_status) { 20 | blink(); 21 | } 22 | // blinker should be precise to 0 microseconds 23 | if (abs(BLINK_RATE_US - timer_info_us(0)) > 1) { 24 | Serial.println("Failed blink test"); 25 | Serial.printf("%i != %i\n", BLINK_RATE_US, timer_info_us(0)); 26 | return 1; 27 | } 28 | // TEST_ASSERT_INT32_WITHIN(15, BLINK_RATE_US, timer_info_us(0)); 29 | return 0; 30 | } 31 | 32 | int runTests(void) { 33 | test_setup_blink(); 34 | int errs = test_blink(); 35 | return errs; 36 | 37 | // UNITY_BEGIN(); 38 | // RUN_TEST(test_setup_blink); 39 | // RUN_TEST(test_blink); 40 | // return UNITY_END(); 41 | } 42 | 43 | 44 | int main() { 45 | // Wait ~2 seconds before the Unity test runner 46 | // establishes connection with a board Serial interface 47 | while (!Serial) {}; 48 | Serial.println("Start blink tests"); 49 | delay(2000); 50 | 51 | int errors = runTests(); 52 | 53 | Serial.println("Finished tests"); 54 | Serial.printf("%i failed\n", errors); 55 | 56 | return 0; 57 | } -------------------------------------------------------------------------------- /src/firmware/examples/can/basic_can_read.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "buff_cpp/timing.h" 3 | #include "motor_drivers/rm_can_interface.h" 4 | 5 | 6 | #define TARGET_CAN_BUS 2 7 | #define TARGET_ESC_TYPE 1 8 | #define TARGET_ESC_ID 4 9 | 10 | #define VERBOSITY 2 11 | 12 | RM_CAN_Interface rm_can_ux; 13 | 14 | int run_can_test() { 15 | int errors = 0; 16 | CAN_message_t tmp; 17 | uint32_t duration = 5000; 18 | byte test[3] = {TARGET_CAN_BUS, TARGET_ESC_TYPE, TARGET_ESC_ID}; 19 | 20 | rm_can_ux.set_index(0, test); 21 | 22 | if (VERBOSITY) { 23 | print_rm_config_struct(&rm_can_ux.motor_arr[0]); 24 | for (int i = 0; i < MAX_CAN_RETURN_IDS; i++) { 25 | Serial.printf("\t[%d]\t%i\t%i\n", i, rm_can_ux.can_motor_arr[0][i], rm_can_ux.can_motor_arr[1][i]); 26 | } 27 | } 28 | 29 | timer_set(0); 30 | while(timer_info_ms(0) < duration) { 31 | if (rm_can_ux.can2.read(tmp)) { 32 | int8_t motor_id = rm_can_ux.motor_index_from_return(TARGET_CAN_BUS, tmp.id); 33 | rm_can_ux.set_feedback(TARGET_CAN_BUS, &tmp); 34 | 35 | if (VERBOSITY && motor_id == 0) { 36 | // Serial.printf("\t[%i] Invalid Device: %X:%i (rid, motor_id)\n", timer_info_us(0), tmp.id - 0x201, motor_id); 37 | Serial.printf("Motor feebdack[0]: %f\n", rm_can_ux.motor_arr[0].data[0]); 38 | } 39 | else if (VERBOSITY) { 40 | errors += 1; 41 | Serial.printf("\t[%i] Invalid Device: %X:%i (rid, motor_id)\n", timer_info_us(0), tmp.id - 0x201, motor_id); 42 | } 43 | 44 | Serial.println(motor_id); 45 | } 46 | } 47 | 48 | return errors; 49 | } 50 | 51 | int main() { 52 | // Wait ~2 seconds before the Unity test runner 53 | // establishes connection with a board Serial interface 54 | while (!Serial) {}; 55 | if (VERBOSITY) { 56 | Serial.println("Running basic CAN read tests"); 57 | Serial.printf("\tCAN bus (number):\t%i\n\tESC Type:\t\t%i\n\tESC ID:\t\t\t%i\n", TARGET_CAN_BUS, TARGET_ESC_TYPE, TARGET_ESC_ID); 58 | } 59 | 60 | int errors = run_can_test(); 61 | 62 | if (VERBOSITY) { 63 | Serial.println("Finished tests"); 64 | Serial.printf("%i failed\n", errors); 65 | } 66 | 67 | return 0; 68 | } -------------------------------------------------------------------------------- /src/firmware/examples/controllers/test_controllers.cpp: -------------------------------------------------------------------------------- 1 | // #include 2 | // #include "unity.h" 3 | // #include "sensors/dr16.cpp" 4 | // #include "buff_cpp/timing.cpp" 5 | 6 | // BUFF_PID controller; 7 | 8 | // void setUp() { 9 | // // set stuff up here 10 | // } 11 | 12 | // void tearDown() { 13 | // // clean stuff up here 14 | // } 15 | 16 | 17 | // void test_pid() { 18 | // TEST_ASSERT_EQUAL_FLOAT(0, controller.error_sum()); 19 | 20 | // controller.set_gains(1, 0, 0); 21 | // timer_set(0); 22 | // float control = controller.update(1.0); 23 | // TEST_ASSERT_EQUAL_FLOAT(1.0 controller.update(1.0)); 24 | // TEST_ASSERT_EQUAL_FLOAT(1.0 controller.error_sum()); 25 | // TEST_ASSERT_EQUAL_FLOAT(1.0 controller.previous_error()); 26 | 27 | // controller.set_gains(0, 1, 0); 28 | // TEST_ASSERT_EQUAL_FLOAT(10.0 controller.update(9.0)); 29 | // TEST_ASSERT_EQUAL_FLOAT(10.0 controller.error_sum()); 30 | // TEST_ASSERT_EQUAL_FLOAT(9.0 controller.previous_error()); 31 | 32 | // controller.set_gains(0, 0, 1); 33 | // TEST_ASSERT_EQUAL_FLOAT(1.0 controller.update(10.0)); 34 | // TEST_ASSERT_EQUAL_FLOAT(20.0 controller.error_sum()); 35 | // TEST_ASSERT_EQUAL_FLOAT(10.0 controller.previous_error()); 36 | // } 37 | 38 | 39 | // int run_controller_tests() { 40 | // UNITY_BEGIN(); 41 | // RUN_TEST(test_pid); 42 | // return UNITY_END(); 43 | // } 44 | 45 | // // Runs once 46 | // void setup() { 47 | // // Wait ~2 seconds before the Unity test runner 48 | // // establishes connection with a board Serial interface 49 | // delay(2000); 50 | 51 | // run_controller_tests(); 52 | // } 53 | 54 | // // Runs continuously 55 | // void loop() { 56 | 57 | // } 58 | 59 | int main() { 60 | // Wait ~2 seconds before the Unity test runner 61 | // establishes connection with a board Serial interface 62 | return 0; 63 | } -------------------------------------------------------------------------------- /src/firmware/examples/hid_debug/input_dump.cpp: -------------------------------------------------------------------------------- 1 | // #include "unity.h" 2 | #include "buff_cpp/blink.h" 3 | #include "buff_cpp/timing.h" 4 | #include "buff_cpp/device_manager.h" 5 | 6 | uint32_t cycle_time_us = 1000; 7 | uint32_t cycle_time_ms = cycle_time_us / 1000; 8 | float cycle_time_s = cycle_time_us / 1E6; 9 | 10 | 11 | Device_Manager device_manager; // all firmware pipelines are implemented in this object. 12 | // Device Manager provides a single function call for each of the pipelines 13 | // with unit tests we can analyze the execution time and complexity of each 14 | // pipeline. Then organize them into the master loop accordingly 15 | // Runs once 16 | void setup() { 17 | Serial.begin(1000000); // the serial monitor is actually always active (for debug use Serial.println & tycmd) 18 | 19 | if (Serial) { 20 | Serial.println("-- TEENSY SERIAL START --"); 21 | Serial.println("-- new build... who dis? --"); 22 | } 23 | } 24 | 25 | // Master loop 26 | int main() { // Basically a schudeling algorithm 27 | setup(); 28 | 29 | while(1) { 30 | timer_set(0); 31 | 32 | // handle any hid input output 33 | device_manager.read_sensors(); // read a single sensor each call (increments the sensor id automatically) 34 | device_manager.step_controllers(cycle_time_s); // given the current inputs and feedback compute a control 35 | // device_manager.hid_input_switch(); // check for an input packet (data request/control input) handle accordingly 36 | switch (device_manager.input_report.read()) { 37 | case 64: 38 | device_manager.input_report.print(); 39 | 40 | blink(); // only blink when connected to a robot 41 | if (device_manager.input_report.get(0) == 2){ 42 | return 0; 43 | } 44 | device_manager.report_switch(); 45 | device_manager.output_report.put_int32(60, ARM_DWT_CYCCNT); 46 | 47 | break; 48 | 49 | default: 50 | break; 51 | } 52 | 53 | device_manager.output_report.write(); 54 | device_manager.output_report.clear(); 55 | 56 | // device_manager.push_can(); // push data on and off the can bus 57 | device_manager.rm_can_ux.zero_can(); // Shutdown motors if can disconnects 58 | 59 | for (int i = 0; i < NUM_CAN_BUSES; i++) { 60 | device_manager.rm_can_ux.read_can(i); 61 | } 62 | 63 | timer_wait_us(0, cycle_time_us); // normalize master loop cycle time to cycle_time_us 64 | // blink(); // helpful if you think the loop is crashing (light will pause) 65 | } 66 | 67 | return 0; 68 | } -------------------------------------------------------------------------------- /src/firmware/examples/hid_debug/output_dump.cpp: -------------------------------------------------------------------------------- 1 | // #include "unity.h" 2 | #include "buff_cpp/blink.h" 3 | #include "buff_cpp/timing.h" 4 | #include "buff_cpp/device_manager.h" 5 | 6 | uint32_t cycle_time_us = 1000; 7 | uint32_t cycle_time_ms = cycle_time_us / 1000; 8 | float cycle_time_s = cycle_time_us / 1E6; 9 | 10 | 11 | Device_Manager device_manager; // all firmware pipelines are implemented in this object. 12 | // Device Manager provides a single function call for each of the pipelines 13 | // with unit tests we can analyze the execution time and complexity of each 14 | // pipeline. Then organize them into the master loop accordingly 15 | // Runs once 16 | void setup() { 17 | Serial.begin(1000000); // the serial monitor is actually always active (for debug use Serial.println & tycmd) 18 | 19 | if (Serial) { 20 | Serial.println("-- TEENSY SERIAL START --"); 21 | Serial.println("-- new build... who dis? --"); 22 | } 23 | } 24 | 25 | // Master loop 26 | int main() { // Basically a schudeling algorithm 27 | setup(); 28 | 29 | while(1) { 30 | timer_set(0); 31 | 32 | // handle any hid input output 33 | device_manager.read_sensors(); // read a single sensor each call (increments the sensor id automatically) 34 | device_manager.step_controllers(cycle_time_s); // given the current inputs and feedback compute a control 35 | // device_manager.hid_input_switch(); // check for an input packet (data request/control input) handle accordingly 36 | switch (device_manager.input_report.read()) { 37 | case 64: 38 | blink(); // only blink when connected to a robot 39 | device_manager.report_switch(); 40 | device_manager.output_report.put_int32(60, ARM_DWT_CYCCNT); 41 | device_manager.output_report.print(); 42 | 43 | break; 44 | 45 | default: 46 | break; 47 | } 48 | 49 | device_manager.output_report.write(); 50 | device_manager.output_report.clear(); 51 | 52 | // device_manager.push_can(); // push data on and off the can bus 53 | device_manager.rm_can_ux.zero_can(); // Shutdown motors if can disconnects 54 | 55 | for (int i = 0; i < NUM_CAN_BUSES; i++) { 56 | device_manager.rm_can_ux.read_can(i); 57 | } 58 | 59 | timer_wait_us(0, cycle_time_us); // normalize master loop cycle time to cycle_time_us 60 | // blink(); // helpful if you think the loop is crashing (light will pause) 61 | } 62 | 63 | return 0; 64 | } -------------------------------------------------------------------------------- /src/firmware/examples/icm20649/main.cpp: -------------------------------------------------------------------------------- 1 | // #include "unity.h" 2 | #include "buff_cpp/timing.h" 3 | #include "buff_cpp/loggers.h" 4 | #include "sensors/icm20649.h" 5 | 6 | ICM20649 imu; 7 | 8 | void setUp(void) { 9 | 10 | } 11 | 12 | void tearDown(void) { 13 | 14 | } 15 | 16 | int test_imu_read(void) { 17 | // check 18 | Serial.println("\tRead Test: ..."); 19 | timer_set(0); 20 | imu.readSensor(); 21 | timer_mark(0); 22 | Serial.println(); 23 | 24 | return float_leq(0.0, abs(imu.getAccelX()), "get accel failed check"); 25 | // TEST_ASSERT_FLOAT_IS_NOT_NAN(imu.getAccelX); 26 | // TEST_ASSERT(0.0 < abs(imu.getAccelY)); 27 | // TEST_ASSERT_FLOAT_IS_NOT_NAN(imu.getAccelY); 28 | // TEST_ASSERT(0.0 < abs(imu.getAccelZ)); 29 | // TEST_ASSERT_FLOAT_IS_NOT_NAN(imu.getAccelZ); 30 | // TEST_ASSERT(0.0 < abs(imu.getGyroX)); 31 | // TEST_ASSERT_FLOAT_IS_NOT_NAN(imu.getGyroX); 32 | // TEST_ASSERT(0.0 < abs(imu.getGyroY)); 33 | // TEST_ASSERT_FLOAT_IS_NOT_NAN(imu.getGyroY); 34 | // TEST_ASSERT(0.0 < abs(imu.getGyroZ)); 35 | // TEST_ASSERT_FLOAT_IS_NOT_NAN(imu.getGyroZ); 36 | } 37 | 38 | int run_imu_tests(void) { 39 | int errors = 0; 40 | errors += test_imu_read(); 41 | return errors; 42 | // UNITY_BEGIN(); 43 | // RUN_TEST(test_imu_read); 44 | 45 | // return UNITY_END(); 46 | } 47 | 48 | int main() { 49 | // Wait ~2 seconds before the Unity test runner 50 | // establishes connection with a board Serial interface 51 | while (!Serial) {}; 52 | Serial.println("Start icm20649 tests"); 53 | delay(2000); 54 | 55 | int errors = run_imu_tests(); 56 | 57 | Serial.println("Finished tests"); 58 | Serial.printf("%i failed\n", errors); 59 | 60 | return 0; 61 | } -------------------------------------------------------------------------------- /src/firmware/libraries/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CU-Robotics/buff-code/103f94d388423e3b665714ca481a4295f4198c7a/src/firmware/libraries/.DS_Store -------------------------------------------------------------------------------- /src/firmware/libraries/Adafruit_BusIO/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CU-Robotics/buff-code/103f94d388423e3b665714ca481a4295f4198c7a/src/firmware/libraries/Adafruit_BusIO/.DS_Store -------------------------------------------------------------------------------- /src/firmware/libraries/Adafruit_BusIO/Adafruit_I2CDevice.h: -------------------------------------------------------------------------------- 1 | #ifndef Adafruit_I2CDevice_h 2 | #define Adafruit_I2CDevice_h 3 | 4 | #include 5 | #include 6 | 7 | ///< The class which defines how we will talk to this device over I2C 8 | class Adafruit_I2CDevice { 9 | public: 10 | Adafruit_I2CDevice(uint8_t addr, TwoWire *theWire = &Wire); 11 | uint8_t address(void); 12 | bool begin(bool addr_detect = true); 13 | void end(void); 14 | bool detected(void); 15 | 16 | bool read(uint8_t *buffer, size_t len, bool stop = true); 17 | bool write(const uint8_t *buffer, size_t len, bool stop = true, 18 | const uint8_t *prefix_buffer = nullptr, size_t prefix_len = 0); 19 | bool write_then_read(const uint8_t *write_buffer, size_t write_len, 20 | uint8_t *read_buffer, size_t read_len, 21 | bool stop = false); 22 | bool setSpeed(uint32_t desiredclk); 23 | 24 | /*! @brief How many bytes we can read in a transaction 25 | * @return The size of the Wire receive/transmit buffer */ 26 | size_t maxBufferSize() { return _maxBufferSize; } 27 | 28 | private: 29 | uint8_t _addr; 30 | TwoWire *_wire; 31 | bool _begun; 32 | size_t _maxBufferSize; 33 | bool _read(uint8_t *buffer, size_t len, bool stop); 34 | }; 35 | 36 | #endif // Adafruit_I2CDevice_h 37 | -------------------------------------------------------------------------------- /src/firmware/libraries/Adafruit_BusIO/Adafruit_I2CRegister.h: -------------------------------------------------------------------------------- 1 | #ifndef _ADAFRUIT_I2C_REGISTER_H_ 2 | #define _ADAFRUIT_I2C_REGISTER_H_ 3 | 4 | #include 5 | #include 6 | 7 | typedef Adafruit_BusIO_Register Adafruit_I2CRegister; 8 | typedef Adafruit_BusIO_RegisterBits Adafruit_I2CRegisterBits; 9 | 10 | #endif 11 | -------------------------------------------------------------------------------- /src/firmware/libraries/Adafruit_BusIO/examples/i2c_address_detect/i2c_address_detect.ino: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | Adafruit_I2CDevice i2c_dev = Adafruit_I2CDevice(0x10); 4 | 5 | void setup() { 6 | while (!Serial) { delay(10); } 7 | Serial.begin(115200); 8 | Serial.println("I2C address detection test"); 9 | 10 | if (!i2c_dev.begin()) { 11 | Serial.print("Did not find device at 0x"); 12 | Serial.println(i2c_dev.address(), HEX); 13 | while (1); 14 | } 15 | Serial.print("Device found on address 0x"); 16 | Serial.println(i2c_dev.address(), HEX); 17 | } 18 | 19 | void loop() { 20 | 21 | } 22 | -------------------------------------------------------------------------------- /src/firmware/libraries/Adafruit_BusIO/examples/i2c_readwrite/i2c_readwrite.ino: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #define I2C_ADDRESS 0x60 4 | Adafruit_I2CDevice i2c_dev = Adafruit_I2CDevice(I2C_ADDRESS); 5 | 6 | 7 | void setup() { 8 | while (!Serial) { delay(10); } 9 | Serial.begin(115200); 10 | Serial.println("I2C device read and write test"); 11 | 12 | if (!i2c_dev.begin()) { 13 | Serial.print("Did not find device at 0x"); 14 | Serial.println(i2c_dev.address(), HEX); 15 | while (1); 16 | } 17 | Serial.print("Device found on address 0x"); 18 | Serial.println(i2c_dev.address(), HEX); 19 | 20 | uint8_t buffer[32]; 21 | // Try to read 32 bytes 22 | i2c_dev.read(buffer, 32); 23 | Serial.print("Read: "); 24 | for (uint8_t i=0; i<32; i++) { 25 | Serial.print("0x"); Serial.print(buffer[i], HEX); Serial.print(", "); 26 | } 27 | Serial.println(); 28 | 29 | // read a register by writing first, then reading 30 | buffer[0] = 0x0C; // we'll reuse the same buffer 31 | i2c_dev.write_then_read(buffer, 1, buffer, 2, false); 32 | Serial.print("Write then Read: "); 33 | for (uint8_t i=0; i<2; i++) { 34 | Serial.print("0x"); Serial.print(buffer[i], HEX); Serial.print(", "); 35 | } 36 | Serial.println(); 37 | } 38 | 39 | void loop() { 40 | 41 | } 42 | -------------------------------------------------------------------------------- /src/firmware/libraries/Adafruit_BusIO/examples/i2c_registers/i2c_registers.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #define I2C_ADDRESS 0x60 5 | Adafruit_I2CDevice i2c_dev = Adafruit_I2CDevice(I2C_ADDRESS); 6 | 7 | 8 | void setup() { 9 | while (!Serial) { delay(10); } 10 | Serial.begin(115200); 11 | Serial.println("I2C device register test"); 12 | 13 | if (!i2c_dev.begin()) { 14 | Serial.print("Did not find device at 0x"); 15 | Serial.println(i2c_dev.address(), HEX); 16 | while (1); 17 | } 18 | Serial.print("Device found on address 0x"); 19 | Serial.println(i2c_dev.address(), HEX); 20 | 21 | Adafruit_BusIO_Register id_reg = Adafruit_BusIO_Register(&i2c_dev, 0x0C, 2, LSBFIRST); 22 | uint16_t id; 23 | id_reg.read(&id); 24 | Serial.print("ID register = 0x"); Serial.println(id, HEX); 25 | 26 | Adafruit_BusIO_Register thresh_reg = Adafruit_BusIO_Register(&i2c_dev, 0x01, 2, LSBFIRST); 27 | uint16_t thresh; 28 | thresh_reg.read(&thresh); 29 | Serial.print("Initial threshold register = 0x"); Serial.println(thresh, HEX); 30 | 31 | thresh_reg.write(~thresh); 32 | 33 | Serial.print("Post threshold register = 0x"); Serial.println(thresh_reg.read(), HEX); 34 | } 35 | 36 | void loop() { 37 | 38 | } -------------------------------------------------------------------------------- /src/firmware/libraries/Adafruit_BusIO/examples/i2corspi_register/i2corspi_register.ino: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | // Define which interface to use by setting the unused interface to NULL! 4 | 5 | #define SPIDEVICE_CS 10 6 | Adafruit_SPIDevice *spi_dev = NULL; // new Adafruit_SPIDevice(SPIDEVICE_CS); 7 | 8 | #define I2C_ADDRESS 0x5D 9 | Adafruit_I2CDevice *i2c_dev = new Adafruit_I2CDevice(I2C_ADDRESS); 10 | 11 | void setup() { 12 | while (!Serial) { delay(10); } 13 | Serial.begin(115200); 14 | Serial.println("I2C or SPI device register test"); 15 | 16 | if (spi_dev && !spi_dev->begin()) { 17 | Serial.println("Could not initialize SPI device"); 18 | } 19 | 20 | if (i2c_dev) { 21 | if (i2c_dev->begin()) { 22 | Serial.print("Device found on I2C address 0x"); 23 | Serial.println(i2c_dev->address(), HEX); 24 | } else { 25 | Serial.print("Did not find I2C device at 0x"); 26 | Serial.println(i2c_dev->address(), HEX); 27 | } 28 | } 29 | 30 | Adafruit_BusIO_Register id_reg = Adafruit_BusIO_Register(i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, 0x0F); 31 | uint8_t id=0; 32 | id_reg.read(&id); 33 | Serial.print("ID register = 0x"); Serial.println(id, HEX); 34 | } 35 | 36 | void loop() { 37 | 38 | } 39 | -------------------------------------------------------------------------------- /src/firmware/libraries/Adafruit_BusIO/examples/spi_modetest/spi_modetest.ino: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #define SPIDEVICE_CS 10 4 | Adafruit_SPIDevice spi_dev = Adafruit_SPIDevice(SPIDEVICE_CS, 100000, SPI_BITORDER_MSBFIRST, SPI_MODE1); 5 | //Adafruit_SPIDevice spi_dev = Adafruit_SPIDevice(SPIDEVICE_CS, 13, 12, 11, 100000, SPI_BITORDER_MSBFIRST, SPI_MODE1); 6 | 7 | 8 | void setup() { 9 | while (!Serial) { delay(10); } 10 | Serial.begin(115200); 11 | Serial.println("SPI device mode test"); 12 | 13 | if (!spi_dev.begin()) { 14 | Serial.println("Could not initialize SPI device"); 15 | while (1); 16 | } 17 | } 18 | 19 | void loop() { 20 | Serial.println("\n\nTransfer test"); 21 | for (uint16_t x=0; x<=0xFF; x++) { 22 | uint8_t i = x; 23 | Serial.print("0x"); Serial.print(i, HEX); 24 | spi_dev.read(&i, 1, i); 25 | Serial.print("/"); Serial.print(i, HEX); 26 | Serial.print(", "); 27 | delay(25); 28 | } 29 | } -------------------------------------------------------------------------------- /src/firmware/libraries/Adafruit_BusIO/examples/spi_readwrite/spi_readwrite.ino: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #define SPIDEVICE_CS 10 4 | Adafruit_SPIDevice spi_dev = Adafruit_SPIDevice(SPIDEVICE_CS); 5 | 6 | 7 | void setup() { 8 | while (!Serial) { delay(10); } 9 | Serial.begin(115200); 10 | Serial.println("SPI device read and write test"); 11 | 12 | if (!spi_dev.begin()) { 13 | Serial.println("Could not initialize SPI device"); 14 | while (1); 15 | } 16 | 17 | uint8_t buffer[32]; 18 | 19 | // Try to read 32 bytes 20 | spi_dev.read(buffer, 32); 21 | Serial.print("Read: "); 22 | for (uint8_t i=0; i<32; i++) { 23 | Serial.print("0x"); Serial.print(buffer[i], HEX); Serial.print(", "); 24 | } 25 | Serial.println(); 26 | 27 | // read a register by writing first, then reading 28 | buffer[0] = 0x8F; // we'll reuse the same buffer 29 | spi_dev.write_then_read(buffer, 1, buffer, 2, false); 30 | Serial.print("Write then Read: "); 31 | for (uint8_t i=0; i<2; i++) { 32 | Serial.print("0x"); Serial.print(buffer[i], HEX); Serial.print(", "); 33 | } 34 | Serial.println(); 35 | } 36 | 37 | void loop() { 38 | 39 | } 40 | -------------------------------------------------------------------------------- /src/firmware/libraries/Adafruit_BusIO/examples/spi_registers/spi_registers.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #define SPIDEVICE_CS 10 5 | Adafruit_SPIDevice spi_dev = Adafruit_SPIDevice(SPIDEVICE_CS); 6 | 7 | void setup() { 8 | while (!Serial) { delay(10); } 9 | Serial.begin(115200); 10 | Serial.println("SPI device register test"); 11 | 12 | if (!spi_dev.begin()) { 13 | Serial.println("Could not initialize SPI device"); 14 | while (1); 15 | } 16 | 17 | Adafruit_BusIO_Register id_reg = Adafruit_BusIO_Register(&spi_dev, 0x0F, ADDRBIT8_HIGH_TOREAD); 18 | uint8_t id = 0; 19 | id_reg.read(&id); 20 | Serial.print("ID register = 0x"); Serial.println(id, HEX); 21 | 22 | Adafruit_BusIO_Register thresh_reg = Adafruit_BusIO_Register(&spi_dev, 0x0C, ADDRBIT8_HIGH_TOREAD, 2, LSBFIRST); 23 | uint16_t thresh = 0; 24 | thresh_reg.read(&thresh); 25 | Serial.print("Initial threshold register = 0x"); Serial.println(thresh, HEX); 26 | 27 | thresh_reg.write(~thresh); 28 | 29 | Serial.print("Post threshold register = 0x"); Serial.println(thresh_reg.read(), HEX); 30 | } 31 | 32 | void loop() { 33 | 34 | } 35 | -------------------------------------------------------------------------------- /src/firmware/libraries/Adafruit_ICM20X/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CU-Robotics/buff-code/103f94d388423e3b665714ca481a4295f4198c7a/src/firmware/libraries/Adafruit_ICM20X/.DS_Store -------------------------------------------------------------------------------- /src/firmware/libraries/Adafruit_ICM20X/Adafruit_ICM20649.h: -------------------------------------------------------------------------------- 1 | /*! 2 | * @file Adafruit_ICM20649.h 3 | * 4 | * I2C Driver for the Adafruit ICM20649 6-DoF Wide-Range Accelerometer and 5 | *Gyro library 6 | * 7 | * This is a library for the Adafruit ICM20649 breakout: 8 | * https://www.adafruit.com/products/4464 9 | * 10 | * Adafruit invests time and resources providing this open source code, 11 | * please support Adafruit and open-source hardware by purchasing products from 12 | * Adafruit! 13 | * 14 | * 15 | * BSD license (see license.txt) 16 | */ 17 | 18 | #ifndef _ADAFRUIT_ICM20649_H 19 | #define _ADAFRUIT_ICM20649_H 20 | 21 | #include "Adafruit_ICM20X.h" 22 | 23 | #define ICM20649_I2CADDR_DEFAULT 0x68 ///< ICM20X default i2c address 24 | 25 | /** The accelerometer data range */ 26 | typedef enum { 27 | ICM20649_ACCEL_RANGE_4_G, 28 | ICM20649_ACCEL_RANGE_8_G, 29 | ICM20649_ACCEL_RANGE_16_G, 30 | ICM20649_ACCEL_RANGE_30_G, 31 | } icm20649_accel_range_t; 32 | 33 | /** The gyro data range */ 34 | typedef enum { 35 | ICM20649_GYRO_RANGE_500_DPS, 36 | ICM20649_GYRO_RANGE_1000_DPS, 37 | ICM20649_GYRO_RANGE_2000_DPS, 38 | ICM20649_GYRO_RANGE_4000_DPS, 39 | } icm20649_gyro_range_t; 40 | 41 | /*! 42 | * @brief Class that stores state and functions for interacting with 43 | * the ST ICM20649 6-DoF Accelerometer and Gyro 44 | */ 45 | class Adafruit_ICM20649 : public Adafruit_ICM20X { 46 | public: 47 | Adafruit_ICM20649(); 48 | virtual ~Adafruit_ICM20649(){}; 49 | bool begin_I2C(uint8_t i2c_addr = ICM20649_I2CADDR_DEFAULT, 50 | TwoWire *wire = &Wire, int32_t sensor_id = 0); 51 | 52 | icm20649_accel_range_t getAccelRange(void); 53 | void setAccelRange(icm20649_accel_range_t new_accel_range); 54 | 55 | icm20649_gyro_range_t getGyroRange(void); 56 | void setGyroRange(icm20649_gyro_range_t new_gyro_range); 57 | 58 | private: 59 | void scaleValues(void); 60 | }; 61 | 62 | #endif 63 | -------------------------------------------------------------------------------- /src/firmware/libraries/Adafruit_ICM20X/assets/649.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CU-Robotics/buff-code/103f94d388423e3b665714ca481a4295f4198c7a/src/firmware/libraries/Adafruit_ICM20X/assets/649.jpg -------------------------------------------------------------------------------- /src/firmware/libraries/Adafruit_ICM20X/assets/649.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CU-Robotics/buff-code/103f94d388423e3b665714ca481a4295f4198c7a/src/firmware/libraries/Adafruit_ICM20X/assets/649.png -------------------------------------------------------------------------------- /src/firmware/libraries/Adafruit_ICM20X/assets/948.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CU-Robotics/buff-code/103f94d388423e3b665714ca481a4295f4198c7a/src/firmware/libraries/Adafruit_ICM20X/assets/948.png -------------------------------------------------------------------------------- /src/firmware/libraries/Adafruit_ICM20X/examples/adafruit_icm20x_accel_filter/adafruit_icm20x_accel_filter.ino: -------------------------------------------------------------------------------- 1 | /**************************************************/ 2 | /* ICM20X Accelerometer Digital Low Pass Filter Demo 3 | This example demonstrates the effect of changing the filter cutoff frequency 4 | for an ICM20X accelerometer's Digital Low Pass Filter 5 | 6 | A cutoff frequency is set in `setup()` and you should experiment with changing 7 | it and observing the effect on the signal. 8 | The effect of changing the accelerometer's cutoff frequency is 9 | the most apparent when using the Serial Plotter. */ 10 | /**************************************************/ 11 | 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | Adafruit_ICM20948 icm; 18 | 19 | // uncomment to use the ICM20649 20 | //#include 21 | // Adafruit_ICM20649 icm 22 | 23 | Adafruit_Sensor *accel; 24 | 25 | #define ICM_CS 10 26 | // For software-SPI mode we need SCK/MOSI/MISO pins 27 | #define ICM_SCK 13 28 | #define ICM_MISO 12 29 | #define ICM_MOSI 11 30 | 31 | void setup(void) { 32 | Serial.begin(115200); 33 | while (!Serial) 34 | delay(10); // will pause Zero, Leonardo, etc until serial console opens 35 | if (!icm.begin_I2C()) { 36 | // if (!icm.begin_SPI(ICM_CS)) { 37 | // if (!icm.begin_SPI(ICM_CS, ICM_SCK, ICM_MISO, ICM_MOSI)) { 38 | Serial.println("Failed to find ICM20X chip"); 39 | while (1) { 40 | delay(10); 41 | } 42 | } 43 | 44 | /* Available cutoff frequencies: 45 | ICM20X_ACCEL_FREQ_246_0_HZ 46 | ICM20X_ACCEL_FREQ_111_4_HZ 47 | ICM20X_ACCEL_FREQ_50_4_HZ 48 | ICM20X_ACCEL_FREQ_23_9_HZ 49 | ICM20X_ACCEL_FREQ_11_5_HZ 50 | ICM20X_ACCEL_FREQ_5_7_HZ 51 | ICM20X_ACCEL_FREQ_473_HZ 52 | */ 53 | icm.enableAccelDLPF(true, ICM20X_ACCEL_FREQ_5_7_HZ); 54 | 55 | // Get an Adafruit_Sensor compatible object for the ICM20X's accelerometer 56 | accel = icm.getAccelerometerSensor(); 57 | } 58 | 59 | void loop() { 60 | /* Get a new normalized sensor event */ 61 | sensors_event_t a; 62 | 63 | /* fill the event with the most recent data */ 64 | accel->getEvent(&a); 65 | 66 | Serial.print(a.acceleration.x); 67 | Serial.print(","); Serial.print(a.acceleration.y); 68 | Serial.print(","); Serial.print(a.acceleration.z); 69 | 70 | Serial.println(); 71 | 72 | delayMicroseconds((float)10/1000); 73 | 74 | } 75 | -------------------------------------------------------------------------------- /src/firmware/libraries/Adafruit_ICM20X/license.txt: -------------------------------------------------------------------------------- 1 | Software License Agreement (BSD License) 2 | 3 | Copyright (c) 2019 Bryan Siepert for Adafruit Industries 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 1. Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | 2. Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in the 12 | documentation and/or other materials provided with the distribution. 13 | 3. Neither the name of the copyright holders nor the 14 | names of its contributors may be used to endorse or promote products 15 | derived from this software without specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY 18 | EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 21 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | -------------------------------------------------------------------------------- /src/firmware/libraries/Adafruit_LSM6DS/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CU-Robotics/buff-code/103f94d388423e3b665714ca481a4295f4198c7a/src/firmware/libraries/Adafruit_LSM6DS/.DS_Store -------------------------------------------------------------------------------- /src/firmware/libraries/Adafruit_LSM6DS/Adafruit_ISM330DHCX.cpp: -------------------------------------------------------------------------------- 1 | 2 | /*! 3 | * @file Adafruit_ISM330DHCX.cpp Adafruit ISM330DHCX 6-DoF Accelerometer 4 | * and Gyroscope library 5 | * 6 | * Bryan Siepert for Adafruit Industries 7 | * BSD (see license.txt) 8 | */ 9 | 10 | #include "Arduino.h" 11 | #include 12 | 13 | #include "Adafruit_ISM330DHCX.h" 14 | 15 | /*! 16 | * @brief Instantiates a new ISM330DHCX class 17 | */ 18 | Adafruit_ISM330DHCX::Adafruit_ISM330DHCX(void) {} 19 | 20 | bool Adafruit_ISM330DHCX::_init(int32_t sensor_id) { 21 | // make sure we're talking to the right chip 22 | if (chipID() != ISM330DHCX_CHIP_ID) { 23 | return false; 24 | } 25 | _sensorid_accel = sensor_id; 26 | _sensorid_gyro = sensor_id + 1; 27 | _sensorid_temp = sensor_id + 2; 28 | 29 | reset(); 30 | 31 | // call base class _init() 32 | Adafruit_LSM6DS::_init(sensor_id); 33 | 34 | return true; 35 | } 36 | -------------------------------------------------------------------------------- /src/firmware/libraries/Adafruit_LSM6DS/Adafruit_ISM330DHCX.h: -------------------------------------------------------------------------------- 1 | /*! 2 | * @file Adafruit_ISM330DHCX.h 3 | * 4 | * I2C Driver for the Adafruit ISM330DHCX 6-DoF Accelerometer and Gyroscope 5 | *library 6 | * 7 | * This is a library for the Adafruit ISM330DHCX breakout: 8 | * https://www.adafruit.com/products/4480 9 | * 10 | * Adafruit invests time and resources providing this open source code, 11 | * please support Adafruit and open-source hardware by purchasing products from 12 | * Adafruit! 13 | * 14 | * 15 | * BSD license (see license.txt) 16 | */ 17 | 18 | #ifndef _ADAFRUIT_ISM330DHCX_H 19 | #define _ADAFRUIT_ISM330DHCX_H 20 | 21 | #include "Adafruit_LSM6DSOX.h" 22 | 23 | #define ISM330DHCX_CHIP_ID 0x6B ///< ISM330DHCX default device id from WHOAMI 24 | 25 | /*! 26 | * @brief Class that stores state and functions for interacting with 27 | * the ISM330DHCX I2C Digital Potentiometer 28 | */ 29 | class Adafruit_ISM330DHCX : public Adafruit_LSM6DSOX { 30 | public: 31 | Adafruit_ISM330DHCX(); 32 | ~Adafruit_ISM330DHCX(){}; 33 | 34 | private: 35 | bool _init(int32_t sensor_id); 36 | }; 37 | 38 | #endif 39 | -------------------------------------------------------------------------------- /src/firmware/libraries/Adafruit_LSM6DS/Adafruit_LSM6DS3.cpp: -------------------------------------------------------------------------------- 1 | 2 | /*! 3 | * @file Adafruit_LSM6DS3.cpp Adafruit LSM6DS3 6-DoF Accelerometer 4 | * and Gyroscope library 5 | * 6 | * Bryan Siepert for Adafruit Industries 7 | * BSD (see license.txt) 8 | */ 9 | 10 | #include "Arduino.h" 11 | #include 12 | 13 | #include "Adafruit_LSM6DS3.h" 14 | 15 | /*! 16 | * @brief Instantiates a new LSM6DS3 class 17 | */ 18 | Adafruit_LSM6DS3::Adafruit_LSM6DS3(void) {} 19 | 20 | bool Adafruit_LSM6DS3::_init(int32_t sensor_id) { 21 | // make sure we're talking to the right chip 22 | if (chipID() != LSM6DS3_CHIP_ID) { 23 | return false; 24 | } 25 | _sensorid_accel = sensor_id; 26 | _sensorid_gyro = sensor_id + 1; 27 | _sensorid_temp = sensor_id + 2; 28 | 29 | temperature_sensitivity = 16.0; 30 | 31 | reset(); 32 | 33 | // call base class _init() 34 | Adafruit_LSM6DS::_init(sensor_id); 35 | 36 | return true; 37 | } 38 | 39 | /**************************************************************************/ 40 | /*! 41 | @brief Enables and disables the I2C master bus pulllups. 42 | @param enable_pullups true to enable the I2C pullups, false to disable. 43 | */ 44 | void Adafruit_LSM6DS3::enableI2CMasterPullups(bool enable_pullups) { 45 | Adafruit_BusIO_Register master_config = Adafruit_BusIO_Register( 46 | i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, LSM6DS3_MASTER_CONFIG); 47 | Adafruit_BusIO_RegisterBits i2c_master_pu_en = 48 | Adafruit_BusIO_RegisterBits(&master_config, 1, 3); 49 | 50 | i2c_master_pu_en.write(enable_pullups); 51 | } 52 | -------------------------------------------------------------------------------- /src/firmware/libraries/Adafruit_LSM6DS/Adafruit_LSM6DS3.h: -------------------------------------------------------------------------------- 1 | /*! 2 | * @file Adafruit_LSM6DS3.h 3 | * 4 | * I2C Driver for the Adafruit LSM6DS3 6-DoF Accelerometer and Gyroscope 5 | *library 6 | * 7 | * This is a library for the Adafruit LSM6DS3 breakout: 8 | * https://www.adafruit.com/ 9 | * 10 | * Adafruit invests time and resources providing this open source code, 11 | * please support Adafruit and open-source hardware by purchasing products from 12 | * Adafruit! 13 | * 14 | * 15 | * BSD license (see license.txt) 16 | */ 17 | 18 | #ifndef _ADAFRUIT_LSM6DS3_H 19 | #define _ADAFRUIT_LSM6DS3_H 20 | 21 | #include "Adafruit_LSM6DS.h" 22 | 23 | #define LSM6DS3_CHIP_ID 0x69 ///< LSM6DS3 default device id from WHOAMI 24 | 25 | #define LSM6DS3_MASTER_CONFIG 0x1A ///< I2C Master config 26 | 27 | /*! 28 | * @brief Class that stores state and functions for interacting with 29 | * the LSM6DS3 30 | */ 31 | class Adafruit_LSM6DS3 : public Adafruit_LSM6DS { 32 | public: 33 | Adafruit_LSM6DS3(); 34 | ~Adafruit_LSM6DS3(){}; 35 | 36 | void enableI2CMasterPullups(bool enable_pullups); 37 | 38 | private: 39 | bool _init(int32_t sensor_id); 40 | }; 41 | 42 | #endif 43 | -------------------------------------------------------------------------------- /src/firmware/libraries/Adafruit_LSM6DS/Adafruit_LSM6DS33.cpp: -------------------------------------------------------------------------------- 1 | 2 | /*! 3 | * @file Adafruit_LSM6DS33.cpp Adafruit LSM6DS33 6-DoF Accelerometer 4 | * and Gyroscope library 5 | * 6 | * Bryan Siepert for Adafruit Industries 7 | * BSD (see license.txt) 8 | */ 9 | 10 | #include "Arduino.h" 11 | #include 12 | 13 | #include "Adafruit_LSM6DS33.h" 14 | 15 | /*! 16 | * @brief Instantiates a new LSM6DS33 class 17 | */ 18 | Adafruit_LSM6DS33::Adafruit_LSM6DS33(void) {} 19 | 20 | bool Adafruit_LSM6DS33::_init(int32_t sensor_id) { 21 | // make sure we're talking to the right chip 22 | if (chipID() != LSM6DS33_CHIP_ID) { 23 | return false; 24 | } 25 | _sensorid_accel = sensor_id; 26 | _sensorid_gyro = sensor_id + 1; 27 | _sensorid_temp = sensor_id + 2; 28 | 29 | temperature_sensitivity = 16.0; 30 | 31 | reset(); 32 | if (chipID() != LSM6DS33_CHIP_ID) { 33 | return false; 34 | } 35 | 36 | // call base class _init() 37 | Adafruit_LSM6DS::_init(sensor_id); 38 | 39 | return true; 40 | } 41 | -------------------------------------------------------------------------------- /src/firmware/libraries/Adafruit_LSM6DS/Adafruit_LSM6DS33.h: -------------------------------------------------------------------------------- 1 | /*! 2 | * @file Adafruit_LSM6DS33.h 3 | * 4 | * I2C Driver for the Adafruit LSM6DS33 6-DoF Accelerometer and Gyroscope 5 | *library 6 | * 7 | * This is a library for the Adafruit LSM6DS33 breakout: 8 | * https://www.adafruit.com/products/4480 9 | * 10 | * Adafruit invests time and resources providing this open source code, 11 | * please support Adafruit and open-source hardware by purchasing products from 12 | * Adafruit! 13 | * 14 | * 15 | * BSD license (see license.txt) 16 | */ 17 | 18 | #ifndef _ADAFRUIT_LSM6DS33_H 19 | #define _ADAFRUIT_LSM6DS33_H 20 | 21 | #include "Adafruit_LSM6DS.h" 22 | 23 | #define LSM6DS33_CHIP_ID 0x69 ///< LSM6DS33 default device id from WHOAMI 24 | 25 | /*! 26 | * @brief Class that stores state and functions for interacting with 27 | * the LSM6DS33 I2C Digital Potentiometer 28 | */ 29 | class Adafruit_LSM6DS33 : public Adafruit_LSM6DS { 30 | public: 31 | Adafruit_LSM6DS33(); 32 | ~Adafruit_LSM6DS33(){}; 33 | 34 | private: 35 | bool _init(int32_t sensor_id); 36 | }; 37 | 38 | #endif 39 | -------------------------------------------------------------------------------- /src/firmware/libraries/Adafruit_LSM6DS/Adafruit_LSM6DS3TRC.cpp: -------------------------------------------------------------------------------- 1 | 2 | /*! 3 | * @file Adafruit_LSM6DS3TRC.cpp Adafruit LSM6DS3TR-C 6-DoF Accelerometer 4 | * and Gyroscope library 5 | * 6 | * Written by ladyada for Adafruit Industries 7 | * BSD (see license.txt) 8 | */ 9 | 10 | #include "Arduino.h" 11 | #include 12 | 13 | #include "Adafruit_LSM6DS3TRC.h" 14 | 15 | /*! 16 | * @brief Instantiates a new LSM6DS3TRC class 17 | */ 18 | Adafruit_LSM6DS3TRC::Adafruit_LSM6DS3TRC(void) {} 19 | 20 | bool Adafruit_LSM6DS3TRC::_init(int32_t sensor_id) { 21 | // make sure we're talking to the right chip 22 | if (chipID() != LSM6DS3TRC_CHIP_ID) { 23 | return false; 24 | } 25 | _sensorid_accel = sensor_id; 26 | _sensorid_gyro = sensor_id + 1; 27 | _sensorid_temp = sensor_id + 2; 28 | 29 | reset(); 30 | 31 | // call base class _init() 32 | Adafruit_LSM6DS::_init(sensor_id); 33 | 34 | return true; 35 | } 36 | 37 | /**************************************************************************/ 38 | /*! 39 | @brief Enables and disables the pedometer function 40 | @param enable True to turn on the pedometer function, false to turn off 41 | */ 42 | /**************************************************************************/ 43 | void Adafruit_LSM6DS3TRC::enablePedometer(bool enable) { 44 | // enable or disable functionality 45 | Adafruit_BusIO_Register ctrl10 = Adafruit_BusIO_Register( 46 | i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, LSM6DS_CTRL10_C); 47 | 48 | Adafruit_BusIO_RegisterBits ped_en = 49 | Adafruit_BusIO_RegisterBits(&ctrl10, 1, 4); 50 | ped_en.write(enable); 51 | 52 | Adafruit_BusIO_RegisterBits func_en = 53 | Adafruit_BusIO_RegisterBits(&ctrl10, 1, 2); 54 | func_en.write(enable); 55 | 56 | resetPedometer(); 57 | } 58 | 59 | /**************************************************************************/ 60 | /*! 61 | @brief Enables and disables the I2C master bus pulllups. 62 | @param enable_pullups true to enable the I2C pullups, false to disable. 63 | */ 64 | void Adafruit_LSM6DS3TRC::enableI2CMasterPullups(bool enable_pullups) { 65 | Adafruit_BusIO_Register master_config = Adafruit_BusIO_Register( 66 | i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, LSM6DS3TRC_MASTER_CONFIG); 67 | Adafruit_BusIO_RegisterBits i2c_master_pu_en = 68 | Adafruit_BusIO_RegisterBits(&master_config, 1, 3); 69 | 70 | i2c_master_pu_en.write(enable_pullups); 71 | } 72 | -------------------------------------------------------------------------------- /src/firmware/libraries/Adafruit_LSM6DS/Adafruit_LSM6DS3TRC.h: -------------------------------------------------------------------------------- 1 | /*! 2 | * @file Adafruit_LSM6DSL.h 3 | * 4 | * I2C Driver for the Adafruit LSM6DSL 6-DoF Accelerometer and Gyroscope 5 | *library 6 | * 7 | * This is a library for the Adafruit LSM6DSL breakout: 8 | * https://www.adafruit.com/ 9 | * 10 | * Adafruit invests time and resources providing this open source code, 11 | * please support Adafruit and open-source hardware by purchasing products from 12 | * Adafruit! 13 | * 14 | * 15 | * BSD license (see license.txt) 16 | */ 17 | 18 | #ifndef _ADAFRUIT_LSM6DS3TRC_H 19 | #define _ADAFRUIT_LSM6DS3TRC_H 20 | 21 | #include "Adafruit_LSM6DS.h" 22 | 23 | #define LSM6DS3TRC_CHIP_ID 0x6A ///< LSM6DSL default device id from WHOAMI 24 | 25 | #define LSM6DS3TRC_MASTER_CONFIG 0x1A ///< I2C Master config 26 | 27 | /*! 28 | * @brief Class that stores state and functions for interacting with 29 | * the LSM6DS3TRC 30 | */ 31 | class Adafruit_LSM6DS3TRC : public Adafruit_LSM6DS { 32 | public: 33 | Adafruit_LSM6DS3TRC(); 34 | ~Adafruit_LSM6DS3TRC(){}; 35 | 36 | void enableI2CMasterPullups(bool enable_pullups); 37 | void enablePedometer(bool enable); 38 | 39 | private: 40 | bool _init(int32_t sensor_id); 41 | }; 42 | 43 | #endif 44 | -------------------------------------------------------------------------------- /src/firmware/libraries/Adafruit_LSM6DS/Adafruit_LSM6DSL.cpp: -------------------------------------------------------------------------------- 1 | 2 | /*! 3 | * @file Adafruit_LSM6DSL.cpp Adafruit LSM6DSL 6-DoF Accelerometer 4 | * and Gyroscope library 5 | * 6 | * Adapted by Eugene Anikin for Adafruit Industries 7 | * BSD (see license.txt) 8 | */ 9 | 10 | #include "Arduino.h" 11 | #include 12 | 13 | #include "Adafruit_LSM6DSL.h" 14 | 15 | /*! 16 | * @brief Instantiates a new LSM6DSL class 17 | */ 18 | Adafruit_LSM6DSL::Adafruit_LSM6DSL(void) {} 19 | 20 | bool Adafruit_LSM6DSL::_init(int32_t sensor_id) { 21 | // make sure we're talking to the right chip 22 | if (chipID() != LSM6DSL_CHIP_ID) { 23 | return false; 24 | } 25 | _sensorid_accel = sensor_id; 26 | _sensorid_gyro = sensor_id + 1; 27 | _sensorid_temp = sensor_id + 2; 28 | 29 | reset(); 30 | 31 | // call base class _init() 32 | Adafruit_LSM6DS::_init(sensor_id); 33 | 34 | return true; 35 | } 36 | 37 | /**************************************************************************/ 38 | /*! 39 | @brief Enables and disables the I2C master bus pulllups. 40 | @param enable_pullups true to enable the I2C pullups, false to disable. 41 | */ 42 | void Adafruit_LSM6DSL::enableI2CMasterPullups(bool enable_pullups) { 43 | Adafruit_BusIO_Register master_config = Adafruit_BusIO_Register( 44 | i2c_dev, spi_dev, ADDRBIT8_HIGH_TOREAD, LSM6DSL_MASTER_CONFIG); 45 | Adafruit_BusIO_RegisterBits i2c_master_pu_en = 46 | Adafruit_BusIO_RegisterBits(&master_config, 1, 3); 47 | 48 | i2c_master_pu_en.write(enable_pullups); 49 | } 50 | -------------------------------------------------------------------------------- /src/firmware/libraries/Adafruit_LSM6DS/Adafruit_LSM6DSL.h: -------------------------------------------------------------------------------- 1 | /*! 2 | * @file Adafruit_LSM6DSL.h 3 | * 4 | * I2C Driver for the Adafruit LSM6DSL 6-DoF Accelerometer and Gyroscope 5 | *library 6 | * 7 | * This is a library for the Adafruit LSM6DSL breakout: 8 | * https://www.adafruit.com/ 9 | * 10 | * Adafruit invests time and resources providing this open source code, 11 | * please support Adafruit and open-source hardware by purchasing products from 12 | * Adafruit! 13 | * 14 | * 15 | * BSD license (see license.txt) 16 | */ 17 | 18 | #ifndef _ADAFRUIT_LSM6DSL_H 19 | #define _ADAFRUIT_LSM6DSL_H 20 | 21 | #include "Adafruit_LSM6DS.h" 22 | 23 | #define LSM6DSL_CHIP_ID 0x6A ///< LSM6DSL default device id from WHOAMI 24 | 25 | #define LSM6DSL_MASTER_CONFIG 0x1A ///< I2C Master config 26 | 27 | /*! 28 | * @brief Class that stores state and functions for interacting with 29 | * the LSM6DSL 30 | */ 31 | class Adafruit_LSM6DSL : public Adafruit_LSM6DS { 32 | public: 33 | Adafruit_LSM6DSL(); 34 | ~Adafruit_LSM6DSL(){}; 35 | 36 | void enableI2CMasterPullups(bool enable_pullups); 37 | 38 | private: 39 | bool _init(int32_t sensor_id); 40 | }; 41 | 42 | #endif 43 | -------------------------------------------------------------------------------- /src/firmware/libraries/Adafruit_LSM6DS/Adafruit_LSM6DSO32.h: -------------------------------------------------------------------------------- 1 | /*! 2 | * @file Adafruit_LSM6DSO32.h 3 | * 4 | * I2C Driver for the Adafruit LSM6DSO32 6-DoF Accelerometer and Gyroscope 5 | *library 6 | * 7 | * This is a library for the Adafruit LSM6DSO32 breakout: 8 | * https://www.adafruit.com/products/PID_HERE 9 | * 10 | * Adafruit invests time and resources providing this open source code, 11 | * please support Adafruit and open-source hardware by purchasing products from 12 | * Adafruit! 13 | * 14 | * 15 | * BSD license (see license.txt) 16 | */ 17 | 18 | #ifndef _ADAFRUIT_LSM6DSO32_H 19 | #define _ADAFRUIT_LSM6DSO32_H 20 | 21 | #include "Adafruit_LSM6DS.h" 22 | #include "Adafruit_LSM6DSOX.h" 23 | #define LSM6DSO32_CHIP_ID 0x6C ///< LSM6DSO32 default device id from WHOAMI 24 | 25 | /** The accelerometer data range */ 26 | typedef enum dso32_accel_range { 27 | LSM6DSO32_ACCEL_RANGE_4_G, 28 | LSM6DSO32_ACCEL_RANGE_32_G, 29 | LSM6DSO32_ACCEL_RANGE_8_G, 30 | LSM6DSO32_ACCEL_RANGE_16_G 31 | } lsm6dso32_accel_range_t; 32 | 33 | /*! 34 | * @brief Class that stores state and functions for interacting with 35 | * the LSM6DSO32 I2C Digital Potentiometer 36 | */ 37 | class Adafruit_LSM6DSO32 : public Adafruit_LSM6DSOX { 38 | public: 39 | Adafruit_LSM6DSO32(); 40 | 41 | lsm6dso32_accel_range_t getAccelRange(void); 42 | void setAccelRange(lsm6dso32_accel_range_t new_range); 43 | void _read(void); 44 | 45 | private: 46 | bool _init(int32_t sensor_id); 47 | }; 48 | 49 | #endif 50 | -------------------------------------------------------------------------------- /src/firmware/libraries/Adafruit_LSM6DS/Adafruit_LSM6DSOX.h: -------------------------------------------------------------------------------- 1 | /*! 2 | * @file Adafruit_LSM6DSOX.h 3 | * 4 | * I2C Driver for the Adafruit LSM6DSOX 6-DoF Accelerometer and Gyroscope 5 | *library 6 | * 7 | * This is a library for the Adafruit LSM6DSOX breakout: 8 | * https://www.adafruit.com/products/PID_HERE 9 | * 10 | * Adafruit invests time and resources providing this open source code, 11 | * please support Adafruit and open-source hardware by purchasing products from 12 | * Adafruit! 13 | * 14 | * 15 | * BSD license (see license.txt) 16 | */ 17 | 18 | #ifndef _ADAFRUIT_LSM6DSOX_H 19 | #define _ADAFRUIT_LSM6DSOX_H 20 | 21 | #include "Adafruit_LSM6DS.h" 22 | 23 | #define LSM6DSOX_CHIP_ID 0x6C ///< LSM6DSOX default device id from WHOAMI 24 | 25 | #define LSM6DSOX_FUNC_CFG_ACCESS 0x1 ///< Enable embedded functions register 26 | #define LSM6DSOX_PIN_CTRL 0x2 ///< Pin control register 27 | 28 | #define LSM6DSOX_INT1_CTRL 0x0D ///< Interrupt enable for data ready 29 | #define LSM6DSOX_CTRL1_XL 0x10 ///< Main accelerometer config register 30 | #define LSM6DSOX_CTRL2_G 0x11 ///< Main gyro config register 31 | #define LSM6DSOX_CTRL3_C 0x12 ///< Main configuration register 32 | #define LSM6DSOX_CTRL9_XL 0x18 ///< Includes i3c disable bit 33 | 34 | #define LSM6DSOX_MASTER_CONFIG 0x14 35 | ///< I2C Master config; access must be enabled with bit SHUB_REG_ACCESS 36 | ///< is set to '1' in FUNC_CFG_ACCESS (01h). 37 | 38 | /*! 39 | * @brief Class that stores state and functions for interacting with 40 | * the LSM6DSOX I2C Digital Potentiometer 41 | */ 42 | class Adafruit_LSM6DSOX : public Adafruit_LSM6DS { 43 | public: 44 | Adafruit_LSM6DSOX(); 45 | 46 | void enableI2CMasterPullups(bool enable_pullups); 47 | void disableSPIMasterPullups(bool disable_pullups); 48 | 49 | private: 50 | bool _init(int32_t sensor_id); 51 | }; 52 | 53 | #endif 54 | -------------------------------------------------------------------------------- /src/firmware/libraries/Adafruit_LSM6DS/assets/board.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CU-Robotics/buff-code/103f94d388423e3b665714ca481a4295f4198c7a/src/firmware/libraries/Adafruit_LSM6DS/assets/board.jpg -------------------------------------------------------------------------------- /src/firmware/libraries/Adafruit_LSM6DS/assets/board.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CU-Robotics/buff-code/103f94d388423e3b665714ca481a4295f4198c7a/src/firmware/libraries/Adafruit_LSM6DS/assets/board.png -------------------------------------------------------------------------------- /src/firmware/libraries/Adafruit_LSM6DS/examples/adafruit_lsm6ds_pedometer/adafruit_lsm6ds_pedometer.ino: -------------------------------------------------------------------------------- 1 | // Basic demo for accelerometer/gyro readings from Adafruit LSM6DS33 2 | 3 | #include 4 | #include 5 | 6 | Adafruit_LSM6DS33 lsm; // can use any LSM6DS/ISM330 family chip! 7 | //Adafruit_LSM6DS3TRC lsm; // uncomment to use LSM6DS3TR-C 8 | 9 | // For SPI mode, we need a CS pin 10 | #define LSM_CS 10 11 | // For software-SPI mode we need SCK/MOSI/MISO pins 12 | #define LSM_SCK 13 13 | #define LSM_MISO 12 14 | #define LSM_MOSI 11 15 | 16 | 17 | void setup(void) { 18 | Serial.begin(115200); 19 | while (!Serial) 20 | delay(10); // will pause Zero, Leonardo, etc until serial console opens 21 | 22 | Serial.println("Adafruit LSM6DS pedometer test!"); 23 | 24 | if (!lsm.begin_I2C()) { 25 | // if (!lsm.begin_SPI(LSM_CS)) { 26 | // if (!lsm.begin_SPI(LSM_CS, LSM_SCK, LSM_MISO, LSM_MOSI)) { 27 | Serial.println("Failed to find LSM6DS chip"); 28 | while (1) { 29 | delay(10); 30 | } 31 | } 32 | 33 | Serial.println("LSM6DS Found!"); 34 | 35 | // Set to 2G range and 26 Hz update rate 36 | lsm.setAccelRange(LSM6DS_ACCEL_RANGE_2_G); 37 | lsm.setGyroRange(LSM6DS_GYRO_RANGE_250_DPS); 38 | lsm.setAccelDataRate(LSM6DS_RATE_26_HZ); 39 | lsm.setGyroDataRate(LSM6DS_RATE_26_HZ); 40 | 41 | // step detect output on INT1 42 | lsm.configInt1(false, false, false, true); 43 | 44 | // turn it on! 45 | Serial.println("Enable ped"); 46 | lsm.enablePedometer(true); 47 | } 48 | 49 | void loop() { 50 | Serial.print("Steps taken: "); Serial.println(lsm.readPedometer()); 51 | delay(100); // can wait as long as you like! 52 | } -------------------------------------------------------------------------------- /src/firmware/libraries/Adafruit_LSM6DS/examples/adafruit_lsm6ds_shake/adafruit_lsm6ds_shake.ino: -------------------------------------------------------------------------------- 1 | // Basic demo for accelerometer/gyro readings from Adafruit LSM6DS33 2 | 3 | #include 4 | #include 5 | 6 | // For SPI mode, we need a CS pin 7 | #define LSM_CS 10 8 | // For software-SPI mode we need SCK/MOSI/MISO pins 9 | #define LSM_SCK 13 10 | #define LSM_MISO 12 11 | #define LSM_MOSI 11 12 | 13 | 14 | Adafruit_LSM6DS33 lsm6ds; // uncomment to use LSM6DS33 15 | //Adafruit_LSM6DS3TRC lsm6ds; // uncomment to use LSM6DS3TR-C 16 | 17 | void setup(void) { 18 | Serial.begin(115200); 19 | while (!Serial) 20 | delay(10); // will pause Zero, Leonardo, etc until serial console opens 21 | 22 | Serial.println("Adafruit LSM6DS shake test!"); 23 | 24 | if (!lsm6ds.begin_I2C()) { 25 | // if (!lsm6ds.begin_SPI(LSM_CS)) { 26 | // if (!lsm6ds.begin_SPI(LSM_CS, LSM_SCK, LSM_MISO, LSM_MOSI)) { 27 | Serial.println("Failed to find LSM6DS chip"); 28 | while (1) { 29 | delay(10); 30 | } 31 | } 32 | 33 | Serial.println("LSM6DS Found!"); 34 | 35 | // enable shake detection 36 | lsm6ds.enableWakeup(true); 37 | } 38 | 39 | void loop() { 40 | // check for shake 41 | if (lsm6ds.shake()) { 42 | Serial.println("SHAKE!"); 43 | } 44 | } -------------------------------------------------------------------------------- /src/firmware/libraries/Adafruit_LSM6DS/license.txt: -------------------------------------------------------------------------------- 1 | Software License Agreement (BSD License) 2 | 3 | Copyright (c) 2019 Bryan Siepert for Adafruit Industries 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 1. Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | 2. Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in the 12 | documentation and/or other materials provided with the distribution. 13 | 3. Neither the name of the copyright holders nor the 14 | names of its contributors may be used to endorse or promote products 15 | derived from this software without specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY 18 | EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 21 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | -------------------------------------------------------------------------------- /src/firmware/libraries/Adafruit_Sensor/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CU-Robotics/buff-code/103f94d388423e3b665714ca481a4295f4198c7a/src/firmware/libraries/Adafruit_Sensor/.DS_Store -------------------------------------------------------------------------------- /src/firmware/libraries/Adafruit_Sensor/._.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CU-Robotics/buff-code/103f94d388423e3b665714ca481a4295f4198c7a/src/firmware/libraries/Adafruit_Sensor/._.DS_Store -------------------------------------------------------------------------------- /src/firmware/libraries/FlexCAN_T4/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CU-Robotics/buff-code/103f94d388423e3b665714ca481a4295f4198c7a/src/firmware/libraries/FlexCAN_T4/.DS_Store -------------------------------------------------------------------------------- /src/firmware/libraries/FlexCAN_T4/._.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CU-Robotics/buff-code/103f94d388423e3b665714ca481a4295f4198c7a/src/firmware/libraries/FlexCAN_T4/._.DS_Store -------------------------------------------------------------------------------- /src/firmware/libraries/FlexCAN_T4/FlexCAN_T4Beta1BoardArchive.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CU-Robotics/buff-code/103f94d388423e3b665714ca481a4295f4198c7a/src/firmware/libraries/FlexCAN_T4/FlexCAN_T4Beta1BoardArchive.zip -------------------------------------------------------------------------------- /src/firmware/libraries/FlexCAN_T4/LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 Antonio Brewer 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /src/firmware/libraries/FlexCAN_T4/examples/BiDirectionalForward/BiDirectionalForward.ino: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | FlexCAN_T4 can1; 4 | FlexCAN_T4 can2; 5 | CAN_message_t msg; 6 | 7 | void setup(void) { 8 | can1.begin(); 9 | can1.setBaudRate(250000); 10 | can2.begin(); 11 | can2.setBaudRate(250000); 12 | } 13 | 14 | void loop() { 15 | if ( can1.read(msg) ) { 16 | can2.write(msg); 17 | } 18 | else if ( can2.read(msg) ) { 19 | can1.write(msg); 20 | } 21 | } 22 | -------------------------------------------------------------------------------- /src/firmware/libraries/FlexCAN_T4/examples/CAN2.0_example_FIFO_with_interrupts.ino: -------------------------------------------------------------------------------- 1 | #include 2 | FlexCAN_T4 Can0; 3 | 4 | void setup(void) { 5 | Serial.begin(115200); delay(400); 6 | pinMode(6, OUTPUT); digitalWrite(6, LOW); /* optional tranceiver enable pin */ 7 | Can0.begin(); 8 | Can0.setBaudRate(1000000); 9 | Can0.setMaxMB(16); 10 | Can0.enableFIFO(); 11 | Can0.enableFIFOInterrupt(); 12 | Can0.onReceive(canSniff); 13 | Can0.mailboxStatus(); 14 | } 15 | 16 | void canSniff(const CAN_message_t &msg) { 17 | Serial.print("MB "); Serial.print(msg.mb); 18 | Serial.print(" OVERRUN: "); Serial.print(msg.flags.overrun); 19 | Serial.print(" LEN: "); Serial.print(msg.len); 20 | Serial.print(" EXT: "); Serial.print(msg.flags.extended); 21 | Serial.print(" TS: "); Serial.print(msg.timestamp); 22 | Serial.print(" ID: "); Serial.print(msg.id, HEX); 23 | Serial.print(" Buffer: "); 24 | for ( uint8_t i = 0; i < msg.len; i++ ) { 25 | Serial.print(msg.buf[i], HEX); Serial.print(" "); 26 | } Serial.println(); 27 | } 28 | 29 | void loop() { 30 | Can0.events(); 31 | 32 | static uint32_t timeout = millis(); 33 | if ( millis() - timeout > 200 ) { 34 | CAN_message_t msg; 35 | msg.id = random(0x1,0x7FE); 36 | for ( uint8_t i = 0; i < 8; i++ ) msg.buf[i] = i + 1; 37 | Can0.write(msg); 38 | timeout = millis(); 39 | } 40 | 41 | } 42 | -------------------------------------------------------------------------------- /src/firmware/libraries/FlexCAN_T4/examples/CAN2.0_example_FIFO_with_interrupts/CAN2.0_example_FIFO_with_interrupts.ino: -------------------------------------------------------------------------------- 1 | #include 2 | FlexCAN_T4 Can0; 3 | 4 | void setup(void) { 5 | Serial.begin(115200); delay(400); 6 | pinMode(6, OUTPUT); digitalWrite(6, LOW); /* optional tranceiver enable pin */ 7 | Can0.begin(); 8 | Can0.setBaudRate(1000000); 9 | Can0.setMaxMB(16); 10 | Can0.enableFIFO(); 11 | Can0.enableFIFOInterrupt(); 12 | Can0.onReceive(canSniff); 13 | Can0.mailboxStatus(); 14 | } 15 | 16 | void canSniff(const CAN_message_t &msg) { 17 | Serial.print("MB "); Serial.print(msg.mb); 18 | Serial.print(" OVERRUN: "); Serial.print(msg.flags.overrun); 19 | Serial.print(" LEN: "); Serial.print(msg.len); 20 | Serial.print(" EXT: "); Serial.print(msg.flags.extended); 21 | Serial.print(" TS: "); Serial.print(msg.timestamp); 22 | Serial.print(" ID: "); Serial.print(msg.id, HEX); 23 | Serial.print(" Buffer: "); 24 | for ( uint8_t i = 0; i < msg.len; i++ ) { 25 | Serial.print(msg.buf[i], HEX); Serial.print(" "); 26 | } Serial.println(); 27 | } 28 | 29 | void loop() { 30 | Can0.events(); 31 | 32 | static uint32_t timeout = millis(); 33 | if ( millis() - timeout > 200 ) { 34 | CAN_message_t msg; 35 | msg.id = random(0x1,0x7FE); 36 | for ( uint8_t i = 0; i < 8; i++ ) msg.buf[i] = i + 1; 37 | Can0.write(msg); 38 | timeout = millis(); 39 | } 40 | 41 | } 42 | -------------------------------------------------------------------------------- /src/firmware/libraries/FlexCAN_T4/examples/beta_sample/beta_sample.ino: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | FlexCAN_T4 can1; 4 | FlexCAN_T4 can2; 5 | CAN_message_t msg; 6 | 7 | void setup(void) { 8 | can1.begin(); 9 | can1.setBaudRate(250000); 10 | can2.begin(); 11 | can2.setBaudRate(250000); 12 | } 13 | 14 | void loop() { 15 | 16 | 17 | if ( can1.read(msg) ) { 18 | Serial.print("CAN1 "); 19 | Serial.print("MB: "); Serial.print(msg.mb); 20 | Serial.print(" ID: 0x"); Serial.print(msg.id, HEX ); 21 | Serial.print(" EXT: "); Serial.print(msg.flags.extended ); 22 | Serial.print(" LEN: "); Serial.print(msg.len); 23 | Serial.print(" DATA: "); 24 | for ( uint8_t i = 0; i < 8; i++ ) { 25 | Serial.print(msg.buf[i]); Serial.print(" "); 26 | } 27 | Serial.print(" TS: "); Serial.println(msg.timestamp); 28 | } 29 | else if ( can2.read(msg) ) { 30 | Serial.print("CAN2 "); 31 | Serial.print("MB: "); Serial.print(msg.mb); 32 | Serial.print(" ID: 0x"); Serial.print(msg.id, HEX ); 33 | Serial.print(" EXT: "); Serial.print(msg.flags.extended ); 34 | Serial.print(" LEN: "); Serial.print(msg.len); 35 | Serial.print(" DATA: "); 36 | for ( uint8_t i = 0; i < 8; i++ ) { 37 | Serial.print(msg.buf[i]); Serial.print(" "); 38 | } 39 | Serial.print(" TS: "); Serial.println(msg.timestamp); 40 | } 41 | } 42 | -------------------------------------------------------------------------------- /src/firmware/libraries/FlexCAN_T4/examples/isotp_example_send_receive/isotp_example_send_receive.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | isotp tp; /* 16 slots for multi-ID support, at 512bytes buffer each payload rebuild */ 4 | 5 | FlexCAN_T4 Can1; 6 | 7 | void myCallback(const ISOTP_data &config, const uint8_t *buf) { 8 | Serial.print("ID: "); 9 | Serial.print(config.id, HEX); 10 | Serial.print("\tLEN: "); 11 | Serial.print(config.len); 12 | Serial.print("\tFINAL ARRAY: "); 13 | for ( int i = 0; i < config.len; i++ ) { 14 | Serial.print(buf[i], HEX); 15 | Serial.print(" "); 16 | } Serial.println(); 17 | } 18 | 19 | void canSniff(const CAN_message_t &msg) { 20 | Serial.print("MB "); Serial.print(msg.mb); 21 | Serial.print(" OVERRUN: "); Serial.print(msg.flags.overrun); 22 | Serial.print(" LEN: "); Serial.print(msg.len); 23 | Serial.print(" EXT: "); Serial.print(msg.flags.extended); 24 | Serial.print(" TS: "); Serial.print(msg.timestamp); 25 | Serial.print(" ID: "); Serial.print(msg.id, HEX); 26 | Serial.print(" BUS: "); Serial.print(msg.bus); 27 | Serial.print(" Buffer: "); 28 | for ( uint8_t i = 0; i < msg.len; i++ ) { 29 | Serial.print(msg.buf[i], HEX); Serial.print(" "); 30 | } Serial.println(); 31 | } 32 | 33 | void setup() { 34 | Serial.begin(115200); delay(400); 35 | Can1.begin(); 36 | Can1.setClock(CLK_60MHz); 37 | Can1.setBaudRate(95238); 38 | Can1.setMaxMB(16); 39 | Can1.enableFIFO(); 40 | Can1.enableFIFOInterrupt(); 41 | // Can1.onReceive(canSniff); 42 | tp.begin(); 43 | tp.setWriteBus(&Can1); /* we write to this bus */ 44 | tp.onReceive(myCallback); /* set callback */ 45 | } 46 | 47 | void loop() { 48 | static uint32_t sendTimer = millis(); 49 | if ( millis() - sendTimer > 1000 ) { 50 | uint8_t buf[] = { 1, 2, 3, 4, 5, 6, 7, 8, 9, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 0, 1, 2, 3, 4, 5 }; 51 | const char b[] = "01413AAAAABCDEFGHIJKLMNOPQRSTUVWXYZ0123456789ABCDEFGHIJKLMNOPQRSTUVWXYZ0123456789ABCDEFGHIJKLMNOPQRSTUVWXYZ0123456789ABCDEFGHIJKLMNOPQRSTUVWXYZ0123456789"; 52 | ISOTP_data config; 53 | config.id = 0x666; 54 | config.flags.extended = 0; /* standard frame */ 55 | config.separation_time = 10; /* time between back-to-back frames in millisec */ 56 | tp.write(config, buf, sizeof(buf)); 57 | tp.write(config, b, sizeof(b)); 58 | sendTimer = millis(); 59 | } 60 | } 61 | -------------------------------------------------------------------------------- /src/firmware/libraries/FlexCAN_T4/examples/isotp_server_example/isotp_server_example.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | const uint32_t canid = 0x666; 4 | const uint32_t request = 0x020902; 5 | uint8_t myData[] = { 0x49, 0x2, 0x1, 1, 2, 3, 4, 5, 6, 7, 8, 9, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 0, 1, 2, 3, 1, 4, 5, 6, 7, 8, 9, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 0, 3, 3, 2, 4, 4, 0xF, 0xF, 0xF, 0xF, 0xF, 0xF, 0xF, 0xF, 0xF, 0xF, 0xF, 0xF, 0xF, 0xF, 0xF, 0xF, 0xF, 0xF, 0xF, 0xF, 0xF, 0xF, 0xF, 0xF, 0xF, 0xF, 0xF, 0xF, 0xF, 0xF, 0xF, 0xF, 0xF, 0xF, 0xF, 0xF, 0xF, 0xF, 0xF, 0xF, 0xF, 0xF, 0xF, 0xF, 0xF, 0xF, 0xF, 0xF, 0xF, 0xF, 0xF, 0xF, 0xF, 0xF, 0xF, 0xF, 0xF, 0xF, 0xF, 0xF, 0xF, 0x5 }; 6 | isotp_server myResource; 7 | 8 | FlexCAN_T4 Can1; 9 | 10 | void canSniff(const CAN_message_t &msg) { 11 | Serial.print("MB "); Serial.print(msg.mb); 12 | Serial.print(" OVERRUN: "); Serial.print(msg.flags.overrun); 13 | Serial.print(" LEN: "); Serial.print(msg.len); 14 | Serial.print(" EXT: "); Serial.print(msg.flags.extended); 15 | Serial.print(" TS: "); Serial.print(msg.timestamp); 16 | Serial.print(" ID: "); Serial.print(msg.id, HEX); 17 | Serial.print(" BUS: "); Serial.print(msg.bus); 18 | Serial.print(" Buffer: "); 19 | for ( uint8_t i = 0; i < msg.len; i++ ) { 20 | Serial.print(msg.buf[i], HEX); Serial.print(" "); 21 | } Serial.println(); 22 | } 23 | 24 | void setup() { 25 | Serial.begin(115200); delay(400); 26 | Can1.begin(); 27 | Can1.setClock(CLK_60MHz); 28 | Can1.setBaudRate(95238); 29 | Can1.setMaxMB(16); 30 | Can1.enableFIFO(); 31 | Can1.enableFIFOInterrupt(); 32 | Can1.onReceive(canSniff); 33 | myResource.begin(); 34 | myResource.setWriteBus(&Can1); /* we write to this bus */ 35 | } 36 | 37 | void loop() { 38 | } 39 | -------------------------------------------------------------------------------- /src/firmware/libraries/FlexCAN_T4/examples/mailbox_filtering_example_with_interrupts/mailbox_filtering_example_with_interrupts.ino: -------------------------------------------------------------------------------- 1 | #include 2 | FlexCAN_T4 Can0; 3 | 4 | #define NUM_TX_MAILBOXES 2 5 | #define NUM_RX_MAILBOXES 6 6 | void setup(void) { 7 | Serial.begin(115200); delay(400); 8 | Can0.begin(); 9 | Can0.setBaudRate(250000); 10 | Can0.setMaxMB(NUM_TX_MAILBOXES + NUM_RX_MAILBOXES); 11 | for (int i = 0; i 2 | 3 | FlexCAN_T4 can1; 4 | FlexCAN_T4 can2; 5 | CAN_message_t msg; 6 | 7 | void setup(void) { 8 | can1.begin(); 9 | can1.setBaudRate(250000); 10 | can2.begin(); 11 | can2.setBaudRate(250000); 12 | } 13 | 14 | void loop() { 15 | 16 | 17 | if ( can1.read(msg) ) { 18 | Serial.print("CAN1 "); 19 | Serial.print("MB: "); Serial.print(msg.mb); 20 | Serial.print(" ID: 0x"); Serial.print(msg.id, HEX ); 21 | Serial.print(" EXT: "); Serial.print(msg.flags.extended ); 22 | Serial.print(" LEN: "); Serial.print(msg.len); 23 | Serial.print(" DATA: "); 24 | for ( uint8_t i = 0; i < 8; i++ ) { 25 | Serial.print(msg.buf[i]); Serial.print(" "); 26 | } 27 | Serial.print(" TS: "); Serial.println(msg.timestamp); 28 | } 29 | else if ( can2.read(msg) ) { 30 | Serial.print("CAN2 "); 31 | Serial.print("MB: "); Serial.print(msg.mb); 32 | Serial.print(" ID: 0x"); Serial.print(msg.id, HEX ); 33 | Serial.print(" EXT: "); Serial.print(msg.flags.extended ); 34 | Serial.print(" LEN: "); Serial.print(msg.len); 35 | Serial.print(" DATA: "); 36 | for ( uint8_t i = 0; i < 8; i++ ) { 37 | Serial.print(msg.buf[i]); Serial.print(" "); 38 | } 39 | Serial.print(" TS: "); Serial.println(msg.timestamp); 40 | } 41 | } 42 | -------------------------------------------------------------------------------- /src/firmware/libraries/FreqMeasureMulti/FreqMeasureMulti.h: -------------------------------------------------------------------------------- 1 | #ifndef FreqMeasureMulti_h 2 | #define FreqMeasureMulti_h 3 | 4 | #include 5 | 6 | #define FREQMEASUREMULTI_BUFFER_LEN 24 7 | // capture modes 8 | #define FREQMEASUREMULTI_RAISING 1 9 | #define FREQMEASUREMULTI_FALLING 2 10 | #define FREQMEASUREMULTI_INTERLEAVE 3 11 | #define FREQMEASUREMULTI_SPACE_ONLY 5 12 | #define FREQMEASUREMULTI_MARK_ONLY 6 13 | #define FREQMEASUREMULTI_ALTERNATE 7 14 | // result constants for fmultiRecord.level 15 | #define LEVEL_SPACE_ONLY 0 16 | #define LEVEL_SPACE_MARK 1 17 | #define LEVEL_MARK_SPACE 2 18 | #define LEVEL_MARK_ONLY 3 19 | #define LEVEL_UNDEFINED 255 20 | 21 | typedef struct { 22 | uint8_t level; 23 | uint32_t count; 24 | } fmultiRecord; 25 | 26 | #if defined(__IMXRT1062__) 27 | #include "FreqMeasureMultiIMXRT.h" 28 | #else 29 | 30 | // Teensy 3.x and LC 31 | class FreqMeasureMulti 32 | { 33 | public: 34 | bool begin(uint32_t pin); 35 | bool begin(uint32_t pin, uint8_t mode); 36 | uint32_t available(void); 37 | uint32_t read(void); 38 | uint8_t readLevel(void); 39 | static float countToFrequency(uint32_t count); 40 | static float countToNanoseconds(uint32_t count); 41 | void end(void); 42 | private: 43 | fmultiRecord buffer_value[FREQMEASUREMULTI_BUFFER_LEN]; 44 | uint8_t buffer_head; 45 | uint8_t buffer_tail; 46 | 47 | friend void ftm0_isr(void); 48 | void isr(bool inc); 49 | uint8_t channel; 50 | uint8_t last_read_level; 51 | uint32_t raiscap_previous; 52 | uint32_t fallcap_previous; 53 | bool act_on_fall, act_on_raise, read_diff; 54 | bool next_is_falling; 55 | }; 56 | #endif //defined(__IMXRT1062__) 57 | 58 | #endif 59 | 60 | -------------------------------------------------------------------------------- /src/firmware/libraries/FreqMeasureMulti/FreqMeasureMultiIMXRT.h: -------------------------------------------------------------------------------- 1 | #ifndef FreqMeasureMultiIMXRT_h 2 | #define FreqMeasureMultiIMXRT_h 3 | 4 | #if defined(__IMXRT1062__) 5 | 6 | struct freq_pwm_pin_info_struct { 7 | IMXRT_FLEXPWM_t *pflexpwm; 8 | uint8_t module; // 0-3, 0-3 9 | uint8_t channel; // 0=X, 1=A, 2=B 10 | uint8_t muxval; // 11 | uint8_t irq; // which IRQ to use 12 | void (*isr)(); // interrupt handler. 13 | volatile uint32_t *select_input_register; // Which register controls the selection 14 | const uint32_t select_val; // Value for that selection 15 | }; 16 | 17 | 18 | 19 | class FreqMeasureMulti 20 | { 21 | public: 22 | bool begin(uint32_t pin); 23 | bool begin(uint32_t pin, uint8_t mode); 24 | uint32_t available(void); 25 | uint32_t read(void); 26 | uint8_t readLevel(void); 27 | static float countToFrequency(uint32_t count); 28 | static float countToNanoseconds(uint32_t count); 29 | void end(void); 30 | private: 31 | static const struct freq_pwm_pin_info_struct freq_pwm_pin_info[]; 32 | static FreqMeasureMulti *list[16]; 33 | fmultiRecord buffer_value[FREQMEASUREMULTI_BUFFER_LEN]; 34 | uint8_t buffer_head; 35 | uint8_t buffer_tail; 36 | 37 | IMXRT_FLEXPWM_t *_pflexpwm; 38 | void processChannelISR(uint8_t channel, uint32_t capture, uint8_t edge); 39 | void isr(); 40 | uint8_t last_read_level; 41 | 42 | uint8_t _pin; // remember the pin number; 43 | uint8_t _mode; // remember the mode we are using. 44 | uint8_t _channel; 45 | FreqMeasureMulti *_next = nullptr; // in case we have multiple ones who are on the same module. 46 | uint32_t _capture_previous[2]; // Not sure yet 47 | uint16_t capture_msw = 0; 48 | 49 | static void flexpwm1_0_isr(); 50 | static void flexpwm1_1_isr(); 51 | static void flexpwm1_2_isr(); 52 | static void flexpwm1_3_isr(); 53 | static void flexpwm2_0_isr(); 54 | static void flexpwm2_1_isr(); 55 | static void flexpwm2_2_isr(); 56 | static void flexpwm2_3_isr(); 57 | static void flexpwm3_0_isr(); 58 | static void flexpwm3_1_isr(); 59 | static void flexpwm3_3_isr(); 60 | static void flexpwm4_0_isr(); 61 | static void flexpwm4_1_isr(); 62 | static void flexpwm4_2_isr(); 63 | }; 64 | 65 | #endif // IMXRT 66 | #endif 67 | 68 | -------------------------------------------------------------------------------- /src/firmware/libraries/FreqMeasureMulti/docs/issue_template.md: -------------------------------------------------------------------------------- 1 | Please use this form only to report code defects or bugs. 2 | 3 | For any question, even questions directly pertaining to this code, post your question on the forum. 4 | 5 | Teensy: forum.pjrc.com 6 | 7 | If you are experiencing trouble but not certain of the cause, or need help using this code, ask on the forum. This is not the place to ask for support or help, even directly related to this code. Only use this form you are certain you have discovered a defect in this code! 8 | 9 | Please verify the problem occurs when using the very latest version, using the newest version of Arduino and any other related software. 10 | 11 | 12 | ----------------------------- Remove above ----------------------------- 13 | 14 | 15 | 16 | ### Description 17 | 18 | Describe your problem. 19 | 20 | 21 | 22 | ### Steps To Reproduce Problem 23 | 24 | Please give detailed instructions needed for anyone to attempt to reproduce the problem. 25 | 26 | 27 | 28 | ### Hardware & Software 29 | 30 | Board 31 | Shields / modules used 32 | Arduino IDE version 33 | Teensyduino version 34 | Operating system & version 35 | Any other software or hardware? 36 | 37 | 38 | ### Arduino Sketch 39 | 40 | ```cpp 41 | // Change the code below by your sketch (please try to give the smallest code which demonstrates the problem) 42 | #include 43 | 44 | // libraries: give links/details so anyone can compile your code for the same result 45 | 46 | void setup() { 47 | } 48 | 49 | void loop() { 50 | } 51 | ``` 52 | 53 | 54 | ### Errors or Incorrect Output 55 | 56 | If you see any errors or incorrect output, please show it here. Please use copy & paste to give an exact copy of the message. Details matter, so please show (not merely describe) the actual message or error exactly as it appears. 57 | 58 | 59 | -------------------------------------------------------------------------------- /src/firmware/libraries/FreqMeasureMulti/examples/Serial_Output/Serial_Output.ino: -------------------------------------------------------------------------------- 1 | /* FreqMeasureMulti - Example with serial output 2 | * http://www.pjrc.com/teensy/td_libs_FreqMeasure.html 3 | * 4 | * This example code is in the public domain. 5 | */ 6 | #include 7 | 8 | // Measure 3 frequencies at the same time! :-) 9 | FreqMeasureMulti freq1; 10 | FreqMeasureMulti freq2; 11 | FreqMeasureMulti freq3; 12 | 13 | void setup() { 14 | Serial.begin(57600); 15 | while (!Serial) ; // wait for Arduino Serial Monitor 16 | delay(10); 17 | Serial.println("FreqMeasureMulti Begin"); 18 | delay(10); 19 | freq1.begin(6); 20 | freq2.begin(9); 21 | freq3.begin(10); 22 | } 23 | 24 | float sum1=0, sum2=0, sum3=0; 25 | int count1=0, count2=0, count3=0; 26 | elapsedMillis timeout; 27 | 28 | void loop() { 29 | if (freq1.available()) { 30 | sum1 = sum1 + freq1.read(); 31 | count1 = count1 + 1; 32 | } 33 | if (freq2.available()) { 34 | sum2 = sum2 + freq2.read(); 35 | count2 = count2 + 1; 36 | } 37 | if (freq3.available()) { 38 | sum3 = sum3 + freq3.read(); 39 | count3 = count3 + 1; 40 | } 41 | // print results every half second 42 | if (timeout > 500) { 43 | if (count1 > 0) { 44 | Serial.print(freq1.countToFrequency(sum1 / count1)); 45 | } else { 46 | Serial.print("(no pulses)"); 47 | } 48 | Serial.print(", "); 49 | if (count2 > 0) { 50 | Serial.print(freq2.countToFrequency(sum2 / count2)); 51 | } else { 52 | Serial.print("(no pulses)"); 53 | } 54 | Serial.print(", "); 55 | if (count3 > 0) { 56 | Serial.print(freq3.countToFrequency(sum3 / count3)); 57 | } else { 58 | Serial.print("(no pulses)"); 59 | } 60 | Serial.println(); 61 | sum1 = 0; 62 | sum2 = 0; 63 | sum3 = 0; 64 | count1 = 0; 65 | count2 = 0; 66 | count3 = 0; 67 | timeout = 0; 68 | } 69 | } 70 | 71 | -------------------------------------------------------------------------------- /src/firmware/libraries/FreqMeasureMulti/keywords.txt: -------------------------------------------------------------------------------- 1 | FreqMeasureMulti KEYWORD1 2 | begin KEYWORD2 3 | available KEYWORD2 4 | read KEYWORD2 5 | readLevel KEYWORD2 6 | countToFrequency KEYWORD2 7 | countToNanoseconds KEYWORD2 8 | end KEYWORD2 9 | 10 | FREQMEASUREMULTI_RAISING LITERAL1 11 | FREQMEASUREMULTI_FALLING LITERAL1 12 | FREQMEASUREMULTI_INTERLEAVE LITERAL1 13 | FREQMEASUREMULTI_SPACE_ONLY LITERAL1 14 | FREQMEASUREMULTI_MARK_ONLY LITERAL1 15 | FREQMEASUREMULTI_ALTERNATE LITERAL1 16 | LEVEL_SPACE_ONLY LITERAL1 17 | LEVEL_SPACE_MARK LITERAL1 18 | LEVEL_MARK_SPACE LITERAL1 19 | LEVEL_MARK_ONLY LITERAL1 20 | LEVEL_UNDEFINED LITERAL1 21 | -------------------------------------------------------------------------------- /src/firmware/libraries/SPI/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CU-Robotics/buff-code/103f94d388423e3b665714ca481a4295f4198c7a/src/firmware/libraries/SPI/.DS_Store -------------------------------------------------------------------------------- /src/firmware/libraries/SPI/._.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CU-Robotics/buff-code/103f94d388423e3b665714ca481a4295f4198c7a/src/firmware/libraries/SPI/._.DS_Store -------------------------------------------------------------------------------- /src/firmware/libraries/SPI/examples/DigitalPotControl/DigitalPotControl.ino: -------------------------------------------------------------------------------- 1 | /* 2 | Digital Pot Control 3 | 4 | This example controls an Analog Devices AD5206 digital potentiometer. 5 | The AD5206 has 6 potentiometer channels. Each channel's pins are labeled 6 | A - connect this to voltage 7 | W - this is the pot's wiper, which changes when you set it 8 | B - connect this to ground. 9 | 10 | The AD5206 is SPI-compatible,and to command it, you send two bytes, 11 | one with the channel number (0 - 5) and one with the resistance value for the 12 | channel (0 - 255). 13 | 14 | The circuit: 15 | * All A pins of AD5206 connected to +5V 16 | * All B pins of AD5206 connected to ground 17 | * An LED and a 220-ohm resisor in series connected from each W pin to ground 18 | * CS - to digital pin 10 (SS pin) 19 | * SDI - to digital pin 11 (MOSI pin) 20 | * CLK - to digital pin 13 (SCK pin) 21 | 22 | created 10 Aug 2010 23 | by Tom Igoe 24 | 25 | Thanks to Heather Dewey-Hagborg for the original tutorial, 2005 26 | 27 | */ 28 | 29 | 30 | // include the SPI library: 31 | #include 32 | 33 | 34 | // set pin 10 as the slave select for the digital pot: 35 | const int slaveSelectPin = 10; 36 | 37 | void setup() { 38 | // set the slaveSelectPin as an output: 39 | pinMode (slaveSelectPin, OUTPUT); 40 | digitalWrite (slaveSelectPin, HIGH); 41 | // initialize SPI: 42 | SPI.begin(); 43 | } 44 | 45 | void loop() { 46 | // go through the six channels of the digital pot: 47 | for (int channel = 0; channel < 6; channel++) { 48 | // change the resistance on this channel from min to max: 49 | for (int level = 0; level < 255; level++) { 50 | digitalPotWrite(channel, level); 51 | delay(10); 52 | } 53 | // wait a second at the top: 54 | delay(100); 55 | // change the resistance on this channel from max to min: 56 | for (int level = 0; level < 255; level++) { 57 | digitalPotWrite(channel, 255 - level); 58 | delay(10); 59 | } 60 | } 61 | 62 | } 63 | 64 | void digitalPotWrite(int address, int value) { 65 | // gain control of the SPI port 66 | // and configure settings 67 | SPI.beginTransaction(SPISettings(4000000, MSBFIRST, SPI_MODE0)); 68 | // take the SS pin low to select the chip: 69 | digitalWrite(slaveSelectPin,LOW); 70 | // send in the address and value via SPI: 71 | SPI.transfer(address); 72 | SPI.transfer(value); 73 | // take the SS pin high to de-select the chip: 74 | digitalWrite(slaveSelectPin,HIGH); 75 | // release control of the SPI port 76 | SPI.endTransaction(); 77 | } 78 | -------------------------------------------------------------------------------- /src/firmware/libraries/Wire/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CU-Robotics/buff-code/103f94d388423e3b665714ca481a4295f4198c7a/src/firmware/libraries/Wire/.DS_Store -------------------------------------------------------------------------------- /src/firmware/libraries/Wire/._.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CU-Robotics/buff-code/103f94d388423e3b665714ca481a4295f4198c7a/src/firmware/libraries/Wire/._.DS_Store -------------------------------------------------------------------------------- /src/firmware/libraries/Wire/examples/digital_potentiometer/digital_potentiometer.ino: -------------------------------------------------------------------------------- 1 | // I2C Digital Potentiometer 2 | // by Nicholas Zambetti 3 | // and Shawn Bonkowski 4 | 5 | // Demonstrates use of the Wire library 6 | // Controls AD5171 digital potentiometer via I2C/TWI 7 | 8 | // Created 31 March 2006 9 | 10 | // This example code is in the public domain. 11 | 12 | // This example code is in the public domain. 13 | 14 | 15 | #include 16 | 17 | void setup() 18 | { 19 | Wire.begin(); // join i2c bus (address optional for master) 20 | } 21 | 22 | byte val = 0; 23 | 24 | void loop() 25 | { 26 | Wire.beginTransmission(44); // transmit to device #44 (0x2c) 27 | // device address is specified in datasheet 28 | Wire.write(byte(0x00)); // sends instruction byte 29 | Wire.write(val); // sends potentiometer value byte 30 | Wire.endTransmission(); // stop transmitting 31 | 32 | val++; // increment value 33 | if(val == 64) // if reached 64th position (max) 34 | { 35 | val = 0; // start over from lowest value 36 | } 37 | delay(500); 38 | } 39 | 40 | -------------------------------------------------------------------------------- /src/firmware/libraries/Wire/examples/master_reader/master_reader.ino: -------------------------------------------------------------------------------- 1 | // Wire Master Reader 2 | // by Nicholas Zambetti 3 | 4 | // Demonstrates use of the Wire library 5 | // Reads data from an I2C/TWI slave device 6 | // Refer to the "Wire Slave Sender" example for use with this 7 | 8 | // Created 29 March 2006 9 | 10 | // This example code is in the public domain. 11 | 12 | 13 | #include 14 | 15 | int led = LED_BUILTIN; 16 | 17 | void setup() 18 | { 19 | pinMode(led, OUTPUT); 20 | Wire.begin(); // join i2c bus (address optional for master) 21 | Serial.begin(9600); // start serial for output 22 | } 23 | 24 | void loop() 25 | { 26 | Serial.print("read: "); 27 | 28 | digitalWrite(led, HIGH); // briefly flash the LED 29 | Wire.requestFrom(8, 6); // request 6 bytes from slave device #8 30 | 31 | while(Wire.available()) { // slave may send less than requested 32 | char c = Wire.read(); // receive a byte as character 33 | Serial.print(c); // print the character 34 | } 35 | 36 | Serial.println(); 37 | digitalWrite(led, LOW); 38 | delay(500); 39 | } 40 | -------------------------------------------------------------------------------- /src/firmware/libraries/Wire/examples/master_writer/master_writer.ino: -------------------------------------------------------------------------------- 1 | // Wire Master Writer 2 | // by Nicholas Zambetti 3 | 4 | // Demonstrates use of the Wire library 5 | // Writes data to an I2C/TWI slave device 6 | // Refer to the "Wire Slave Receiver" example for use with this 7 | 8 | // Created 29 March 2006 9 | 10 | // This example code is in the public domain. 11 | 12 | 13 | #include 14 | 15 | int led = LED_BUILTIN; 16 | 17 | void setup() 18 | { 19 | pinMode(led, OUTPUT); 20 | Wire.begin(); // join i2c bus (address optional for master) 21 | } 22 | 23 | byte x = 0; 24 | 25 | void loop() 26 | { 27 | digitalWrite(led, HIGH); // briefly flash the LED 28 | Wire.beginTransmission(9); // transmit to device #9 29 | Wire.write("x is "); // sends five bytes 30 | Wire.write(x); // sends one byte 31 | Wire.endTransmission(); // stop transmitting 32 | digitalWrite(led, LOW); 33 | 34 | x++; 35 | delay(500); 36 | } 37 | -------------------------------------------------------------------------------- /src/firmware/libraries/Wire/examples/slave_receiver/slave_receiver.ino: -------------------------------------------------------------------------------- 1 | // Wire Slave Receiver 2 | // by Nicholas Zambetti 3 | 4 | // Demonstrates use of the Wire library 5 | // Receives data as an I2C/TWI slave device 6 | // Refer to the "Wire Master Writer" example for use with this 7 | 8 | // Created 29 March 2006 9 | 10 | // This example code is in the public domain. 11 | 12 | 13 | #include 14 | 15 | int led = LED_BUILTIN; 16 | 17 | void setup() 18 | { 19 | pinMode(led, OUTPUT); 20 | Wire.begin(9); // join i2c bus with address #9 21 | Wire.onReceive(receiveEvent); // register event 22 | Serial.begin(9600); // start serial for output 23 | } 24 | 25 | void loop() 26 | { 27 | delay(100); 28 | } 29 | 30 | // function that executes whenever data is received from master 31 | // this function is registered as an event, see setup() 32 | void receiveEvent(int howMany) 33 | { 34 | digitalWrite(led, HIGH); // briefly flash the LED 35 | while(Wire.available() > 1) { // loop through all but the last 36 | char c = Wire.read(); // receive byte as a character 37 | Serial.print(c); // print the character 38 | } 39 | int x = Wire.read(); // receive byte as an integer 40 | Serial.println(x); // print the integer 41 | digitalWrite(led, LOW); 42 | } 43 | -------------------------------------------------------------------------------- /src/firmware/libraries/Wire/examples/slave_sender/slave_sender.ino: -------------------------------------------------------------------------------- 1 | // Wire Slave Sender 2 | // by Nicholas Zambetti 3 | 4 | // Demonstrates use of the Wire library 5 | // Sends data as an I2C/TWI slave device 6 | // Refer to the "Wire Master Reader" example for use with this 7 | 8 | // Created 29 March 2006 9 | 10 | // This example code is in the public domain. 11 | 12 | 13 | #include 14 | 15 | int led = LED_BUILTIN; 16 | 17 | void setup() 18 | { 19 | pinMode(led, OUTPUT); 20 | Wire.begin(8); // join i2c bus with address #8 21 | Wire.onRequest(requestEvent); // register event 22 | } 23 | 24 | void loop() 25 | { 26 | delay(100); 27 | } 28 | 29 | // function that executes whenever data is requested by master 30 | // this function is registered as an event, see setup() 31 | void requestEvent() 32 | { 33 | digitalWrite(led, HIGH); // briefly flash the LED 34 | Wire.write("hello "); // respond with message of 6 bytes 35 | // as expected by master 36 | digitalWrite(led, LOW); 37 | } 38 | -------------------------------------------------------------------------------- /src/firmware/libraries/Wire/keywords.txt: -------------------------------------------------------------------------------- 1 | ####################################### 2 | # Syntax Coloring Map For Wire 3 | ####################################### 4 | 5 | ####################################### 6 | # Datatypes (KEYWORD1) 7 | ####################################### 8 | 9 | ####################################### 10 | # Methods and Functions (KEYWORD2) 11 | ####################################### 12 | 13 | begin KEYWORD2 14 | beginTransmission KEYWORD2 15 | endTransmission KEYWORD2 16 | requestFrom KEYWORD2 17 | send KEYWORD2 18 | receive KEYWORD2 19 | onReceive KEYWORD2 20 | onRequest KEYWORD2 21 | setClock KEYWORD2 22 | setSDA KEYWORD2 23 | setSCL KEYWORD2 24 | 25 | ####################################### 26 | # Instances (KEYWORD2) 27 | ####################################### 28 | 29 | Wire KEYWORD2 30 | 31 | ####################################### 32 | # Constants (LITERAL1) 33 | ####################################### 34 | 35 | -------------------------------------------------------------------------------- /src/firmware/libraries/Wire/library.properties: -------------------------------------------------------------------------------- 1 | name=Wire 2 | version=1.0 3 | author=Arduino 4 | maintainer=Paul Stoffregen 5 | sentence=This library allows you to communicate with I2C and Two Wire Interface devices. 6 | paragraph=It allows the communication with I2C devices like temperature sensors, realtime clocks and many others using SDA (Data Line) and SCL (Clock Line). 7 | category=Communication 8 | url=http://www.arduino.cc/en/Reference/Wire 9 | architectures=* 10 | 11 | -------------------------------------------------------------------------------- /src/firmware/libraries/Wire/utility/twi.h: -------------------------------------------------------------------------------- 1 | /* 2 | twi.h - TWI/I2C library for Wiring & Arduino 3 | Copyright (c) 2006 Nicholas Zambetti. All right reserved. 4 | 5 | This library is free software; you can redistribute it and/or 6 | modify it under the terms of the GNU Lesser General Public 7 | License as published by the Free Software Foundation; either 8 | version 2.1 of the License, or (at your option) any later version. 9 | 10 | This library is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 13 | Lesser General Public License for more details. 14 | 15 | You should have received a copy of the GNU Lesser General Public 16 | License along with this library; if not, write to the Free Software 17 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 18 | */ 19 | 20 | #ifndef twi_h 21 | #define twi_h 22 | 23 | #include 24 | 25 | //#define ATMEGA8 26 | 27 | #ifndef TWI_FREQ 28 | #define TWI_FREQ 100000L 29 | #endif 30 | 31 | #ifndef TWI_BUFFER_LENGTH 32 | #define TWI_BUFFER_LENGTH 32 33 | #endif 34 | 35 | #define TWI_READY 0 36 | #define TWI_MRX 1 37 | #define TWI_MTX 2 38 | #define TWI_SRX 3 39 | #define TWI_STX 4 40 | 41 | void twi_init(void); 42 | void twi_setAddress(uint8_t); 43 | uint8_t twi_readFrom(uint8_t, uint8_t*, uint8_t, uint8_t); 44 | uint8_t twi_writeTo(uint8_t, uint8_t*, uint8_t, uint8_t, uint8_t); 45 | uint8_t twi_transmit(const uint8_t*, uint8_t); 46 | void twi_attachSlaveRxEvent( void (*)(uint8_t*, int) ); 47 | void twi_attachSlaveTxEvent( void (*)(void) ); 48 | void twi_reply(uint8_t); 49 | void twi_stop(void); 50 | void twi_releaseBus(void); 51 | 52 | #endif 53 | 54 | -------------------------------------------------------------------------------- /src/firmware/src/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CU-Robotics/buff-code/103f94d388423e3b665714ca481a4295f4198c7a/src/firmware/src/.DS_Store -------------------------------------------------------------------------------- /src/firmware/src/algorithms/lowpass_filter.h: -------------------------------------------------------------------------------- 1 | 2 | 3 | #ifndef LP_FILTER_H 4 | #define LP_FILTER_H 5 | 6 | 7 | struct LPFilter { 8 | float K = 0.4; 9 | float output = 0; 10 | float prev_measurement = 0.0; 11 | 12 | float filter(float measurement, float dt) { 13 | output += K * (measurement - prev_measurement); 14 | return output; 15 | } 16 | }; 17 | 18 | #endif -------------------------------------------------------------------------------- /src/firmware/src/algorithms/pid_filter.h: -------------------------------------------------------------------------------- 1 | #ifndef PID_FILTER_H 2 | #define PID_FILTER_H 3 | 4 | #include "math.h" 5 | 6 | struct PIDFilter { 7 | float K[4] = {0.0, 0.0, 0.0, 0.0}; // P, I, D, F 8 | float sumError = 0.0; 9 | float prevError = 0.0; 10 | // float variableFeedForward = 1.0; 11 | 12 | float setpoint; 13 | float measurement; 14 | float output; 15 | 16 | float filter(int deltaTime) { 17 | float error = setpoint - measurement; 18 | sumError += error * deltaTime; 19 | output = (K[0] * error) 20 | // + (K[1] * sumError) 21 | + (K[2] * ((error - prevError) / deltaTime)); 22 | // + (K[3] * variableFeedForward); 23 | prevError = error; 24 | if (fabs(output) > 1.0) output /= fabs(output); 25 | return output; 26 | } 27 | 28 | // void applyVariableFeedForward(float var) { 29 | // float variableFeedForward = var; 30 | // } 31 | }; 32 | 33 | #endif -------------------------------------------------------------------------------- /src/firmware/src/algorithms/rolling_average_filter.h: -------------------------------------------------------------------------------- 1 | #ifndef ROLLING_AVERAGE_FILTER_H 2 | #define ROLLING_AVERAGE_FILTER_H 3 | 4 | #include 5 | 6 | struct RollingAverageFilter { 7 | float sum = 0.0; 8 | std::vector samples; 9 | int max_samples; 10 | int curr_sample_offset = 0; 11 | 12 | RollingAverageFilter(int max_samples) { 13 | this->max_samples = max_samples; 14 | samples.resize(max_samples); 15 | } 16 | 17 | void addMeasurement(float measurement) { 18 | sum += measurement; 19 | sum -= samples[curr_sample_offset]; 20 | samples[curr_sample_offset] = measurement; 21 | curr_sample_offset++; 22 | if (curr_sample_offset >= max_samples) curr_sample_offset = 0; 23 | } 24 | 25 | float getMean() { return sum / (float)(max_samples); }; 26 | }; 27 | 28 | #endif -------------------------------------------------------------------------------- /src/firmware/src/buff_cpp/blink.cpp: -------------------------------------------------------------------------------- 1 | #include "buff_cpp/blink.h" 2 | 3 | uint32_t blinker_timer_mark; 4 | bool blinker_status; 5 | 6 | void setup_blink() { 7 | // Hardware setup 8 | blinker_status = false; 9 | blinker_timer_mark = ARM_DWT_CYCCNT; 10 | 11 | pinMode(BLINK_PIN, OUTPUT); 12 | digitalWrite(BLINK_PIN, blinker_status); 13 | } 14 | 15 | void blink(){ 16 | if ((1E6/F_CPU)*(ARM_DWT_CYCCNT - blinker_timer_mark) > BLINK_RATE_US){ 17 | blinker_status = !blinker_status; 18 | blinker_timer_mark = ARM_DWT_CYCCNT; 19 | digitalWrite(BLINK_PIN, blinker_status); 20 | } 21 | } -------------------------------------------------------------------------------- /src/firmware/src/buff_cpp/blink.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #ifndef BUFF_BLINKER_H 4 | #define BUFF_BLINKER_H 5 | 6 | #define BLINK_RATE_US 250000 7 | #define BLINK_PIN LED_BUILTIN 8 | 9 | /* 10 | 11 | Use a global rate to blink Teensy's built in LED. 12 | Call setup once and then blink() at least as often 13 | as the rate. Works best when called at 10x the rate. 14 | */ 15 | 16 | extern uint32_t blinker_timer_mark; 17 | extern bool blinker_status; 18 | 19 | void setup_blink(); 20 | void blink(); 21 | 22 | #endif -------------------------------------------------------------------------------- /src/firmware/src/buff_cpp/device_manager.h: -------------------------------------------------------------------------------- 1 | #include "sensors/dr16.h" 2 | #include "sensors/revEnc.h" 3 | #include "sensors/lsm6dsox.h" 4 | #include "sensors/refSystem.h" 5 | #include "buff_cpp/controllers.h" 6 | #include "robot_comms/hid_report.h" 7 | #include "motor_drivers/rm_can_interface.h" 8 | 9 | #ifndef BUFF_DEVICE_MANAGER_H 10 | #define BUFF_DEVICE_MANAGER_H 11 | 12 | /* 13 | An organizer for all the pipelines with the firmware. 14 | A large portion of this is HID handling (lots of switches) 15 | Drivers should provide a good api for this object to use them. 16 | 17 | The idea is that by organizing drivers here our main is 18 | dummy simple and easy to adjust the scheduling. 19 | */ 20 | 21 | struct Device_Manager { 22 | Device_Manager(); 23 | 24 | // handlers for HID reports 25 | void initializer_report_handle(); 26 | void feedback_request_handle(); 27 | void control_input_handle(); 28 | void sensor_request_handle(); 29 | 30 | // More HID report handlers 31 | void report_switch(); 32 | void hid_input_switch(uint32_t); 33 | 34 | // Non-HID related pipelines 35 | void push_can(); 36 | void read_sensors(); // add functionality to read the Rev encoders (in addition to another sensor) 37 | void step_controllers(float); 38 | 39 | // Data Store 40 | // this is the cylinder in the flow chart 41 | // drivers will put their data here 42 | Hid_Report input_report; // message being read on teensy from computer (request or command) 43 | Hid_Report output_report; // data being sent from teensy to computer 44 | 45 | // ADD new IMU here, also make sure the 46 | // senor pipeline and dev manager constructors knows about it!! 47 | LSM6DSOX chassis_imu; 48 | DR16 receiver; 49 | RefSystem ref; 50 | RevEnc yawEncoder = RevEnc(1); 51 | RevEnc pitchEncoder = RevEnc(2); 52 | RevEnc xOdometryEncoder = RevEnc(3); 53 | RevEnc yOdometryEncoder = RevEnc(4); 54 | 55 | RM_CAN_Interface rm_can_ux; 56 | Controller_Manager controller_manager; 57 | 58 | // track the time since a connection was made 59 | float lifetime; 60 | 61 | // Logic for state machines (literally which case to run) 62 | // see implementation for more details 63 | int sensor_switch; 64 | int controller_switch; 65 | }; 66 | 67 | #endif -------------------------------------------------------------------------------- /src/firmware/src/buff_cpp/global_robot_state.cpp: -------------------------------------------------------------------------------- 1 | #include "global_robot_state.h" 2 | 3 | #include "motor_drivers/rm_can_interface.h" 4 | 5 | MotorMap::MotorMap(RM_CAN_Interface* rmCAN) { 6 | this->rmCAN = rmCAN; 7 | 8 | byte x_fr[3] = {CANBUS_1, C620, 1}; 9 | rmCAN->set_index(1, x_fr); 10 | byte x_fl[3] = {CANBUS_1, C620, 2}; 11 | rmCAN->set_index(2, x_fl); 12 | byte x_bl[3] = {CANBUS_1, C620, 3}; 13 | rmCAN->set_index(3, x_bl); 14 | byte x_br[3] = {CANBUS_1, C620, 7}; 15 | rmCAN->set_index(7, x_br); 16 | 17 | byte yaw_1[3] = {CANBUS_1, C610, 5}; 18 | rmCAN->set_index(5, yaw_1); 19 | byte yaw_2[3] = {CANBUS_1, C620, 6}; 20 | rmCAN->set_index(6, yaw_2); 21 | 22 | byte pitch_l[3] = {CANBUS_2, C620, 1}; 23 | rmCAN->set_index(9, pitch_l); 24 | byte pitch_r[3] = {CANBUS_2, C620, 2}; 25 | rmCAN->set_index(10, pitch_r); 26 | 27 | byte flywheel_l[3] = {CANBUS_2, C620, 3}; 28 | rmCAN->set_index(11, flywheel_l); 29 | byte flywheel_r[3] = {CANBUS_2, C620, 4}; 30 | rmCAN->set_index(12, flywheel_r); 31 | 32 | byte feeder_l[3] = {CANBUS_2, C610, 5}; 33 | rmCAN->set_index(13, feeder_l); 34 | byte feeder_r[3] = {CANBUS_2, C610, 6}; 35 | rmCAN->set_index(14, feeder_r); 36 | 37 | //rmCAN->addMotor("Yaw 1", 8, CAN1, C620); 38 | 39 | // rmCAN->addMotor("FR Drive", 1, CAN1, C620); 40 | // rmCAN->addMotor("FL Drive", 2, CAN1, C620); 41 | // rmCAN->addMotor("BL Drive", 3, CAN1, C620); 42 | // rmCAN->addMotor("BR Drive", 4, CAN1, C620); 43 | // rmCAN->addMotor("Yaw 1", 5, CAN1, C620); 44 | // rmCAN->addMotor("Yaw 2", 6, CAN1, C620); 45 | // rmCAN->addMotor("Pitch L", 1, CAN2, C620); 46 | // rmCAN->addMotor("Pitch R", 2, CAN2, C620); 47 | // rmCAN->addMotor("Flywheel L", 3, CAN2, C620); 48 | // rmCAN->addMotor("Flywheel R", 4, CAN2, C620); 49 | // rmCAN->addMotor("Feeder L", 5, CAN2, C610); 50 | // rmCAN->addMotor("Feeder R", 6, CAN2, C610); 51 | 52 | pid.K[0] = 0.00045; 53 | } 54 | 55 | void MotorMap::setMotorRPM(int idx, float rpm, int deltaTime) { 56 | pid.setpoint = rpm; 57 | pid.measurement = rmCAN->get_motor_RPM(idx); 58 | pid.filter(deltaTime); 59 | rmCAN->set_output(idx, constrain(pid.output, -1.0, 1.0)); 60 | } 61 | 62 | float MotorMap::generateMotorRPMOutput(int idx, float rpm, int deltaTime) { 63 | pid.setpoint = rpm; 64 | pid.measurement = rmCAN->get_motor_RPM(idx); 65 | pid.filter(deltaTime); 66 | return constrain(pid.output, -1.0, 1.0); 67 | } 68 | 69 | void MotorMap::allOff() { 70 | for (int i = 0; i < 16; i++) 71 | rmCAN->set_output(i, 0); 72 | } -------------------------------------------------------------------------------- /src/firmware/src/buff_cpp/global_robot_state.h: -------------------------------------------------------------------------------- 1 | #include "algorithms/pid_filter.h" 2 | #include "motor_drivers/rm_can_interface.h" 3 | #include "sensors/dr16.h" 4 | #include "sensors/lsm6dsox.h" 5 | #include "sensors/revEnc.h" 6 | #include "sensors/refSystem.h" 7 | 8 | #ifndef GLOBAL_ROBOT_STATE_H 9 | #define GLOBAL_ROBOT_STATE_H 10 | 11 | #define DEMO_CHASSIS_MAX_RPM 8500 12 | #define MATCH_CHASSIS_MAX_RPM 8500 13 | #define DEMO_GIMBAL_YAW_MAX_RPM 2000 14 | #define DEMO_GIMBAL_PITCH_MAX_RPM 200 15 | 16 | enum RobotMode { 17 | OFF, 18 | DEMO, 19 | MATCH 20 | }; 21 | 22 | enum SystemMode { 23 | IDLE, 24 | MANUAL, 25 | AUTO 26 | }; 27 | 28 | struct MotorMap { 29 | MotorMap(RM_CAN_Interface* rmCAN); 30 | void setMotorRPM(int idx, float rpm, int deltaTime); 31 | float generateMotorRPMOutput(int idx, float rpm, int deltaTime); 32 | void allOff(); 33 | 34 | RM_CAN_Interface* rmCAN; 35 | PIDFilter pid; 36 | }; 37 | 38 | struct GlobalRobotState { 39 | RM_CAN_Interface rmCAN; 40 | 41 | DR16 receiver; 42 | LSM6DSOX imu; 43 | RefSystem ref; 44 | RevEnc yawEncoder = RevEnc(1); 45 | RevEnc pitchEncoder = RevEnc(2); 46 | RevEnc xOdometryEncoder = RevEnc(3); 47 | RevEnc yOdometryEncoder = RevEnc(4); 48 | 49 | MotorMap motorMap = MotorMap(&rmCAN); 50 | void setMotorRPM(int idx, float rpm) { motorMap.setMotorRPM(idx, rpm, deltaTime); } 51 | float generateMotorRPMOutput(int idx, float rpm) { return motorMap.generateMotorRPMOutput(idx, rpm, deltaTime); } 52 | 53 | float deltaTime; 54 | 55 | RobotMode robotMode = OFF; 56 | 57 | SystemMode chassisMode = IDLE; 58 | float chassisHeading; // degrees relative to global north 59 | float chassisHeadingRate; // rpm 60 | 61 | SystemMode gimbalMode = IDLE; 62 | float gimbalHeading[2]; // (yaw, pitch): degrees relative to global north 63 | float gimbalHeadingRate[2]; // (yawRate, pitchRate): rpm 64 | 65 | SystemMode shooterMode = IDLE; 66 | }; 67 | 68 | #endif -------------------------------------------------------------------------------- /src/firmware/src/buff_cpp/loggers.h: -------------------------------------------------------------------------------- 1 | 2 | 3 | #ifndef BUFF_LOGGERS 4 | #define BUFF_LOGGERS 5 | 6 | 7 | // #define IS_EQ(a, b) (a = b) ? 1 : 0 8 | // #define ASSERT_EQ(a, b, message) IS_EQ(a, b) 9 | 10 | 11 | // Data types 12 | // dr16 float[7] 13 | /* Basic serial print functions */ 14 | 15 | // template 16 | // int assert_eq(T, T , String); 17 | 18 | // template 19 | // int assert_arr_eq(T*, T*, int, String); 20 | 21 | // template 22 | // int assert_arr_eq(T, T*, int, String); 23 | 24 | int int_eq(int, int, String); 25 | int ints_eq(int, int*, int, String); 26 | int ints_eq(int*, int*, int, String); 27 | int byte_eq(int, int, String); 28 | int bytes_eq(byte, byte*, int, String); 29 | int bytes_eq(byte*, byte*, int, String); 30 | int float_eq(float, float, String); 31 | int floats_eq(float*, float*, int, String); 32 | int float_neq(float, float, String); 33 | int float_nan(float); 34 | int float_leq(float, float, String); 35 | int float_geq(float, float, String); 36 | 37 | void timer_print(uint32_t, String); 38 | 39 | #endif -------------------------------------------------------------------------------- /src/firmware/src/buff_cpp/teensy_io.cpp: -------------------------------------------------------------------------------- 1 | #include "teensy_io.h" 2 | 3 | 4 | Teensy_IO::Teensy_IO() { 5 | num_pins = 0; 6 | } 7 | 8 | bool Teensy_IO::setup_pin(int pin, int mode) { 9 | if (num_pins == MAX_PINS) { 10 | return false; 11 | } 12 | 13 | pinMode(pin, mode); 14 | analogWrite(pin, 0); 15 | pins[num_pins] = pin; 16 | num_pins ++; 17 | return true; 18 | } 19 | 20 | void Teensy_IO::write_pin(int pin, int value) { 21 | if (0 < pin && pin < num_pins) 22 | analogWrite(pins[pin], value); 23 | } 24 | 25 | int Teensy_IO::read_pin(int pin) { 26 | if (0 < pin && pin < num_pins) 27 | return analogRead(pin); 28 | else 29 | return -1; 30 | } 31 | -------------------------------------------------------------------------------- /src/firmware/src/buff_cpp/teensy_io.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #ifndef ANALOG_PINS_H 4 | #define ANALOG_PINS_H 5 | 6 | #define MAX_PINS 8 7 | 8 | 9 | struct Teensy_IO { 10 | Teensy_IO(); 11 | bool setup_pin(int, int); 12 | void write_pin(int, int); 13 | int read_pin(int); 14 | 15 | int num_pins; 16 | int pins[MAX_PINS]; 17 | }; 18 | 19 | #endif -------------------------------------------------------------------------------- /src/firmware/src/buff_cpp/timing.cpp: -------------------------------------------------------------------------------- 1 | #include "buff_cpp/timing.h" 2 | 3 | uint32_t timers[NUM_BUFF_TIMERS]; 4 | 5 | void init_timers() { 6 | for (size_t i = 0; i < NUM_BUFF_TIMERS; i++) { 7 | timers[i] = ARM_DWT_CYCCNT; 8 | } 9 | } 10 | 11 | uint32_t duration_info(uint32_t start, uint32_t stop){ 12 | /* 13 | Helper to print info about *very small* durations in 14 | time. Prints the cycles and time in ns. 15 | @param 16 | start: (uint32_t) value of ARM_DWT_CYCCNT at the beginning of a duration 17 | stop: (uint32_t) value of ARM_DWT_CYCNT at the end of a duration 18 | @return 19 | delta_ns: (uint32_t) duration in nanoseconds 20 | */ 21 | uint32_t delta_cycles = stop - start; 22 | uint32_t delta_ns = CYCLES_TO_NS(delta_cycles); 23 | Serial.printf( "\t%1lu cycles, %1lu ns\n", delta_cycles, delta_ns); 24 | return delta_ns; 25 | } 26 | 27 | void timer_set(int idx) { 28 | timers[idx] = ARM_DWT_CYCCNT; 29 | } 30 | 31 | void timer_mark(int idx) { 32 | duration_info(timers[idx], ARM_DWT_CYCCNT); 33 | } 34 | 35 | uint32_t timer_info_us(int idx) { 36 | return DURATION_US(timers[idx], ARM_DWT_CYCCNT); 37 | } 38 | 39 | uint32_t timer_info_ms(int idx) { 40 | return DURATION_MS(timers[idx], ARM_DWT_CYCCNT); 41 | } 42 | 43 | void timer_wait_us(int idx, uint32_t duration){ 44 | /* 45 | Helper to pause for a duration. Duration starts 46 | when timer_set() is called. 47 | @param 48 | duration: (uint32_t) microseconds to wait (from when timer_set() was called) 49 | @return 50 | None 51 | */ 52 | while(DURATION_US(timers[idx], ARM_DWT_CYCCNT) < duration) {} 53 | } 54 | 55 | void timer_wait_ms(int idx, uint32_t duration){ 56 | /* 57 | Helper to pause for a duration. Duration starts 58 | when timer_set() is called. 59 | @param 60 | duration: (uint32_t) milliseconds to wait (from when timer_set() was called) 61 | @return 62 | None 63 | */ 64 | while(DURATION_MS(timers[idx], ARM_DWT_CYCCNT) < duration) {} 65 | } -------------------------------------------------------------------------------- /src/firmware/src/buff_cpp/timing.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #ifndef BUFF_TIMING 4 | #define BUFF_TIMING 5 | 6 | // Some generic converters 7 | #define MS_TO_US(ms) (ms * 1000) 8 | #define MS_TO_NS(ms) (ms * 1000000) 9 | #define US_TO_NS(us) (us * 1000) 10 | #define US_TO_MS(us) (us / 1000) 11 | #define NS_TO_US(ns) (ns / 1000) 12 | #define NS_TO_MS(ns) (ns / 1000000) 13 | 14 | // pass ARM_DWT_CYCNT to this to get the timing down to nanoseconds 15 | #define CYCLES_TO_MS(cycles) ((cycles)*(1E3/F_CPU)) 16 | #define CYCLES_TO_US(cycles) ((cycles)*(1E6/F_CPU)) 17 | #define CYCLES_TO_NS(cycles) ((cycles)*(1E9/F_CPU)) 18 | 19 | // Get time duration from two cycle counts 20 | #define DURATION_MS(cyccnt1, cyccnt2) (CYCLES_TO_MS(cyccnt2 - cyccnt1)) 21 | #define DURATION_US(cyccnt1, cyccnt2) (CYCLES_TO_US(cyccnt2 - cyccnt1)) 22 | #define DURATION_NS(cyccnt1, cyccnt2) (CYCLES_TO_NS(cyccnt2 - cyccnt1)) 23 | 24 | 25 | #define NUM_BUFF_TIMERS 10 26 | 27 | extern uint32_t timers[NUM_BUFF_TIMERS]; 28 | 29 | void init_timers(); 30 | uint32_t duration_info(uint32_t, uint32_t); 31 | void timer_set(int); 32 | void timer_mark(int); 33 | uint32_t timer_info_us(int); 34 | uint32_t timer_info_ms(int); 35 | void timer_wait_us(int, uint32_t); 36 | void timer_wait_ms(int, uint32_t); 37 | 38 | #endif -------------------------------------------------------------------------------- /src/firmware/src/main.cpp: -------------------------------------------------------------------------------- 1 | #include "buff_cpp/blink.h" 2 | #include "buff_cpp/timing.h" 3 | #include "buff_cpp/loggers.h" 4 | #include "buff_cpp/device_manager.h" 5 | 6 | uint32_t cycle_time_us = 1000; 7 | uint32_t cycle_time_ms = cycle_time_us / 1000; 8 | float cycle_time_s = cycle_time_us / 1E6; 9 | 10 | 11 | Device_Manager device_manager; // all firmware pipelines are implemented in this object. 12 | // Device Manager provides a single function call for each of the pipelines 13 | // with unit tests we can analyze the execution time and complexity of each 14 | // pipeline. Then organize them into the master loop accordingly 15 | 16 | // Runs once 17 | void setup() { 18 | Serial.begin(1000000); // the serial monitor is actually always active (for debug use Serial.println & tycmd) 19 | 20 | if (Serial) { 21 | Serial.println("-- TEENSY SERIAL START --"); 22 | Serial.println("-- new build... who dis? --"); 23 | } 24 | } 25 | 26 | // Master loop 27 | int main() { // Basically a schudeling algorithm 28 | setup(); 29 | 30 | while(1) { 31 | timer_set(0); 32 | 33 | // handle any hid input output 34 | device_manager.read_sensors(); // read a single sensor each call (increments the sensor id automatically) 35 | device_manager.step_controllers(cycle_time_s); // given the current inputs and feedback compute a control 36 | device_manager.hid_input_switch(cycle_time_us); // check for an input packet (data request/control input) handle accordingly 37 | device_manager.push_can(); // push data on and off the can bus 38 | 39 | timer_wait_us(0, cycle_time_us); // normalize master loop cycle time to cycle_time_u 40 | // blink(); // helpful if you think the loop is crashing (light will pause) 41 | } 42 | 43 | return 0; 44 | } 45 | -------------------------------------------------------------------------------- /src/firmware/src/robot_comms/hid_report.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #ifndef BUFFHID_REPORT_H 4 | #define BUFFHID_REPORT_H 5 | 6 | #define HID_REPORT_SIZE_BYTES 64 7 | 8 | typedef union 9 | { 10 | float number; 11 | byte bytes[4]; 12 | } FLOATBYTE_t; 13 | 14 | /* 15 | A Packet struct to clean up the inserts and reads. 16 | The goal is to have an hid.fill_report_xxx(data), 17 | that will put the data in our HID report structure and 18 | send it to serial. Similarly hid.read() should decompose 19 | the recieved report into smaller data. 20 | 21 | the main priority is putting data in and out of the packet. 22 | There is some discussion to be had on whose job it is to 23 | determine indexing, for now it will be the developers responsibility. 24 | 25 | Get/Set requirements: 26 | IMU: float[9] 27 | 28 | DR16: byte[18] 29 | 30 | CAN: int16[i][3] (feedback) 31 | int16[i] (command) 32 | 33 | Teensy clock: int32 34 | 35 | HID_REPORT also has quick functions to 36 | read and write its data. 37 | */ 38 | 39 | struct Hid_Report { 40 | byte data[HID_REPORT_SIZE_BYTES]; 41 | 42 | // constructor 43 | Hid_Report(); 44 | 45 | // for debug, print the packet with serial.print 46 | void print(); 47 | // clear all data in the packet 48 | void clear(); 49 | 50 | // Getters/Setters 51 | byte get(int); 52 | void put(int, byte); 53 | 54 | int32_t get_int32(int); 55 | void put_int32(int, int32_t); 56 | 57 | float get_float(int); 58 | void put_float(int, float); 59 | 60 | void get_chars(int, int, char*); 61 | void put_chars(int, char*); 62 | 63 | void rgets(byte*, int, int); 64 | void rputs(byte*, int, int); 65 | 66 | // fill data with the available HID packet, (wait if none?) 67 | int8_t read(); 68 | // send data as HID packet 69 | int8_t write(); 70 | 71 | }; 72 | 73 | #endif -------------------------------------------------------------------------------- /src/firmware/src/sensors/dr16.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #ifndef BUFF_DR16_H 4 | #define BUFF_DR16_H 5 | 6 | #define REMOTE_CONTROL_LEN 7 7 | #define JOYSTICK_X_SENSITIVITY 0.05 8 | #define JOYSTICK_Y_SENSITIVITY 1.5 9 | #define JOYSTICK_PAN_SENSITIVITY 0.01 10 | #define JOYSTICK_PITCH_SENSITIVITY 0.01 11 | 12 | /* 13 | Driver software for the dr16 receiver. 14 | Parses the serial input to a robot action. 15 | 16 | Remote control format (float) 17 | 1: x speed 18 | 2: y speed 19 | 3: omega 20 | 4: dpitch 21 | 5: dyaw 22 | 6: dfeeder 23 | 7: const 24 | */ 25 | 26 | float normalize_channel(int16_t); 27 | float wrap_radians(float); 28 | 29 | struct DR16_DATA { 30 | float l_stick_x; 31 | float l_stick_y; 32 | float r_stick_x; 33 | float r_stick_y; 34 | float wheel; 35 | int l_switch; 36 | int r_switch; 37 | }; 38 | 39 | struct DR16 { 40 | DR16(); 41 | DR16(HardwareSerial*); 42 | void print_receiver_input(byte*); 43 | void print_control_data(); 44 | void generate_control_from_joysticks(); 45 | void generate_output(); 46 | void control_test(); 47 | bool read(); 48 | 49 | float numBytes; 50 | unsigned long lastTime; 51 | 52 | float data[7]; 53 | DR16_DATA out; 54 | 55 | float demo[]; 56 | 57 | HardwareSerial* serial; 58 | }; 59 | 60 | #endif -------------------------------------------------------------------------------- /src/firmware/src/sensors/icm20649.cpp: -------------------------------------------------------------------------------- 1 | #include "icm20649.h" 2 | 3 | ICM20649::ICM20649() { 4 | // start i2c communications 5 | icm20649.begin_I2C(); 6 | 7 | // get the sensor objects linked to the sensor 8 | icm_temp = icm20649.getTemperatureSensor(); 9 | icm_accel = icm20649.getAccelerometerSensor(); 10 | icm_gyro = icm20649.getGyroSensor(); 11 | } 12 | 13 | // void ICM20649::readSensor() { 14 | // icm_temp = icm20649.getTemperatureSensor(); 15 | // icm_accel = icm20649.getAccelerometerSensor(); 16 | // icm_gyro = icm20649.getGyroSensor(); 17 | // } 18 | 19 | void ICM20649::readSensor() { 20 | icm_temp->getEvent(&temp); 21 | icm_accel->getEvent(&accel); 22 | icm_gyro->getEvent(&gyro); 23 | 24 | accelX = accel.acceleration.x; 25 | accelY = accel.acceleration.y; 26 | accelZ = accel.acceleration.z; 27 | 28 | gyroX = gyro.gyro.x; 29 | gyroY = gyro.gyro.y; 30 | gyroZ = gyro.gyro.z; 31 | } 32 | -------------------------------------------------------------------------------- /src/firmware/src/sensors/icm20649.h: -------------------------------------------------------------------------------- 1 | // #include 2 | #include 3 | // #include 4 | 5 | #ifndef BUFF_ICM20649_H 6 | #define BUFF_ICM20649_H 7 | 8 | class ICM20649 { 9 | public: 10 | ICM20649(); 11 | void readSensor(); 12 | float getTemperature(){ return temp.temperature; }; 13 | 14 | float getAccelX() { return accelX; }; 15 | float getAccelY() { return accelY; }; 16 | float getAccelZ() { return accelZ; }; 17 | 18 | float getGyroX() { return gyroX; }; 19 | float getGyroY() { return gyroY; }; 20 | float getGyroZ() { return gyroZ; }; 21 | private: 22 | Adafruit_ICM20649 icm20649; 23 | Adafruit_Sensor *icm_temp, *icm_accel, *icm_gyro; 24 | sensors_event_t accel; 25 | sensors_event_t gyro; 26 | sensors_event_t temp; 27 | 28 | float accelX = 0; 29 | float accelY = 0; 30 | float accelZ = 0; 31 | 32 | float gyroX = 0; 33 | float gyroY = 0; 34 | float gyroZ = 0; 35 | }; 36 | 37 | #endif 38 | -------------------------------------------------------------------------------- /src/firmware/src/sensors/refSystem.h: -------------------------------------------------------------------------------- 1 | #ifndef REF_SYS_H 2 | #define REF_SYS_H 3 | 4 | struct RefData { 5 | char curr_stage = '\r'; 6 | char comp_type = '\r'; 7 | char comp_result = '\r'; 8 | 9 | int rem_time = -1; 10 | 11 | int red_hero_hp = -1; 12 | int red_sentry_hp = -1; 13 | int red_infantry_hp = -1; 14 | int blue_hero_hp = -1; 15 | int blue_sentry_hp = -1; 16 | int blue_infantry_hp = -1; 17 | int red_hero_max_hp = -1; 18 | int red_sentry_max_hp = -1; 19 | int red_infantry_max_hp = -1; 20 | int blue_hero_max_hp = -1; 21 | int blue_sentry_max_hp = -1; 22 | int blue_infantry_max_hp = -1; 23 | 24 | int red_one_rem_proj = -1; 25 | int red_two_rem_proj = -1; 26 | int blue_one_rem_proj = -1; 27 | int blue_two_rem_proj = -1; 28 | 29 | char ref_warning = '\r'; 30 | int foul_robot_id = -1; 31 | 32 | int robot_id = -1; 33 | int robot_level = -1; 34 | int robot_health = -1; 35 | 36 | int red_hero_robot_level = -1; 37 | int red_infantry_robot_level = -1; 38 | int red_sentry_robot_level = -1; 39 | int blue_hero_robot_level = -1; 40 | int blue_infantry_robot_level = -1; 41 | int blue_sentry_robot_level = -1; 42 | 43 | int robot_1_cool_val = -1; //17mm 44 | int robot_1_barr_heat_lim = -1; //17mm 45 | int robot_1_speed_lim = -1; //17mm 46 | 47 | int robot_2_cool_val = -1; //17mm 48 | int robot_2_barr_heat_lim = -1; //17mm 49 | int robot_2_speed_lim = -1; //17mm 50 | 51 | int robot_42_cool_val = -1; 52 | int robot_42_heat_lim = -1; 53 | int robot_42_speed_lim = -1; 54 | 55 | int robot_power_lim = -1; 56 | 57 | int chassis_voltage = -1; 58 | int chassis_current = -1; 59 | int power_buffer = -1; 60 | 61 | int robot_buff = -1; 62 | 63 | int launch_freq = -1; 64 | int launch_speed = -1; 65 | 66 | int rem_17_proj = -1; 67 | int rem_42_proj = -1; 68 | 69 | int chassis_on = -1; 70 | int gimbal_on = -1; 71 | int shooter_on = -1; 72 | }; 73 | 74 | class RefSystem{ 75 | public: 76 | RefSystem(); 77 | void init(); 78 | bool read_serial(); 79 | RefData data; 80 | }; 81 | 82 | #endif -------------------------------------------------------------------------------- /src/firmware/src/sensors/revEnc.cpp: -------------------------------------------------------------------------------- 1 | #include "revEnc.h" 2 | 3 | RevEnc::RevEnc(uint8_t encPin) { 4 | // On the Teensy 4.1 encPin can be pin 0-9, 22-25, 28,2 9, 33, 36, 37, 42-47, 48-50(dups), 51, 52-53 (dups), 54 5 | // Is this necessary? 6 | if ((encPin >= 0 && encPin <= 9) || (encPin >= 22 && encPin <= 25) || encPin == 28 || encPin == 29 || encPin == 33 || encPin == 36 || encPin == 37 || (encPin >= 42 && encPin <= 54)) { 7 | this->inPin = encPin; 8 | pinMode(this->inPin, INPUT); // Set the pin used to measure the encoder to be an input 9 | } else { 10 | this->inPin = -1; 11 | } 12 | 13 | freq.begin(this->inPin, FREQMEASUREMULTI_MARK_ONLY); 14 | } 15 | 16 | int RevEnc::getAngleRaw() { 17 | // Burn through buffer of values in freq 18 | while (this->freq.available() > 1) { 19 | this->freq.read(); 20 | } 21 | 22 | int angle = round(freq.countToNanoseconds(freq.read()) / 1000.0); 23 | if (angle >= 1 && angle <= 1025) { 24 | return angle; 25 | } 26 | 27 | return -1; 28 | } 29 | 30 | float RevEnc::getAngle() { 31 | int rawAngle = this->getAngleRaw(); 32 | return rawAngle == -1 ? -1 : map(rawAngle, 1, 1024, 0, 36000) / 100.0; 33 | } -------------------------------------------------------------------------------- /src/firmware/src/sensors/revEnc.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #ifndef RevEnc_H 4 | #define RevEnc_H 5 | 6 | class RevEnc { 7 | private: 8 | uint8_t inPin; 9 | FreqMeasureMulti freq; 10 | public: 11 | RevEnc(uint8_t encPin); 12 | int getAngleRaw(); 13 | float getAngle(); 14 | }; 15 | 16 | #endif -------------------------------------------------------------------------------- /src/firmware/teensy4/Arduino.h: -------------------------------------------------------------------------------- 1 | // This header file is in the public domain. 2 | 3 | #ifndef Arduino_h 4 | #define Arduino_h 5 | 6 | #include "WProgram.h" 7 | #include "pins_arduino.h" 8 | 9 | #endif 10 | -------------------------------------------------------------------------------- /src/firmware/teensy4/Blink.cc: -------------------------------------------------------------------------------- 1 | // Only for testing with the Makefile - not actually part of the core library 2 | 3 | #include 4 | 5 | int led = 13; 6 | 7 | void setup() { 8 | pinMode(led, OUTPUT); 9 | } 10 | 11 | void loop() { 12 | digitalWrite(led, HIGH); 13 | delay(1000); 14 | digitalWrite(led, LOW); 15 | delay(1000); 16 | } 17 | -------------------------------------------------------------------------------- /src/firmware/teensy4/Client.h: -------------------------------------------------------------------------------- 1 | /* 2 | Client.h - Base class that provides Client 3 | Copyright (c) 2011 Adrian McEwen. All right reserved. 4 | 5 | This library is free software; you can redistribute it and/or 6 | modify it under the terms of the GNU Lesser General Public 7 | License as published by the Free Software Foundation; either 8 | version 2.1 of the License, or (at your option) any later version. 9 | 10 | This library is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 13 | Lesser General Public License for more details. 14 | 15 | You should have received a copy of the GNU Lesser General Public 16 | License along with this library; if not, write to the Free Software 17 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 18 | */ 19 | 20 | #if ARDUINO >= 100 21 | 22 | #ifndef client_h 23 | #define client_h 24 | #include "Print.h" 25 | #include "Stream.h" 26 | #include "IPAddress.h" 27 | 28 | class Client : public Stream { 29 | 30 | public: 31 | virtual int connect(IPAddress ip, uint16_t port) =0; 32 | virtual int connect(const char *host, uint16_t port) =0; 33 | virtual size_t write(uint8_t) =0; 34 | virtual size_t write(const uint8_t *buf, size_t size) =0; 35 | virtual int available() = 0; 36 | virtual int read() = 0; 37 | virtual int read(uint8_t *buf, size_t size) = 0; 38 | virtual int peek() = 0; 39 | virtual void flush() = 0; 40 | virtual void stop() = 0; 41 | virtual uint8_t connected() = 0; 42 | virtual operator bool() = 0; 43 | protected: 44 | uint8_t* rawIPAddress(IPAddress& addr) { return addr.raw_address(); }; 45 | }; 46 | 47 | #endif 48 | #endif 49 | -------------------------------------------------------------------------------- /src/firmware/teensy4/CrashReport.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | class CrashReportClass: public Printable { 7 | public: 8 | virtual size_t printTo(Print& p) const; 9 | void clear(); 10 | operator bool(); 11 | void breadcrumb(unsigned int num, unsigned int value) { 12 | // crashreport_breadcrumbs_struct occupies exactly 1 cache row 13 | volatile struct crashreport_breadcrumbs_struct *bc = 14 | (struct crashreport_breadcrumbs_struct *)0x2027FFC0; 15 | if (num >= 1 && num <= 6) { 16 | num--; 17 | bc->value[num] = value; 18 | bc->bitmask |= (1 << num); 19 | arm_dcache_flush((void *)bc, sizeof(struct crashreport_breadcrumbs_struct)); 20 | } 21 | } 22 | }; 23 | 24 | extern CrashReportClass CrashReport; 25 | -------------------------------------------------------------------------------- /src/firmware/teensy4/IPAddress.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | IPAddress.cpp - Base class that provides IPAddress 3 | Copyright (c) 2011 Adrian McEwen. All right reserved. 4 | 5 | This library is free software; you can redistribute it and/or 6 | modify it under the terms of the GNU Lesser General Public 7 | License as published by the Free Software Foundation; either 8 | version 2.1 of the License, or (at your option) any later version. 9 | 10 | This library is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 13 | Lesser General Public License for more details. 14 | 15 | You should have received a copy of the GNU Lesser General Public 16 | License along with this library; if not, write to the Free Software 17 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 18 | */ 19 | 20 | #include 21 | #include "IPAddress.h" 22 | 23 | size_t IPAddress::printTo(Print& p) const 24 | { 25 | int i=0; 26 | while (1) { 27 | p.print(_address.bytes[i], DEC); 28 | if (++i >= 4) return 4; 29 | p.write('.'); 30 | } 31 | } 32 | 33 | bool IPAddress::fromString(const char *address) 34 | { 35 | unsigned int acc = 0; // Accumulator 36 | unsigned int dots = 0; 37 | 38 | while (*address) { 39 | char c = *address++; 40 | if (c >= '0' && c <= '9') { 41 | acc = acc * 10 + (c - '0'); 42 | if (acc > 255) { 43 | // Value out of [0..255] range 44 | return false; 45 | } 46 | } else if (c == '.') { 47 | if (dots == 3) { 48 | // Too much dots (there must be 3 dots) 49 | return false; 50 | } 51 | _address.bytes[dots++] = acc; 52 | acc = 0; 53 | } else { 54 | // Invalid char 55 | return false; 56 | } 57 | } 58 | if (dots != 3) { 59 | // Too few dots (there must be 3 dots) 60 | return false; 61 | } 62 | _address.bytes[3] = acc; 63 | return true; 64 | } 65 | 66 | -------------------------------------------------------------------------------- /src/firmware/teensy4/Keyboard.h: -------------------------------------------------------------------------------- 1 | // empty Keyboard.h file, for compability with Arduino's Keyboard examples 2 | 3 | // This header file is in the public domain. 4 | -------------------------------------------------------------------------------- /src/firmware/teensy4/MIDIUSB.h: -------------------------------------------------------------------------------- 1 | #ifndef MIDIUSB_h 2 | #define MIDIUSB_h 3 | 4 | // For compatibility with Arduino's MIDIUSB library 5 | 6 | #include "usb_midi.h" 7 | 8 | #ifdef __cplusplus 9 | #if !defined(USB_MIDI) && !defined(USB_MIDI4) && !defined(USB_MIDI16) && !defined(USB_MIDI_SERIAL) && !defined(USB_MIDI4_SERIAL) && !defined(USB_MIDI16_SERIAL) && !defined(USB_MIDI_AUDIO_SERIAL) && !defined(USB_MIDI16_AUDIO_SERIAL) && !defined(USB_EVERYTHING) 10 | #error "Please select MIDI in Tools > USB Type to use MIDIUSB.h" 11 | #endif 12 | 13 | typedef struct { 14 | union { 15 | struct { 16 | uint8_t header; 17 | uint8_t byte1; 18 | uint8_t byte2; 19 | uint8_t byte3; 20 | }; 21 | uint32_t word; 22 | }; 23 | } midiEventPacket_t; 24 | 25 | 26 | class MIDI_ 27 | { 28 | public: 29 | constexpr MIDI_(void) { } 30 | uint32_t available(void) { 31 | return usb_midi_available(); 32 | } 33 | midiEventPacket_t read(void) { 34 | midiEventPacket_t event; 35 | event.word = usb_midi_read_message(); 36 | return event; 37 | } 38 | void flush(void) { 39 | usb_midi_flush_output(); 40 | } 41 | void sendMIDI(midiEventPacket_t event) { 42 | usb_midi_write_packed(event.word); 43 | } 44 | size_t write(const uint8_t *buffer, size_t size) { 45 | // TODO - is this really needed? 46 | return 0; 47 | } 48 | operator bool() { 49 | // TODO - is this really needed? 50 | return true; 51 | } 52 | }; 53 | extern MIDI_ MidiUSB; 54 | 55 | #endif // __cplusplus 56 | #endif // MIDIUSB_h 57 | -------------------------------------------------------------------------------- /src/firmware/teensy4/Mouse.h: -------------------------------------------------------------------------------- 1 | // empty Mouse.h file, for compability with Arduino's Mouse examples 2 | 3 | // This header file is in the public domain. 4 | -------------------------------------------------------------------------------- /src/firmware/teensy4/Printable.h: -------------------------------------------------------------------------------- 1 | /* 2 | Printable.h - Interface class that allows printing of complex types 3 | Copyright (c) 2011 Adrian McEwen. All right reserved. 4 | 5 | This library is free software; you can redistribute it and/or 6 | modify it under the terms of the GNU Lesser General Public 7 | License as published by the Free Software Foundation; either 8 | version 2.1 of the License, or (at your option) any later version. 9 | 10 | This library is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 13 | Lesser General Public License for more details. 14 | 15 | You should have received a copy of the GNU Lesser General Public 16 | License along with this library; if not, write to the Free Software 17 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 18 | */ 19 | 20 | #ifndef Printable_h 21 | #define Printable_h 22 | 23 | #ifdef __cplusplus 24 | 25 | #include 26 | 27 | class Print; 28 | 29 | /** The Printable class provides a way for new classes to allow themselves to be printed. 30 | By deriving from Printable and implementing the printTo method, it will then be possible 31 | for users to print out instances of this class by passing them into the usual 32 | Print::print and Print::println methods. 33 | */ 34 | class Printable 35 | { 36 | public: 37 | virtual size_t printTo(Print& p) const = 0; 38 | }; 39 | 40 | 41 | #endif 42 | #endif 43 | -------------------------------------------------------------------------------- /src/firmware/teensy4/Server.h: -------------------------------------------------------------------------------- 1 | /* 2 | Server.h - Base class that provides Server 3 | Copyright (c) 2011 Adrian McEwen. All right reserved. 4 | 5 | This library is free software; you can redistribute it and/or 6 | modify it under the terms of the GNU Lesser General Public 7 | License as published by the Free Software Foundation; either 8 | version 2.1 of the License, or (at your option) any later version. 9 | 10 | This library is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 13 | Lesser General Public License for more details. 14 | 15 | You should have received a copy of the GNU Lesser General Public 16 | License along with this library; if not, write to the Free Software 17 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 18 | */ 19 | 20 | #if ARDUINO >= 100 21 | 22 | #ifndef server_h 23 | #define server_h 24 | 25 | #include "Print.h" 26 | 27 | class Server : public Print { 28 | public: 29 | virtual void begin() =0; 30 | }; 31 | 32 | #endif 33 | #endif 34 | -------------------------------------------------------------------------------- /src/firmware/teensy4/WMath.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Part of the Wiring project - http://wiring.org.co 3 | Copyright (c) 2004-06 Hernando Barragan 4 | Modified 13 August 2006, David A. Mellis for Arduino - http://www.arduino.cc/ 5 | 6 | This library is free software; you can redistribute it and/or 7 | modify it under the terms of the GNU Lesser General Public 8 | License as published by the Free Software Foundation; either 9 | version 2.1 of the License, or (at your option) any later version. 10 | 11 | This library is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | Lesser General Public License for more details. 15 | 16 | You should have received a copy of the GNU Lesser General 17 | Public License along with this library; if not, write to the 18 | Free Software Foundation, Inc., 59 Temple Place, Suite 330, 19 | Boston, MA 02111-1307 USA 20 | */ 21 | 22 | #include 23 | 24 | static uint32_t seed; 25 | 26 | void randomSeed(uint32_t newseed) 27 | { 28 | if (newseed > 0) seed = newseed; 29 | } 30 | 31 | void srandom(unsigned int newseed) 32 | { 33 | seed = newseed; 34 | } 35 | 36 | int32_t random(void) 37 | { 38 | int32_t hi, lo, x; 39 | 40 | // the algorithm used in avr-libc 1.6.4 41 | x = seed; 42 | if (x == 0) x = 123459876; 43 | hi = x / 127773; 44 | lo = x % 127773; 45 | x = 16807 * lo - 2836 * hi; 46 | if (x < 0) x += 0x7FFFFFFF; 47 | seed = x; 48 | return x; 49 | } 50 | 51 | uint32_t random(uint32_t howbig) 52 | { 53 | if (howbig == 0) return 0; 54 | return random() % howbig; 55 | } 56 | 57 | int32_t random(int32_t howsmall, int32_t howbig) 58 | { 59 | if (howsmall >= howbig) return howsmall; 60 | int32_t diff = howbig - howsmall; 61 | return random(diff) + howsmall; 62 | } 63 | 64 | unsigned int makeWord(unsigned int w) { return w; } 65 | unsigned int makeWord(unsigned char h, unsigned char l) { return (h << 8) | l; } 66 | 67 | -------------------------------------------------------------------------------- /src/firmware/teensy4/avr/eeprom.h: -------------------------------------------------------------------------------- 1 | /* Simple compatibility headers for AVR code used with ARM chips 2 | * Copyright (c) 2015 Paul Stoffregen 3 | * 4 | * Permission is hereby granted, free of charge, to any person obtaining a copy 5 | * of this software and associated documentation files (the "Software"), to deal 6 | * in the Software without restriction, including without limitation the rights 7 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | * copies of the Software, and to permit persons to whom the Software is 9 | * furnished to do so, subject to the following conditions: 10 | * 11 | * The above copyright notice and this permission notice shall be included in 12 | * all copies or substantial portions of the Software. 13 | * 14 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 20 | * THE SOFTWARE. 21 | */ 22 | 23 | // Guidelines for editing this file: 24 | // https://forum.pjrc.com/threads/34537-Teensy-LC-Increase-EEPROM-Size/page2 25 | 26 | #ifndef _AVR_EEPROM_H_ 27 | #define _AVR_EEPROM_H_ 1 28 | 29 | #include 30 | #include 31 | 32 | #include "avr_functions.h" 33 | 34 | #if defined(ARDUINO_TEENSY40) 35 | #define E2END 0x437 36 | #elif defined(ARDUINO_TEENSY41) 37 | #define E2END 0x10BB 38 | #elif defined(ARDUINO_TEENSY_MICROMOD) 39 | #define E2END 0x10BB 40 | #endif 41 | 42 | #endif 43 | -------------------------------------------------------------------------------- /src/firmware/teensy4/avr/interrupt.h: -------------------------------------------------------------------------------- 1 | // This header file is in the public domain. 2 | -------------------------------------------------------------------------------- /src/firmware/teensy4/avr/io.h: -------------------------------------------------------------------------------- 1 | /* Simple compatibility headers for AVR code used with ARM chips 2 | * Copyright (c) 2015 Paul Stoffregen 3 | * 4 | * Permission is hereby granted, free of charge, to any person obtaining a copy 5 | * of this software and associated documentation files (the "Software"), to deal 6 | * in the Software without restriction, including without limitation the rights 7 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | * copies of the Software, and to permit persons to whom the Software is 9 | * furnished to do so, subject to the following conditions: 10 | * 11 | * The above copyright notice and this permission notice shall be included in 12 | * all copies or substantial portions of the Software. 13 | * 14 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 20 | * THE SOFTWARE. 21 | */ 22 | 23 | #include "../avr_emulation.h" 24 | -------------------------------------------------------------------------------- /src/firmware/teensy4/avr/power.h: -------------------------------------------------------------------------------- 1 | /* Simple compatibility headers for AVR code used with ARM chips 2 | * Copyright (c) 2015 Paul Stoffregen 3 | * 4 | * Permission is hereby granted, free of charge, to any person obtaining a copy 5 | * of this software and associated documentation files (the "Software"), to deal 6 | * in the Software without restriction, including without limitation the rights 7 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | * copies of the Software, and to permit persons to whom the Software is 9 | * furnished to do so, subject to the following conditions: 10 | * 11 | * The above copyright notice and this permission notice shall be included in 12 | * all copies or substantial portions of the Software. 13 | * 14 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 20 | * THE SOFTWARE. 21 | */ 22 | 23 | #ifndef _AVR_POWER_H_ 24 | #define _AVR_POWER_H_ 1 25 | 26 | #endif 27 | -------------------------------------------------------------------------------- /src/firmware/teensy4/avr/sleep.h: -------------------------------------------------------------------------------- 1 | /* Simple compatibility headers for AVR code used with ARM chips 2 | * Copyright (c) 2015 Paul Stoffregen 3 | * 4 | * Permission is hereby granted, free of charge, to any person obtaining a copy 5 | * of this software and associated documentation files (the "Software"), to deal 6 | * in the Software without restriction, including without limitation the rights 7 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | * copies of the Software, and to permit persons to whom the Software is 9 | * furnished to do so, subject to the following conditions: 10 | * 11 | * The above copyright notice and this permission notice shall be included in 12 | * all copies or substantial portions of the Software. 13 | * 14 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 20 | * THE SOFTWARE. 21 | */ 22 | 23 | #ifndef _AVR_SLEEP_H_ 24 | #define _AVR_SLEEP_H_ 1 25 | 26 | #define SLEEP_MODE_IDLE 0 27 | #define SLEEP_MODE_ADC 0 28 | #define SLEEP_MODE_PWR_DOWN 1 29 | #define SLEEP_MODE_PWR_SAVE 1 30 | #define SLEEP_MODE_STANDBY 1 31 | #define SLEEP_MODE_EXT_STANDBY 1 32 | 33 | #define set_sleep_mode(mode) // TODO: actually set the mode... 34 | #define sleep_enable() 35 | #define sleep_disable() 36 | #define sleep_cpu() ({__asm__ volatile("wfi");}) 37 | #define sleep_bod_disable() 38 | #define sleep_mode() ({__asm__ volatile("wfi");}) 39 | 40 | // workaround for early versions of Nordic's BLE library 41 | // EIMSK moved to a dummy byte in avr_emulation... 42 | //#if defined(HAL_ACI_TL_H__) && defined(PLATFORM_H__) 43 | //#define EIMSK uint8_t EIMSKworkaround=0; EIMSKworkaround 44 | //#endif 45 | 46 | #endif 47 | -------------------------------------------------------------------------------- /src/firmware/teensy4/avr/wdt.h: -------------------------------------------------------------------------------- 1 | // This header file is in the public domain. 2 | 3 | #ifndef _AVR_WDT_H_ 4 | #define _AVR_WDT_H_ 5 | 6 | #define WDTO_15MS 0 7 | #define WDTO_30MS 1 8 | #define WDTO_60MS 2 9 | #define WDTO_120MS 3 10 | #define WDTO_250MS 4 11 | #define WDTO_500MS 5 12 | #define WDTO_1S 6 13 | #define WDTO_2S 7 14 | #define WDTO_4S 8 15 | #define WDTO_8S 9 16 | 17 | #define wdt_reset() 18 | #define wdt_enable(timeout) 19 | #define wdt_disable() 20 | 21 | #endif 22 | -------------------------------------------------------------------------------- /src/firmware/teensy4/core_id.h: -------------------------------------------------------------------------------- 1 | // This header file is in the public domain. 2 | 3 | #ifndef CORE_TEENSY 4 | #define CORE_TEENSY 5 | #endif 6 | -------------------------------------------------------------------------------- /src/firmware/teensy4/debug/printf.h: -------------------------------------------------------------------------------- 1 | // uncommenting the line below will enable the debug printf statements in cores\teensy4 2 | // by default it will print to the Serial4 TX pin at baud rate of 115200 3 | //#define PRINT_DEBUG_STUFF 4 | 5 | // uncommenting the line below will switch to doing outputs to USB Serial or SEREMU instead of Serial4 6 | // However some of the earlier print statements that happen before USB is initialized will be lost 7 | // if you need those outputs, better to use Serial 4. 8 | //#define PRINT_DEBUG_USING_USB // if both defined will try to direct stuff out USB Serial or SEREMU 9 | 10 | #ifdef PRINT_DEBUG_STUFF 11 | // defining printf this way breaks things like Serial.printf() in C++ :( 12 | #define printf(...) printf_debug(__VA_ARGS__) 13 | #ifdef __cplusplus 14 | extern "C" { 15 | #endif 16 | void printf_debug_init(void); 17 | void printf_debug(const char *format, ...); 18 | #ifdef __cplusplus 19 | } 20 | #endif 21 | 22 | #else 23 | #define printf_init() 24 | #define printf(...) 25 | #define printf_debug_init() 26 | 27 | #endif 28 | -------------------------------------------------------------------------------- /src/firmware/teensy4/extmem.c: -------------------------------------------------------------------------------- 1 | // External memory allocation functions. Attempt to use external memory, 2 | // but automatically fall back to internal RAM if external RAM can't be used. 3 | 4 | #include 5 | #include "smalloc.h" 6 | #include "wiring.h" 7 | 8 | #if defined(ARDUINO_TEENSY41) 9 | // Teensy 4.1 external RAM address range is 0x70000000 to 0x7FFFFFFF 10 | #define HAS_EXTRAM 11 | #define IS_EXTMEM(addr) (((uint32_t)ptr >> 28) == 7) 12 | #endif 13 | 14 | 15 | void *extmem_malloc(size_t size) 16 | { 17 | #ifdef HAS_EXTRAM 18 | void *ptr = sm_malloc_pool(&extmem_smalloc_pool, size); 19 | if (ptr) return ptr; 20 | #endif 21 | return malloc(size); 22 | } 23 | 24 | void extmem_free(void *ptr) 25 | { 26 | #ifdef HAS_EXTRAM 27 | if (IS_EXTMEM(ptr)) { 28 | sm_free_pool(&extmem_smalloc_pool, ptr); 29 | return; 30 | } 31 | #endif 32 | free(ptr); 33 | } 34 | 35 | void *extmem_calloc(size_t nmemb, size_t size) 36 | { 37 | return extmem_malloc(nmemb * size); 38 | } 39 | 40 | void *extmem_realloc(void *ptr, size_t size) 41 | { 42 | #ifdef HAS_EXTRAM 43 | if (IS_EXTMEM(ptr)) { 44 | return sm_realloc_pool(&extmem_smalloc_pool, ptr, size); 45 | } 46 | #endif 47 | return realloc(ptr, size); 48 | } 49 | 50 | 51 | -------------------------------------------------------------------------------- /src/firmware/teensy4/fuse.c: -------------------------------------------------------------------------------- 1 | #include "core_pins.h" 2 | #include "imxrt.h" 3 | 4 | uint32_t IMXRTfuseRead(volatile uint32_t *fuses) 5 | { 6 | if (((uint32_t)fuses & 0x0F) != 0) return 0; // illegal address 7 | uint32_t addr = ((uint32_t)fuses - (uint32_t)&HW_OCOTP_LOCK) >> 4; 8 | if (addr > 0x2F) return 0; // illegal address 9 | if (HW_OCOTP_CTRL & HW_OCOTP_CTRL_ERROR) { 10 | HW_OCOTP_CTRL_CLR = HW_OCOTP_CTRL_ERROR; 11 | } 12 | HW_OCOTP_CTRL = HW_OCOTP_CTRL_ADDR(addr); 13 | HW_OCOTP_READ_CTRL = HW_OCOTP_READ_CTRL_READ_FUSE; 14 | while (HW_OCOTP_CTRL & HW_OCOTP_CTRL_BUSY) ; // wait 15 | return HW_OCOTP_READ_FUSE_DATA; 16 | } 17 | 18 | void IMXRTfuseWrite(volatile uint32_t *fuses, uint32_t value) 19 | { 20 | if (((uint32_t)fuses & 0x0F) != 0) return; // illegal address 21 | uint32_t oldvalue = IMXRTfuseRead(fuses); 22 | if ((oldvalue | value) == oldvalue) return; // no write needed 23 | uint32_t addr = ((uint32_t)fuses - (uint32_t)&HW_OCOTP_LOCK) >> 4; 24 | if (addr > 0x2F) return; // illegal address 25 | if (HW_OCOTP_CTRL & HW_OCOTP_CTRL_ERROR) { 26 | HW_OCOTP_CTRL_CLR = HW_OCOTP_CTRL_ERROR; 27 | } 28 | // TODO: abort writing to certain fuses known to brick Lockable Teensy 29 | HW_OCOTP_CTRL = HW_OCOTP_CTRL_WR_UNLOCK(0x3E77) | HW_OCOTP_CTRL_ADDR(addr); 30 | HW_OCOTP_DATA = value; 31 | while (HW_OCOTP_CTRL & HW_OCOTP_CTRL_BUSY) ; // wait 32 | delayMicroseconds(2); 33 | if (HW_OCOTP_CTRL & HW_OCOTP_CTRL_ERROR) return; 34 | *fuses = IMXRTfuseRead(fuses); // update shadow register 35 | } 36 | 37 | void IMXRTfuseReload() 38 | { 39 | if (HW_OCOTP_CTRL & HW_OCOTP_CTRL_ERROR) { 40 | HW_OCOTP_CTRL_CLR = HW_OCOTP_CTRL_ERROR; 41 | } 42 | HW_OCOTP_CTRL = HW_OCOTP_CTRL_RELOAD_SHADOWS; 43 | while (HW_OCOTP_CTRL & (HW_OCOTP_CTRL_BUSY | HW_OCOTP_CTRL_RELOAD_SHADOWS)) ; 44 | } 45 | -------------------------------------------------------------------------------- /src/firmware/teensy4/libc.c: -------------------------------------------------------------------------------- 1 | // this file no longer used 2 | -------------------------------------------------------------------------------- /src/firmware/teensy4/math_helper.h: -------------------------------------------------------------------------------- 1 | /* ---------------------------------------------------------------------- 2 | * Copyright (C) 2010 ARM Limited. All rights reserved. 3 | * 4 | * $Date: 29. November 2010 5 | * $Revision: V1.0.3 6 | * 7 | * Project: CMSIS DSP Library 8 | * 9 | * Title: math_helper.h 10 | * 11 | * 12 | * Description: Prototypes of all helper functions required. 13 | * 14 | * Target Processor: Cortex-M4/Cortex-M3 15 | * 16 | * Version 1.0.3 2010/11/29 17 | * Re-organized the CMSIS folders and updated documentation. 18 | * 19 | * Version 1.0.2 2010/11/11 20 | * Documentation updated. 21 | * 22 | * Version 1.0.1 2010/10/05 23 | * Production release and review comments incorporated. 24 | * 25 | * Version 1.0.0 2010/09/20 26 | * Production release and review comments incorporated. 27 | * 28 | * Version 0.0.7 2010/06/10 29 | * Misra-C changes done 30 | * -------------------------------------------------------------------- */ 31 | 32 | 33 | #include "arm_math.h" 34 | 35 | #ifndef MATH_HELPER_H 36 | #define MATH_HELPER_H 37 | 38 | #ifdef __cplusplus 39 | extern "C" 40 | { 41 | #endif 42 | 43 | float arm_snr_f32(float *pRef, float *pTest, uint32_t buffSize); 44 | void arm_float_to_q12_20(float *pIn, q31_t * pOut, uint32_t numSamples); 45 | void arm_provide_guard_bits_q15(q15_t *input_buf, uint32_t blockSize, uint32_t guard_bits); 46 | void arm_provide_guard_bits_q31(q31_t *input_buf, uint32_t blockSize, uint32_t guard_bits); 47 | void arm_float_to_q14(float *pIn, q15_t *pOut, uint32_t numSamples); 48 | void arm_float_to_q29(float *pIn, q31_t *pOut, uint32_t numSamples); 49 | void arm_float_to_q28(float *pIn, q31_t *pOut, uint32_t numSamples); 50 | void arm_float_to_q30(float *pIn, q31_t *pOut, uint32_t numSamples); 51 | void arm_clip_f32(float *pIn, uint32_t numSamples); 52 | uint32_t arm_calc_guard_bits(uint32_t num_adds); 53 | void arm_apply_guard_bits (float32_t * pIn, uint32_t numSamples, uint32_t guard_bits); 54 | uint32_t arm_compare_fixed_q15(q15_t *pIn, q15_t * pOut, uint32_t numSamples); 55 | uint32_t arm_compare_fixed_q31(q31_t *pIn, q31_t *pOut, uint32_t numSamples); 56 | uint32_t arm_calc_2pow(uint32_t guard_bits); 57 | 58 | #ifdef __cplusplus 59 | } 60 | #endif 61 | 62 | #endif 63 | 64 | -------------------------------------------------------------------------------- /src/firmware/teensy4/new.cpp: -------------------------------------------------------------------------------- 1 | /* Teensyduino Core Library 2 | * http://www.pjrc.com/teensy/ 3 | * Copyright (c) 2018 PJRC.COM, LLC. 4 | * 5 | * Permission is hereby granted, free of charge, to any person obtaining 6 | * a copy of this software and associated documentation files (the 7 | * "Software"), to deal in the Software without restriction, including 8 | * without limitation the rights to use, copy, modify, merge, publish, 9 | * distribute, sublicense, and/or sell copies of the Software, and to 10 | * permit persons to whom the Software is furnished to do so, subject to 11 | * the following conditions: 12 | * 13 | * 1. The above copyright notice and this permission notice shall be 14 | * included in all copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 17 | * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 18 | * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 19 | * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS 20 | * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN 21 | * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 22 | * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | * SOFTWARE. 24 | */ 25 | 26 | #include 27 | 28 | void * operator new(size_t size) 29 | { 30 | return malloc(size); 31 | } 32 | 33 | void * operator new[](size_t size) 34 | { 35 | return malloc(size); 36 | } 37 | 38 | void operator delete(void * ptr) 39 | { 40 | free(ptr); 41 | } 42 | 43 | void operator delete[](void * ptr) 44 | { 45 | free(ptr); 46 | } 47 | 48 | void operator delete(void * ptr, size_t size) 49 | { 50 | free(ptr); 51 | } 52 | 53 | void operator delete[](void * ptr, size_t size) 54 | { 55 | free(ptr); 56 | } 57 | 58 | -------------------------------------------------------------------------------- /src/firmware/teensy4/pgmspace.h: -------------------------------------------------------------------------------- 1 | // For compatibility with some ESP8266 programs 2 | #include 3 | -------------------------------------------------------------------------------- /src/firmware/teensy4/serialEvent.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | void serialEvent() __attribute__((weak)); 4 | void serialEvent() { 5 | } 6 | uint8_t _serialEvent_default PROGMEM = 1; 7 | -------------------------------------------------------------------------------- /src/firmware/teensy4/serialEvent1.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include "HardwareSerial.h" 4 | void serialEvent1() __attribute__((weak)); 5 | void serialEvent1() {} // No use calling this so disable if called... 6 | uint8_t _serialEvent1_default PROGMEM = 1; 7 | -------------------------------------------------------------------------------- /src/firmware/teensy4/serialEvent2.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include "HardwareSerial.h" 4 | void serialEvent2() __attribute__((weak)); 5 | void serialEvent2() {} // No use calling this so disable if called... 6 | uint8_t _serialEvent2_default PROGMEM = 1; 7 | -------------------------------------------------------------------------------- /src/firmware/teensy4/serialEvent3.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include "HardwareSerial.h" 4 | void serialEvent3() __attribute__((weak)); 5 | void serialEvent3() {} // No use calling this so disable if called... 6 | uint8_t _serialEvent3_default PROGMEM = 1; 7 | -------------------------------------------------------------------------------- /src/firmware/teensy4/serialEvent4.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include "HardwareSerial.h" 4 | void serialEvent4() __attribute__((weak)); 5 | void serialEvent4() {} // No use calling this so disable if called... 6 | uint8_t _serialEvent4_default PROGMEM = 1; 7 | -------------------------------------------------------------------------------- /src/firmware/teensy4/serialEvent5.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include "HardwareSerial.h" 4 | void serialEvent5() __attribute__((weak)); 5 | void serialEvent5() {} // No use calling this so disable if called... 6 | uint8_t _serialEvent5_default PROGMEM = 1; 7 | -------------------------------------------------------------------------------- /src/firmware/teensy4/serialEvent6.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include "HardwareSerial.h" 4 | void serialEvent6() __attribute__((weak)); 5 | void serialEvent6() {} // No use calling this so disable if called... 6 | uint8_t _serialEvent6_default PROGMEM = 1; 7 | -------------------------------------------------------------------------------- /src/firmware/teensy4/serialEvent7.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include "HardwareSerial.h" 4 | void serialEvent7() __attribute__((weak)); 5 | void serialEvent7() {} // No use calling this so disable if called... 6 | uint8_t _serialEvent7_default PROGMEM = 1; 7 | -------------------------------------------------------------------------------- /src/firmware/teensy4/serialEvent8.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include "HardwareSerial.h" 4 | void serialEvent8() __attribute__((weak)); 5 | void serialEvent8() {} // No use calling this so disable if called... 6 | uint8_t _serialEvent8_default PROGMEM = 1; 7 | -------------------------------------------------------------------------------- /src/firmware/teensy4/serialEventUSB1.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | void serialEventUSB1() __attribute__((weak)); 4 | void serialEventUSB1() {} 5 | uint8_t _serialEventUSB1_default PROGMEM = 1; 6 | -------------------------------------------------------------------------------- /src/firmware/teensy4/serialEventUSB2.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | void serialEventUSB2() __attribute__((weak)); 4 | void serialEventUSB2() {} 5 | uint8_t _serialEventUSB2_default PROGMEM = 1; 6 | -------------------------------------------------------------------------------- /src/firmware/teensy4/sm_alloc_valid.c: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is a part of SMalloc. 3 | * SMalloc is MIT licensed. 4 | * Copyright (c) 2017 Andrey Rys. 5 | */ 6 | 7 | #include "smalloc_i.h" 8 | 9 | int sm_alloc_valid_pool(struct smalloc_pool *spool, const void *p) 10 | { 11 | struct smalloc_hdr *shdr; 12 | 13 | if (!smalloc_verify_pool(spool)) { 14 | errno = EINVAL; 15 | return 0; 16 | } 17 | 18 | if (!p) return 0; 19 | 20 | shdr = USER_TO_HEADER(p); 21 | if (smalloc_is_alloc(spool, shdr)) return 1; 22 | return 0; 23 | } 24 | 25 | int sm_alloc_valid(const void *p) 26 | { 27 | return sm_alloc_valid_pool(&smalloc_curr_pool, p); 28 | } 29 | -------------------------------------------------------------------------------- /src/firmware/teensy4/sm_calloc.c: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is a part of SMalloc. 3 | * SMalloc is MIT licensed. 4 | * Copyright (c) 2017 Andrey Rys. 5 | */ 6 | 7 | #include "smalloc_i.h" 8 | 9 | void *sm_calloc_pool(struct smalloc_pool *spool, size_t x, size_t y) 10 | { 11 | return sm_zalloc_pool(spool, x * y); 12 | } 13 | 14 | void *sm_calloc(size_t x, size_t y) 15 | { 16 | return sm_calloc_pool(&smalloc_curr_pool, x, y); 17 | } 18 | -------------------------------------------------------------------------------- /src/firmware/teensy4/sm_free.c: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is a part of SMalloc. 3 | * SMalloc is MIT licensed. 4 | * Copyright (c) 2017 Andrey Rys. 5 | */ 6 | 7 | #include "smalloc_i.h" 8 | 9 | void sm_free_pool(struct smalloc_pool *spool, void *p) 10 | { 11 | struct smalloc_hdr *shdr; 12 | char *s; 13 | 14 | if (!smalloc_verify_pool(spool)) { 15 | errno = EINVAL; 16 | return; 17 | } 18 | 19 | if (!p) return; 20 | 21 | shdr = USER_TO_HEADER(p); 22 | if (smalloc_is_alloc(spool, shdr)) { 23 | if (spool->do_zero) memset(p, 0, shdr->rsz); 24 | s = CHAR_PTR(p); 25 | s += shdr->usz; 26 | memset(s, 0, HEADER_SZ); 27 | if (spool->do_zero) memset(s+HEADER_SZ, 0, shdr->rsz - shdr->usz); 28 | memset(shdr, 0, HEADER_SZ); 29 | return; 30 | } 31 | 32 | smalloc_UB(spool, p); 33 | return; 34 | } 35 | 36 | void sm_free(void *p) 37 | { 38 | sm_free_pool(&smalloc_curr_pool, p); 39 | } 40 | -------------------------------------------------------------------------------- /src/firmware/teensy4/sm_hash.c: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is a part of SMalloc. 3 | * SMalloc is MIT licensed. 4 | * Copyright (c) 2017 Andrey Rys. 5 | */ 6 | 7 | #include "smalloc_i.h" 8 | 9 | /* An adopted Jenkins one-at-a-time hash */ 10 | #define UIHOP(x, s) do { \ 11 | hash += (x >> s) & 0xff;\ 12 | hash += hash << 10; \ 13 | hash ^= hash >> 6; \ 14 | } while (0) 15 | uintptr_t smalloc_uinthash(uintptr_t x) 16 | { 17 | uintptr_t hash = 0; 18 | 19 | UIHOP(x, 0); 20 | UIHOP(x, 8); 21 | UIHOP(x, 16); 22 | UIHOP(x, 24); 23 | 24 | hash += hash << 3; 25 | hash ^= hash >> 11; 26 | hash += hash << 15; 27 | 28 | return hash; 29 | } 30 | #undef UIHOP 31 | 32 | uintptr_t smalloc_mktag(struct smalloc_hdr *shdr) 33 | { 34 | uintptr_t r = smalloc_uinthash(PTR_UINT(shdr)); 35 | r += shdr->rsz; 36 | r = smalloc_uinthash(r); 37 | r += shdr->usz; 38 | r = smalloc_uinthash(r); 39 | return r; 40 | } 41 | -------------------------------------------------------------------------------- /src/firmware/teensy4/sm_malloc_stats.c: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is a part of SMalloc. 3 | * SMalloc is MIT licensed. 4 | * Copyright (c) 2017 Andrey Rys. 5 | */ 6 | 7 | #include "smalloc_i.h" 8 | 9 | int sm_malloc_stats_pool(struct smalloc_pool *spool, size_t *total, size_t *user, size_t *free, int *nr_blocks) 10 | { 11 | struct smalloc_hdr *shdr, *basehdr; 12 | int r = 0; 13 | 14 | if (!smalloc_verify_pool(spool)) { 15 | errno = EINVAL; 16 | return -1; 17 | } 18 | 19 | if (!total && !user && !free && !nr_blocks) return 0; 20 | 21 | if (total) *total = 0; 22 | if (user) *user = 0; 23 | if (free) *free = 0; 24 | if (nr_blocks) *nr_blocks = 0; 25 | 26 | shdr = basehdr = spool->pool; 27 | while (CHAR_PTR(shdr)-CHAR_PTR(basehdr) < spool->pool_size) { 28 | if (smalloc_is_alloc(spool, shdr)) { 29 | if (total) *total += HEADER_SZ + shdr->rsz + HEADER_SZ; 30 | if (user) *user += shdr->usz; 31 | if (nr_blocks) *nr_blocks += 1; 32 | r = 1; 33 | } 34 | 35 | shdr++; 36 | } 37 | 38 | *free = spool->pool_size - *total; 39 | 40 | return r; 41 | } 42 | 43 | int sm_malloc_stats(size_t *total, size_t *user, size_t *free, int *nr_blocks) 44 | { 45 | return sm_malloc_stats_pool(&smalloc_curr_pool, total, user, free, nr_blocks); 46 | } 47 | -------------------------------------------------------------------------------- /src/firmware/teensy4/sm_pool.c: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is a part of SMalloc. 3 | * SMalloc is MIT licensed. 4 | * Copyright (c) 2017 Andrey Rys. 5 | */ 6 | 7 | #include "smalloc_i.h" 8 | 9 | struct smalloc_pool smalloc_curr_pool; 10 | 11 | int smalloc_verify_pool(struct smalloc_pool *spool) 12 | { 13 | if (!spool->pool || !spool->pool_size) return 0; 14 | if (spool->pool_size % HEADER_SZ) return 0; 15 | return 1; 16 | } 17 | 18 | int sm_align_pool(struct smalloc_pool *spool) 19 | { 20 | size_t x; 21 | 22 | if (smalloc_verify_pool(spool)) return 1; 23 | 24 | x = spool->pool_size % HEADER_SZ; 25 | if (x) spool->pool_size -= x; 26 | if (spool->pool_size <= MIN_POOL_SZ) { 27 | errno = ENOSPC; 28 | return 0; 29 | } 30 | 31 | return 1; 32 | } 33 | 34 | int sm_set_pool(struct smalloc_pool *spool, void *new_pool, size_t new_pool_size, int do_zero, smalloc_oom_handler oom_handler) 35 | { 36 | if (!spool) { 37 | errno = EINVAL; 38 | return 0; 39 | } 40 | 41 | if (!new_pool || !new_pool_size) { 42 | if (smalloc_verify_pool(spool)) { 43 | if (spool->do_zero) memset(spool->pool, 0, spool->pool_size); 44 | memset(spool, 0, sizeof(struct smalloc_pool)); 45 | return 1; 46 | } 47 | 48 | errno = EINVAL; 49 | return 0; 50 | } 51 | 52 | spool->pool = new_pool; 53 | spool->pool_size = new_pool_size; 54 | spool->oomfn = oom_handler; 55 | if (!sm_align_pool(spool)) return 0; 56 | 57 | if (do_zero) { 58 | spool->do_zero = do_zero; 59 | memset(spool->pool, 0, spool->pool_size); 60 | } 61 | 62 | return 1; 63 | } 64 | 65 | int sm_set_default_pool(void *new_pool, size_t new_pool_size, int do_zero, smalloc_oom_handler oom_handler) 66 | { 67 | return sm_set_pool(&smalloc_curr_pool, new_pool, new_pool_size, do_zero, oom_handler); 68 | } 69 | 70 | int sm_release_pool(struct smalloc_pool *spool) 71 | { 72 | return sm_set_pool(spool, NULL, 0, 0, NULL); 73 | } 74 | 75 | int sm_release_default_pool(void) 76 | { 77 | return sm_release_pool(&smalloc_curr_pool); 78 | } 79 | -------------------------------------------------------------------------------- /src/firmware/teensy4/sm_realloc.c: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is a part of SMalloc. 3 | * SMalloc is MIT licensed. 4 | * Copyright (c) 2017 Andrey Rys. 5 | */ 6 | 7 | #include "smalloc_i.h" 8 | 9 | void *sm_realloc_pool(struct smalloc_pool *spool, void *p, size_t n) 10 | { 11 | return sm_realloc_pool_i(spool, p, n, 0); 12 | } 13 | 14 | void *sm_realloc(void *p, size_t n) 15 | { 16 | return sm_realloc_pool_i(&smalloc_curr_pool, p, n, 0); 17 | } 18 | -------------------------------------------------------------------------------- /src/firmware/teensy4/sm_realloc_move.c: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is a part of SMalloc. 3 | * SMalloc is MIT licensed. 4 | * Copyright (c) 2017 Andrey Rys. 5 | */ 6 | 7 | #include "smalloc_i.h" 8 | 9 | void *sm_realloc_move_pool(struct smalloc_pool *spool, void *p, size_t n) 10 | { 11 | return sm_realloc_pool_i(spool, p, n, 1); 12 | } 13 | 14 | void *sm_realloc_move(void *p, size_t n) 15 | { 16 | return sm_realloc_pool_i(&smalloc_curr_pool, p, n, 1); 17 | } 18 | -------------------------------------------------------------------------------- /src/firmware/teensy4/sm_szalloc.c: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is a part of SMalloc. 3 | * SMalloc is MIT licensed. 4 | * Copyright (c) 2017 Andrey Rys. 5 | */ 6 | 7 | #include "smalloc_i.h" 8 | 9 | size_t sm_szalloc_pool(struct smalloc_pool *spool, const void *p) 10 | { 11 | struct smalloc_hdr *shdr; 12 | 13 | if (!smalloc_verify_pool(spool)) { 14 | errno = EINVAL; 15 | return ((size_t)-1); 16 | } 17 | 18 | if (!p) return 0; 19 | 20 | shdr = USER_TO_HEADER(p); 21 | if (smalloc_is_alloc(spool, shdr)) return shdr->usz; 22 | smalloc_UB(spool, p); 23 | return 0; 24 | } 25 | 26 | size_t sm_szalloc(const void *p) 27 | { 28 | return sm_szalloc_pool(&smalloc_curr_pool, p); 29 | } 30 | -------------------------------------------------------------------------------- /src/firmware/teensy4/sm_util.c: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is a part of SMalloc. 3 | * SMalloc is MIT licensed. 4 | * Copyright (c) 2017 Andrey Rys. 5 | */ 6 | 7 | #include "smalloc_i.h" 8 | 9 | static int smalloc_check_bounds(struct smalloc_pool *spool, struct smalloc_hdr *shdr) 10 | { 11 | if (!spool) return 0; 12 | if (CHAR_PTR(shdr) >= CHAR_PTR(spool->pool) 13 | && CHAR_PTR(shdr) <= (CHAR_PTR(spool->pool)+spool->pool_size)) 14 | return 1; 15 | return 0; 16 | } 17 | 18 | static int smalloc_valid_tag(struct smalloc_hdr *shdr) 19 | { 20 | char *s; 21 | uintptr_t r = smalloc_mktag(shdr); 22 | size_t x; 23 | 24 | if (shdr->tag == r) { 25 | s = CHAR_PTR(HEADER_TO_USER(shdr)); 26 | s += shdr->usz; 27 | for (x = 0; x < sizeof(struct smalloc_hdr); x += sizeof(uintptr_t)) { 28 | r = smalloc_uinthash(r); 29 | if (memcmp(s+x, &r, sizeof(uintptr_t)) != 0) return 0; 30 | } 31 | s += x; x = 0; 32 | while (x < shdr->rsz - shdr->usz) { 33 | if (s[x] != '\xFF') return 0; 34 | x++; 35 | } 36 | return 1; 37 | } 38 | return 0; 39 | } 40 | 41 | static void smalloc_do_crash(struct smalloc_pool *spool, const void *p) 42 | { 43 | char *c = NULL; 44 | *c = 'X'; 45 | } 46 | 47 | smalloc_ub_handler smalloc_UB = smalloc_do_crash; 48 | 49 | void sm_set_ub_handler(smalloc_ub_handler handler) 50 | { 51 | if (!handler) smalloc_UB = smalloc_do_crash; 52 | else smalloc_UB = handler; 53 | } 54 | 55 | int smalloc_is_alloc(struct smalloc_pool *spool, struct smalloc_hdr *shdr) 56 | { 57 | if (!smalloc_check_bounds(spool, shdr)) return 0; 58 | if (shdr->rsz == 0) return 0; 59 | if (shdr->rsz > SIZE_MAX) return 0; 60 | if (shdr->usz > SIZE_MAX) return 0; 61 | if (shdr->usz > shdr->rsz) return 0; 62 | if (shdr->rsz % HEADER_SZ) return 0; 63 | if (!smalloc_valid_tag(shdr)) return 0; 64 | return 1; 65 | } 66 | -------------------------------------------------------------------------------- /src/firmware/teensy4/sm_zalloc.c: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is a part of SMalloc. 3 | * SMalloc is MIT licensed. 4 | * Copyright (c) 2017 Andrey Rys. 5 | */ 6 | 7 | #include "smalloc_i.h" 8 | 9 | void *sm_zalloc_pool(struct smalloc_pool *spool, size_t n) 10 | { 11 | void *r = sm_malloc_pool(spool, n); 12 | if (r) memset(r, 0, n); 13 | return r; 14 | } 15 | 16 | void *sm_zalloc(size_t n) 17 | { 18 | return sm_zalloc_pool(&smalloc_curr_pool, n); 19 | } 20 | -------------------------------------------------------------------------------- /src/firmware/teensy4/smalloc_i.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file is a part of SMalloc. 3 | * SMalloc is MIT licensed. 4 | * Copyright (c) 2017 Andrey Rys. 5 | */ 6 | 7 | #ifndef _SMALLOC_I_H 8 | #define _SMALLOC_I_H 9 | 10 | #include "smalloc.h" 11 | #include 12 | #include 13 | #include 14 | 15 | #ifdef __cplusplus 16 | extern "C" { 17 | #endif 18 | 19 | struct smalloc_hdr { 20 | size_t rsz; /* real allocated size with overhead (if any) */ 21 | size_t usz; /* exact user size as reported by s_szalloc */ 22 | uintptr_t tag; /* sum of all the above, hashed value */ 23 | }; 24 | 25 | #define HEADER_SZ (sizeof(struct smalloc_hdr)) 26 | #define MIN_POOL_SZ (HEADER_SZ*20) 27 | 28 | #define VOID_PTR(p) ((void *)p) 29 | #define CHAR_PTR(p) ((char *)p) 30 | #define PTR_UINT(p) ((uintptr_t)VOID_PTR(p)) 31 | #define HEADER_PTR(p) ((struct smalloc_hdr *)p) 32 | #define USER_TO_HEADER(p) (HEADER_PTR((CHAR_PTR(p)-HEADER_SZ))) 33 | #define HEADER_TO_USER(p) (VOID_PTR((CHAR_PTR(p)+HEADER_SZ))) 34 | 35 | extern smalloc_ub_handler smalloc_UB; 36 | 37 | uintptr_t smalloc_uinthash(uintptr_t x); 38 | uintptr_t smalloc_mktag(struct smalloc_hdr *shdr); 39 | int smalloc_verify_pool(struct smalloc_pool *spool); 40 | int smalloc_is_alloc(struct smalloc_pool *spool, struct smalloc_hdr *shdr); 41 | 42 | void *sm_realloc_pool_i(struct smalloc_pool *spool, void *p, size_t n, int nomove); 43 | 44 | #ifdef __cplusplus 45 | } // extern "C" 46 | #endif 47 | 48 | #endif 49 | -------------------------------------------------------------------------------- /src/firmware/teensy4/usb_dev.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "imxrt.h" 3 | 4 | #if !defined(USB_DISABLED) 5 | 6 | typedef struct transfer_struct transfer_t; 7 | struct transfer_struct { 8 | uint32_t next; 9 | volatile uint32_t status; 10 | uint32_t pointer0; 11 | uint32_t pointer1; 12 | uint32_t pointer2; 13 | uint32_t pointer3; 14 | uint32_t pointer4; 15 | uint32_t callback_param; 16 | }; 17 | 18 | #ifdef __cplusplus 19 | extern "C" { 20 | #endif 21 | 22 | void usb_init(void); 23 | void usb_init_serialnumber(void); 24 | 25 | void usb_config_rx(uint32_t ep, uint32_t packet_size, int do_zlp, void (*cb)(transfer_t *)); 26 | void usb_config_tx(uint32_t ep, uint32_t packet_size, int do_zlp, void (*cb)(transfer_t *)); 27 | void usb_config_rx_iso(uint32_t ep, uint32_t packet_size, int mult, void (*cb)(transfer_t *)); 28 | void usb_config_tx_iso(uint32_t ep, uint32_t packet_size, int mult, void (*cb)(transfer_t *)); 29 | 30 | void usb_prepare_transfer(transfer_t *transfer, const void *data, uint32_t len, uint32_t param); 31 | void usb_transmit(int endpoint_number, transfer_t *transfer); 32 | void usb_receive(int endpoint_number, transfer_t *transfer); 33 | uint32_t usb_transfer_status(const transfer_t *transfer); 34 | 35 | void usb_start_sof_interrupts(int interface); 36 | void usb_stop_sof_interrupts(int interface); 37 | 38 | extern void (*usb_timer0_callback)(void); 39 | extern void (*usb_timer1_callback)(void); 40 | 41 | #ifdef __cplusplus 42 | } 43 | #endif 44 | 45 | #else // !defined(USB_DISABLED) 46 | 47 | #ifdef __cplusplus 48 | extern "C" { 49 | #endif 50 | 51 | void usb_init(void); 52 | 53 | #ifdef __cplusplus 54 | } 55 | #endif 56 | 57 | 58 | #endif // !defined(USB_DISABLED) 59 | -------------------------------------------------------------------------------- /src/firmware/teensy4/usb_names.h: -------------------------------------------------------------------------------- 1 | /* Teensyduino Core Library 2 | * http://www.pjrc.com/teensy/ 3 | * Copyright (c) 2017 PJRC.COM, LLC. 4 | * 5 | * Permission is hereby granted, free of charge, to any person obtaining 6 | * a copy of this software and associated documentation files (the 7 | * "Software"), to deal in the Software without restriction, including 8 | * without limitation the rights to use, copy, modify, merge, publish, 9 | * distribute, sublicense, and/or sell copies of the Software, and to 10 | * permit persons to whom the Software is furnished to do so, subject to 11 | * the following conditions: 12 | * 13 | * 1. The above copyright notice and this permission notice shall be 14 | * included in all copies or substantial portions of the Software. 15 | * 16 | * 2. If the Software is incorporated into a build system that allows 17 | * selection among a list of target devices, then similar target 18 | * devices manufactured by PJRC.COM must be included in the list of 19 | * target devices and selectable in the same manner. 20 | * 21 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 22 | * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 23 | * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 24 | * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS 25 | * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN 26 | * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 27 | * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 28 | * SOFTWARE. 29 | */ 30 | 31 | #ifndef _usb_names_h_ 32 | #define _usb_names_h_ 33 | 34 | // These definitions are intended to allow users to override the default 35 | // USB manufacturer, product and serial number strings. 36 | 37 | #include 38 | 39 | #ifdef __cplusplus 40 | extern "C" { 41 | #endif 42 | 43 | struct usb_string_descriptor_struct { 44 | uint8_t bLength; 45 | uint8_t bDescriptorType; 46 | uint16_t wString[]; 47 | }; 48 | 49 | extern struct usb_string_descriptor_struct usb_string_manufacturer_name; 50 | extern struct usb_string_descriptor_struct usb_string_product_name; 51 | extern struct usb_string_descriptor_struct usb_string_serial_number; 52 | 53 | #ifdef __cplusplus 54 | } 55 | #endif 56 | 57 | #endif 58 | -------------------------------------------------------------------------------- /src/firmware/teensy4/usb_rawhid.h: -------------------------------------------------------------------------------- 1 | /* Teensyduino Core Library 2 | * http://www.pjrc.com/teensy/ 3 | * Copyright (c) 2017 PJRC.COM, LLC. 4 | * 5 | * Permission is hereby granted, free of charge, to any person obtaining 6 | * a copy of this software and associated documentation files (the 7 | * "Software"), to deal in the Software without restriction, including 8 | * without limitation the rights to use, copy, modify, merge, publish, 9 | * distribute, sublicense, and/or sell copies of the Software, and to 10 | * permit persons to whom the Software is furnished to do so, subject to 11 | * the following conditions: 12 | * 13 | * 1. The above copyright notice and this permission notice shall be 14 | * included in all copies or substantial portions of the Software. 15 | * 16 | * 2. If the Software is incorporated into a build system that allows 17 | * selection among a list of target devices, then similar target 18 | * devices manufactured by PJRC.COM must be included in the list of 19 | * target devices and selectable in the same manner. 20 | * 21 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 22 | * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 23 | * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 24 | * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS 25 | * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN 26 | * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 27 | * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 28 | * SOFTWARE. 29 | */ 30 | 31 | #pragma once 32 | 33 | #include "usb_desc.h" 34 | 35 | #if defined(RAWHID_INTERFACE) 36 | 37 | #include 38 | 39 | // C language implementation 40 | #ifdef __cplusplus 41 | extern "C" { 42 | #endif 43 | void usb_rawhid_configure(void); 44 | int usb_rawhid_recv(void *buffer, uint32_t timeout); 45 | int usb_rawhid_available(void); 46 | int usb_rawhid_send(const void *buffer, uint32_t timeout); 47 | #ifdef __cplusplus 48 | } 49 | #endif 50 | 51 | 52 | // C++ interface 53 | #ifdef __cplusplus 54 | class usb_rawhid_class 55 | { 56 | public: 57 | int available(void) {return usb_rawhid_available(); } 58 | int recv(void *buffer, uint16_t timeout) { return usb_rawhid_recv(buffer, timeout); } 59 | int send(const void *buffer, uint16_t timeout) { return usb_rawhid_send(buffer, timeout); } 60 | }; 61 | 62 | extern usb_rawhid_class RawHID; 63 | 64 | #endif // __cplusplus 65 | 66 | #endif // RAWHID_INTERFACE 67 | -------------------------------------------------------------------------------- /src/firmware/teensy4/util/delay.h: -------------------------------------------------------------------------------- 1 | /* Teensyduino Core Library 2 | * http://www.pjrc.com/teensy/ 3 | * Copyright (c) 2016 PJRC.COM, LLC. 4 | * 5 | * Permission is hereby granted, free of charge, to any person obtaining 6 | * a copy of this software and associated documentation files (the 7 | * "Software"), to deal in the Software without restriction, including 8 | * without limitation the rights to use, copy, modify, merge, publish, 9 | * distribute, sublicense, and/or sell copies of the Software, and to 10 | * permit persons to whom the Software is furnished to do so, subject to 11 | * the following conditions: 12 | * 13 | * 1. The above copyright notice and this permission notice shall be 14 | * included in all copies or substantial portions of the Software. 15 | * 16 | * 2. If the Software is incorporated into a build system that allows 17 | * selection among a list of target devices, then similar target 18 | * devices manufactured by PJRC.COM must be included in the list of 19 | * target devices and selectable in the same manner. 20 | * 21 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 22 | * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 23 | * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 24 | * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS 25 | * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN 26 | * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 27 | * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 28 | * SOFTWARE. 29 | */ 30 | 31 | #ifndef _delay_us 32 | #define _delay_us(n) delayMicroseconds(n) 33 | #endif 34 | 35 | #ifndef _delay_ms 36 | #define _delay_ms(n) delay(n) 37 | #endif 38 | -------------------------------------------------------------------------------- /src/firmware/teensy4/util/parity.h: -------------------------------------------------------------------------------- 1 | /* Teensyduino Core Library 2 | * http://www.pjrc.com/teensy/ 3 | * Copyright (c) 2016 PJRC.COM, LLC. 4 | * 5 | * Permission is hereby granted, free of charge, to any person obtaining 6 | * a copy of this software and associated documentation files (the 7 | * "Software"), to deal in the Software without restriction, including 8 | * without limitation the rights to use, copy, modify, merge, publish, 9 | * distribute, sublicense, and/or sell copies of the Software, and to 10 | * permit persons to whom the Software is furnished to do so, subject to 11 | * the following conditions: 12 | * 13 | * 1. The above copyright notice and this permission notice shall be 14 | * included in all copies or substantial portions of the Software. 15 | * 16 | * 2. If the Software is incorporated into a build system that allows 17 | * selection among a list of target devices, then similar target 18 | * devices manufactured by PJRC.COM must be included in the list of 19 | * target devices and selectable in the same manner. 20 | * 21 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 22 | * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 23 | * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 24 | * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS 25 | * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN 26 | * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 27 | * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 28 | * SOFTWARE. 29 | */ 30 | 31 | #ifndef _UTIL_PARITY_H_ 32 | #define _UTIL_PARITY_H_ 33 | 34 | static inline uint8_t parity_even_bit(uint8_t x) __attribute__((pure, always_inline, unused)); 35 | static inline uint8_t parity_even_bit(uint8_t x) 36 | { 37 | x ^= x >> 1; 38 | x ^= x >> 2; 39 | x ^= x >> 4; 40 | return x & 1; 41 | } 42 | 43 | #endif 44 | -------------------------------------------------------------------------------- /src/firmware/teensy4/wiring_private.h: -------------------------------------------------------------------------------- 1 | // empty header file, here for libraries which try to include it (eg, Adafruit_GFX) 2 | -------------------------------------------------------------------------------- /src/system_identification/camera_calibration.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | import time 3 | import numpy as np 4 | from scipy import linalg as la 5 | import matplotlib.pyplot as plt 6 | 7 | import rospy 8 | from std_msgs.msg import Float64MultiArray 9 | 10 | gain = 0 11 | set_point = 0 12 | control_active = False 13 | control_out = np.zeros((7)) 14 | 15 | def controller(msg): 16 | if control_active: 17 | output = msg.data[0] 18 | position_ref = msg.data[1] 19 | vel_ref = msg.data[2] 20 | pos_control = (gain * (set_point - msg.data[1])) 21 | control_out[4] = pos_control - (0.25 * (pos_control - control_out[4])) 22 | 23 | def hold_pose(hz, duration): 24 | global gain 25 | global set_point 26 | global control_out 27 | global control_active 28 | 29 | gain = 5000 30 | set_point = np.pi / 3; 31 | rate = rospy.Rate(hz) # 10hz 32 | 33 | pub = rospy.Publisher('control_input', Float64MultiArray, queue_size=10) 34 | 35 | rospy.Subscriber(f"controller_0_report", Float64MultiArray, controller) 36 | 37 | print(f"Calibrator taking robot control: ...") 38 | ctr = 0 39 | control_active = True 40 | while ctr / hz < duration: 41 | 42 | msg = Float64MultiArray() 43 | msg.data = control_out 44 | pub.publish(msg) 45 | rate.sleep() 46 | 47 | if rospy.is_shutdown(): 48 | exit(0) 49 | 50 | ctr += 1 51 | 52 | msg = Float64MultiArray() 53 | msg.data = np.zeros(7) 54 | pub.publish(msg) 55 | rate.sleep() 56 | 57 | control_active = False 58 | 59 | print(f"Calibrator Finished") 60 | 61 | if __name__ == '__main__': 62 | try: 63 | rospy.init_node('camera_calibrator', anonymous=True) 64 | 65 | time.sleep(2) 66 | hz = 100 67 | duration = 5 68 | hold_pose(hz, duration) 69 | 70 | except rospy.ROSInterruptException: 71 | pass -------------------------------------------------------------------------------- /src/system_identification/chassis_identification.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CU-Robotics/buff-code/103f94d388423e3b665714ca481a4295f4198c7a/src/system_identification/chassis_identification.py -------------------------------------------------------------------------------- /src/wrecker/src/test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | 6 | // CAN 7 | FlexCAN_T4 can1; 8 | FlexCAN_T4 can2; 9 | FlexCAN_T4 can3; 10 | CAN_message_t canRecieveMessages[3][11]; 11 | CAN_message_t tempMessage; 12 | int CANTimer = 0; 13 | 14 | c620CAN my_motor; 15 | 16 | uint8_t motor_id = 7, temp_can_num = 1; 17 | elapsedMillis num_millis_elapsed = 0; 18 | 19 | void setup(){ 20 | 21 | can1.begin(); 22 | can2.begin(); 23 | 24 | can1.setBaudRate(1000000); 25 | can2.setBaudRate(1000000); 26 | 27 | my_motor.init(motor_id,temp_can_num); 28 | 29 | 30 | } 31 | 32 | void loop(){ 33 | 34 | if(num_millis_elapsed>50) 35 | { 36 | num_millis_elapsed = 0; 37 | 38 | my_motor.setPower(.05); 39 | 40 | sendC6x0(); 41 | } 42 | 43 | } 44 | 45 | -------------------------------------------------------------------------------- /title-card.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/CU-Robotics/buff-code/103f94d388423e3b665714ca481a4295f4198c7a/title-card.png --------------------------------------------------------------------------------