BMI088 bringup (#839)

* latest bringup code

* fix address, use faster clock after lowering pullup resistor, imu connections rewired, able to get data from bmi088 now!

* seeing reasonable data coming out of gyro and accelerometer

* IMU data is published, visually acc, gyro looks good
- currently seeing IMU rostopic reach ~60HZ

* publish IMU data, updated constants for acc/angular vel

* Joanne Motor Test WIP

* wave test on chain of motors

* Add function for setting min and max motor position limits for all motors. min and max arrays stil need to be updated fully

* Update min and max motor position limits. Still missing neck motors ID 16 & 17

* update bez.yaml and update serial port search from 0 to 10

---------

Co-authored-by: JoanneT8 <[email protected]>
Co-authored-by: Joanne Tan <[email protected]>

soccer_firmware/Core/Inc/BMI088.h

@@ -0,0 +1,102 @@
+#ifndef BMI088_IMU_H
+#define BMI088_IMU_H
+
+#include "stm32f4xx_hal.h"
+#include "main.h"
+
+/* Register defines */
+#define BMI_GYR_I2C_ADDRESS (0x68 << 1)
+#define BMI_ACC_I2C_ADDRESS (0x18 << 1)
+
+#define BMI_ACC_CHIP_ID     0x00
+#define BMI_ACC_DATA      0x12
+#define BMI_TEMP_DATA       0x22
+#define BMI_ACC_CONF      0x40
+#define BMI_ACC_RANGE       0x41
+#define BMI_INT1_IO_CONF      0x53
+#define BMI_INT1_INT2_MAP_DATA  0x58
+#define BMI_ACC_PWR_CONF    0x7C
+#define BMI_ACC_PWR_CTRL    0x7D
+#define BMI_ACC_SOFTRESET     0x7E
+
+#define BMI_GYR_CHIP_ID     0x00
+#define BMI_GYR_DATA      0x02
+#define BMI_GYR_RANGE     0x0F
+#define BMI_GYR_BANDWIDTH   0x10
+#define BMI_GYR_SOFTRESET   0x14
+#define BMI_GYR_INT_CTRL    0x15
+#define BMI_INT3_INT4_IO_CONF 0x16
+#define BMI_INT3_INT4_IO_MAP  0x18
+
+typedef struct {
+
+  /* I2C */
+  I2C_HandleTypeDef *m_i2c_handle;
+//  GPIO_TypeDef    *csAccPinBank;
+//  GPIO_TypeDef    *csGyrPinBank;
+//  uint16_t       csAccPin;
+//  uint16_t       csGyrPin;
+
+  /* DMA */
+  uint8_t readingAcc;
+  uint8_t readingGyr;
+  uint8_t accTxBuf[8];
+  uint8_t gyrTxBuf[7];
+  volatile uint8_t accRxBuf[8];
+  volatile uint8_t gyrRxBuf[7];
+
+  /* Conversion constants (raw to m/s^2 and raw to rad/s) */
+  float accConversion;
+  float gyrConversion;
+
+  /* x-y-z measurements */
+  float acc_mps2[3];
+  float gyr_rps[3];
+
+} BMI088;
+
+/*
+ *
+ * INITIALISATION
+ *
+ */
+uint8_t BMI088_Init(BMI088 *imu, I2C_HandleTypeDef *m_i2c_handle);
+
+/*
+ *
+ * LOW-LEVEL REGISTER FUNCTIONS
+ *
+ */
+uint8_t BMI088_ReadAccRegister(BMI088 *imu, uint8_t regAddr, uint8_t *data);
+uint8_t BMI088_ReadGyrRegister(BMI088 *imu, uint8_t regAddr, uint8_t *data);
+
+uint8_t BMI088_WriteAccRegister(BMI088 *imu, uint8_t regAddr, uint8_t data);
+uint8_t BMI088_WriteGyrRegister(BMI088 *imu, uint8_t regAddr, uint8_t data);
+
+/*
+ *
+ * POLLING
+ *
+ */
+uint8_t BMI088_ReadAccelerometer(BMI088 *imu, I2C_HandleTypeDef *hi2c, int16_t *accX, int16_t *accY, int16_t *accZ);
+uint8_t BMI088_ReadGyroscope(I2C_HandleTypeDef *hi2c, int16_t *gyroX, int16_t *gyroY, int16_t *gyroZ);
+
+/*
+ *
+ * DMA
+ *
+ */
+uint8_t BMI088_ReadAccelerometerDMA(BMI088 *imu);
+void  BMI088_ReadAccelerometerDMA_Complete(BMI088 *imu);
+
+uint8_t BMI088_ReadGyroscopeDMA(BMI088 *imu);
+void  BMI088_ReadGyroscopeDMA_Complete(BMI088 *imu);
+
+/*
+ *
+ * WORKAROUND
+ *
+ */
+void generateClocks2(uint8_t num_clocks, uint8_t send_stop_bits);
+
+#endif

soccer_firmware/Core/Inc/dynamixel_p2.h

@@ -19,6 +19,8 @@ void motor_torque_en_p2(MotorPort *p, uint8_t id, uint8_t val);
 
 void read_motor_id_p2(MotorPort * p);
 void read_motor_present_position_p2(MotorPort * p, uint8_t id);
+void write_min_position_limit_p2(MotorPort *port, uint8_t id, uint32_t limit);
+void write_max_position_limit_p2(MotorPort *port, uint8_t id, uint32_t limit);
 
 void _motor_write_p2(MotorPort *p, uint8_t id, uint16_t addr, uint8_t* data, uint16_t dataLen);
 void _motor_read_p2(MotorPort *p, uint8_t id, uint16_t addr, uint16_t dataLen);