diff --git a/Applications b/Applications index 51cb9ef..829d6c3 160000 --- a/Applications +++ b/Applications @@ -1 +1 @@ -Subproject commit 51cb9ef10a8e3f9d218220c99c8111baa57272c2 +Subproject commit 829d6c321042c7de6f455f2392a99c6b788d70db diff --git a/Old_Src/Old/imu_mpu9250.c b/Old_Src/Old/imu_mpu9250.c deleted file mode 100644 index d94fed2..0000000 --- a/Old_Src/Old/imu_mpu9250.c +++ /dev/null @@ -1,539 +0,0 @@ -// /****************************************************************************** -// Description -// Onboard IMU Utility -// Log -// 12/06/17 Nickel Liang First Draft. Direct copy from sample program. -// 12/07/17 Nickel Liang Functionality Improvements & Dubug -// 12/12/17 Roger Qiu Kalman filter first draft -// 12/28/17 Nickel Liang Bug fix & decode -// *******************************************************************************/ -// -// #include "imu_mpu9250.h" -// -// /* Global Var */ -// imu_t imuChassis; -// // imu_t imuShoot; -// // imu_t imuCamera; -// -// // double p_k_x[2][2] = {{0, 0}, {0, 0}}; //covariance matrix -// // double p_k_y[2][2] = {{0, 0}, {0, 0}}; -// // double p_k_z[2][2] = {{0, 0}, {0, 0}}; -// //float q_bias = 0.0; // gyro bias -// //float a_angle_x = 0.0; // current angle -// -// /* Local Var */ -// // static float dt = 0.023; // sampling interval -// // float q_bias = -0.985; // gyro bias -// // float y_bias = -1.190; -// // float z_bias = -0.007; //yaw bias -// // static float q_angle_x = 0.000002; // angle noise covariance cov(angle, angle) -// // static float q_gyro_x = 0.0065; // gyro noise covariance cov(gyro, gyro) TODO: use static data from y and z axis for more accurate result. -// // static float r_angle = 0.1; // measurement noise covariance -// // const float pi = 3.14159; -// // const float rad_2_deg = 360 / 6.28; -// -// /****************************************************************************** -// Input -// Output -// Description -// Initialize Onboard MPU9250 (Acceleration and Gyro) -// Log -// 02/01/18 Nickel Liang First Draft -// *******************************************************************************/ -// uint8_t RM_IMU_MPU9250_Init(void) { -// uint8_t index = 0; -// uint8_t MPU9250_Init_Data[10][2] = { -// {MPU9250_PWR_MGMT_1, 0x80}, // Reset Device -// {MPU9250_PWR_MGMT_1, 0x03}, // Clock Source - Gyro-Z -// {MPU9250_PWR_MGMT_2, 0x00}, // Enable Acc & Gyro -// {MPU9250_CONFIG, 0x02}, // LPF 92Hz -// {MPU9250_GYRO_CONFIG, 0x18}, // 0x00 = 250dps / factor 131, 0x08 = 500dps / 65.5, 0x10 = 1000dps / 32.8, 0x18 = 2000dps / 16.4 -// {MPU9250_ACCEL_CONFIG, 0x10}, // 0x00 = 2g / 16384, 0x08 = 4g / 8192, 0x10 = 8g / 4096, 0x18 = 16g / 2048 -// {MPU9250_ACCEL_CONFIG_2, 0x02}, // enable LowPassFilter Set Acc LPF 92Hz 7.8ms Delay -// {MPU9250_USER_CTRL, 0x00}, // Enable AUX -// }; -// -// imuChassis.id = RM_MPU9250_Read_Reg(MPU9250_WHO_AM_I); //read id of device,check if MPU9250 or not -// -// for(index = 0; index < 10; index++) { -// RM_MPU9250_Write_Reg(MPU9250_Init_Data[index][0], MPU9250_Init_Data[index][1]); -// HAL_Delay(1); -// } -// RM_IMU_MPU9250_GetData(); -// // RM_MPU9250_UPDATE_ACC_ANGLE(); -// // imuChassis.corrected_angle_data.init_acc_x = imuChassis.corrected_angle_data.acc_x; -// // imuChassis.corrected_angle_data.init_acc_y = imuChassis.corrected_angle_data.acc_y; -// // imuChassis.corrected_angle_data.init_acc_z = imuChassis.corrected_angle_data.acc_z; -// // imuChassis.corrected_angle_data.x = imuChassis.corrected_angle_data.init_acc_x; -// // imuChassis.corrected_angle_data.y = imuChassis.corrected_angle_data.init_acc_y; -// // imuChassis.corrected_angle_data.z = imuChassis.corrected_angle_data.init_acc_z; -// return 0; -// } -// -// /****************************************************************************** -// Input -// Output -// Description -// Get 6 Axis Data from MPU 6500 -// Log -// 12/07/17 Nickel Liang First Draft -// 12/27/17 Nickel Liang Move mpu_buff -// 12/28/17 Nickel Liang Decode -// *******************************************************************************/ -// void RM_IMU_MPU9250_GetData(void) { -// uint8_t mpu_buff[14]; -// // Must read 14 all together -// RM_MPU9250_Read_Regs(MPU9250_ACCEL_XOUT_H, mpu_buff, 14); -// -// float acceFactor = 4096.0; // Check datasheet, 8g = 4096 -// imuChassis.acce.rx = (int16_t)(mpu_buff[0]<<8 | mpu_buff[1]); -// imuChassis.acce.ry = (int16_t)(mpu_buff[2]<<8 | mpu_buff[3]); -// imuChassis.acce.rz = (int16_t)(mpu_buff[4]<<8 | mpu_buff[5]); -// imuChassis.acce.x = (float)(imuChassis.acce.rx/acceFactor); -// imuChassis.acce.y = (float)(imuChassis.acce.ry/acceFactor); -// imuChassis.acce.z = (float)(imuChassis.acce.rz/acceFactor); -// -// float roomTempOffset = 0; // 21 C -// float tempSensitivity = 333.87; // Datasheet p12 -// imuChassis.rtemp = (int16_t)(mpu_buff[6]<<8 | mpu_buff[7]); -// imuChassis.temp = (float)(((imuChassis.rtemp-roomTempOffset)/tempSensitivity) + 21); // Register map p33 -// -// float gyroFactor = 16.4; // Check datasheet, 2000dps = 16.4 -// imuChassis.gyro.rx = (int16_t)(mpu_buff[8]<<8 | mpu_buff[9]); -// imuChassis.gyro.ry = (int16_t)(mpu_buff[10]<<8 | mpu_buff[11]); -// imuChassis.gyro.rz = (int16_t)(mpu_buff[12]<<8 | mpu_buff[13]); -// imuChassis.gyro.x = (float)(imuChassis.gyro.rx/gyroFactor); // Get a value in degree -// imuChassis.gyro.y = (float)(imuChassis.gyro.ry/gyroFactor); -// imuChassis.gyro.z = (float)(imuChassis.gyro.rz/gyroFactor); -// -// // Actually the following code won't work since we need to get all six data from one single read, -// // but we cannot achieve that due to read I2C through SPI limitation. -// // imuChassis.mag.rx = (int16_t)(RM_IST8310_Read_Reg(IST8310_R_XL)<<8 | RM_IST8310_Read_Reg(IST8310_R_XM)); -// // imuChassis.mag.ry = (int16_t)(RM_IST8310_Read_Reg(IST8310_R_YL)<<8 | RM_IST8310_Read_Reg(IST8310_R_YM)); -// // imuChassis.mag.rz = (int16_t)(RM_IST8310_Read_Reg(IST8310_R_ZL)<<8 | RM_IST8310_Read_Reg(IST8310_R_ZM)); -// } -// -// /****************************************************************************** -// Input -// Output -// Description -// Debug utility function -// Log -// 12/27/17 Nickel Liang First Draft -// *******************************************************************************/ -// void RM_DEBUG_IMU9250_PrintRaw(void) { -// RM_IMU_MPU9250_GetData(); -// // RM_IMU_MPU9250_GetData(); -// RM_PRINTF("[ RAW ] ID %02x | ", imuChassis.id); -// RM_PRINTF("Acce X %d \tY %d \tZ %d \t| ", imuChassis.acce.rx, imuChassis.acce.ry, imuChassis.acce.rz); -// RM_PRINTF("Gyro X %d \tY %d \tZ %d \t| ", imuChassis.gyro.rx, imuChassis.gyro.ry, imuChassis.gyro.rz); -// // RM_PRINTF("Magn X %d \tY %d \tZ %d \t| ", imuChassis.mag.rx, imuChassis.mag.ry, imuChassis.mag.rz); -// RM_PRINTF("Temp %d\r\n", imuChassis.rtemp); -// } -// -// /****************************************************************************** -// Input -// Output -// Description -// Debug utility function -// Log -// 12/27/17 Nickel Liang First Draft -// *******************************************************************************/ -// void RM_DEBUG_IMU9250_PrintDecoded(void) { -// RM_IMU_MPU9250_GetData(); -// RM_PRINTF("[DECODED] ID %02x | ", imuChassis.id); -// RM_PRINTF("Acce X %f \tY %f \tZ %f \t| ", imuChassis.acce.x, imuChassis.acce.y, imuChassis.acce.z); -// RM_PRINTF("Gyro X %f \tY %f \tZ %f \t| ", imuChassis.gyro.x, imuChassis.gyro.y, imuChassis.gyro.z); -// RM_PRINTF("Temp %f\r\n", imuChassis.temp); -// } -// -// void RM_DEBUG_IMU9250_PrintKalman(void){ -// RM_PRINTF("[Filtered] ID %02x | ", imuChassis.id); -// RM_PRINTF("Kalman_Gyro X %f \tY %f \tZ %f \t| ", imuChassis.corrected_angle_data.x, imuChassis.corrected_angle_data.y, imuChassis.corrected_angle_data.z); -// RM_PRINTF("Acce X %f \tY %f \tZ %f \t| ", imuChassis.acce.x, imuChassis.acce.y, imuChassis.acce.z); -// RM_PRINTF("Gyro X %f \tY %f \tZ %f \t| ", imuChassis.gyro.x, imuChassis.gyro.y, imuChassis.gyro.z); -// RM_PRINTF("Acc_angle X %f \tY %f \tZ %f \t| ", imuChassis.corrected_angle_data.acc_x, imuChassis.corrected_angle_data.acc_y, imuChassis.corrected_angle_data.acc_z); -// RM_PRINTF("Temp %f\r\n", imuChassis.temp); -// } -// -// /****************************************************************************** -// Input -// Output -// Description -// Update accessible acce data -// Log -// 01/12/18 Roger Qiu First Draft -// 01/14/18 Roger Qiu Bug fix -// *******************************************************************************/ -// // static void RM_MPU9250_UPDATE_ACC_ANGLE(void) { -// // GET_ACC_STATIC_ANGLE(imuChassis.acce.x, imuChassis.acce.y, imuChassis.acce.z, &imuChassis.corrected_angle_data.acc_x, &imuChassis.corrected_angle_data.acc_y, &imuChassis.corrected_angle_data.acc_z); -// // //imuChassis.corrected_angle_data.acc_z = 0; //TODO: change to mag -// // } -// -// /****************************************************************************** -// Input -// - accelerometer Readings -// Output -// Description -// Calculate angles from accelerometer readings -// Log -// 01/12/18 Roger Qiu First Draft -// *******************************************************************************/ -// // static void GET_ACC_STATIC_ANGLE(float grav_x, float grav_y, float grav_z, float *ret_x, float *ret_y, float *ret_z){ -// // *ret_x = atan2(grav_y, grav_z) * rad_2_deg; //roll -// // *ret_y = atan2(grav_x, grav_z) * rad_2_deg; //pitch -// // float grav_scalar = sqrt(grav_x * grav_x + grav_y * grav_y + grav_z * grav_z); -// // /* -// // *ret_x = acos(grav_x / grav_scalar) * rad_2_deg; -// // *ret_y = acos(grav_y / grav_scalar) * rad_2_deg; -// // */ -// // *ret_z = acos(grav_z / grav_scalar) * rad_2_deg; -// // } -// -// /****************************************************************************** -// Input -// Output -// Description -// Need to be regularly called to maintain the accessible data -// Log -// 01/12/18 Roger Qiu First Draft -// *******************************************************************************/ -// // void RM_MPU9250_ANGLE_UPDATE(void){ -// // RM_IMU_MPU9250_GetData(); -// // RM_MPU9250_UPDATE_ACC_ANGLE(); -// // RM_MPU9250_KALMAN_FILTER_UPDATE(imuChassis.gyro.x, imuChassis.corrected_angle_data.acc_x - imuChassis.corrected_angle_data.init_acc_x, &imuChassis.corrected_angle_data.x, p_k_x); -// // RM_MPU9250_KALMAN_FILTER_UPDATE(imuChassis.gyro.y, imuChassis.corrected_angle_data.acc_y - imuChassis.corrected_angle_data.init_acc_y, &imuChassis.corrected_angle_data.y, p_k_y); -// // RM_MPU9250_DISCRETE_INTEGRAL(imuChassis.gyro.z, &imuChassis.corrected_angle_data.z); -// // //RM_MPU9250_KALMAN_FILTER_UPDATE(imuChassis.gyro.z, imuChassis.corrected_angle_data.acc_z, &imuChassis.corrected_angle_data.z, p_k_z); -// // } -// // -// // static void RM_MPU9250_DISCRETE_INTEGRAL(const float gyro_k, float *angle_holder){ -// // float rate = gyro_k - z_bias; -// // *angle_holder = *angle_holder + dt * rate; -// // } -// -// /****************************************************************************** -// Input -// - gyro_k: angular velocity from gyro. -// - acc_angle: angle measured by accelerometer -// - *a_angle: pointer to a axis -// - *p_k: pointer to covariance matrix of an axis -// Description -// Calculate Kalman filter adjusted angle -// Log -// 12/12/17 Roger Qiu First Draft -// 01/12/18 Roger Qiu Bug Fix; parameter updated; it's now working! -// *******************************************************************************/ -// // static void RM_MPU9250_KALMAN_FILTER_UPDATE(const float gyro_k, const float acc_angle, float *a_angle, double p_k[2][2]){ //imuChassis.gyro.x, acc_angle, desired_ang -// // float rate = gyro_k - q_bias; -// // float p_cache[4] = {0, 0, 0, 0}; -// // //double p_k[2][2] = *p_to_mat; -// // //EQ 1 - A priori state estimate -// // //ideal estimated angle -// // *a_angle += rate * dt; //a priori estimate -// // //EQ 2 - A priori estimate covariance -// // //p_k_k-1 (estimate from past observations) -// // p_cache[0] = q_angle_x - p_k[0][1] - p_k[1][0] + dt * p_k[1][1]; -// // p_cache[1] = -1 * p_k[1][1]; -// // p_cache[2] = -1 * p_k[1][1]; -// // p_cache[3] = q_gyro_x; -// // p_k[0][0] += p_cache[0] * dt; // dt^2 << 0. Ignore. -// // p_k[0][1] += p_cache[1] * dt; -// // p_k[1][0] += p_cache[2] * dt; -// // p_k[1][1] += p_cache[3] * dt; -// // //EQ 3 - Optimal Kalman Gain -// // float angle_err = acc_angle - *a_angle; -// // float E = r_angle + p_k[0][0]; -// // float k_0 = p_k[0][0] / E; -// // float k_1 = p_k[1][0] / E; -// // //EQ 4 (and 5) - A posteriori estimate covariance (update original covariance matrix) -// // /* -// // p_k[1][0] = k_1 * p_k[0][0]; -// // p_k[1][1] = k_1 * p_k[0][1]; -// // p_k[0][0] = k_0 * p_k[0][0]; -// // p_k[0][1] = k_0 * p_k[0][1]; -// // */ -// // p_k[0][0] -= k_0 * p_k[0][0]; -// // p_k[0][1] -= k_0 * p_k[0][1]; -// // p_k[1][0] -= k_1 * p_k[0][0]; -// // p_k[1][1] -= k_1 * p_k[0][1]; -// // //calculate adjusted angle -// // *a_angle += k_0 * angle_err; -// // q_bias += k_1 * angle_err; -// // //return *a_angle; -// // } -// -// /****************************************************************************** -// Input -// Output -// Description -// Pull NSS Pin Low. Select MPU9250. -// Log -// 12/07/17 Nickel Liang First Draft -// *******************************************************************************/ -// static void RM_MPU9250_NSS_Low(void) { -// HAL_GPIO_WritePin(GPIOE, GPIO_PIN_4, GPIO_PIN_RESET); -// } -// -// /****************************************************************************** -// Input -// Output -// Description -// Pull NSS Pin High. Invert Select MPU9250. -// Log -// 12/07/17 Nickel Liang First Draft -// *******************************************************************************/ -// static void RM_MPU9250_NSS_High(void) { -// HAL_GPIO_WritePin(GPIOE, GPIO_PIN_4, GPIO_PIN_SET); -// } -// -// /****************************************************************************** -// Input -// reg register to be written -// data data to be written -// Output -// 0 -// Description -// Write a register on MPU9250 -// Log -// 12/07/17 Nickel Liang First Draft -// *******************************************************************************/ -// static uint8_t RM_MPU9250_Write_Reg(uint8_t const reg, uint8_t const data) { -// static uint8_t MPU_Rx, MPU_Tx; -// RM_MPU9250_NSS_Low(); -// MPU_Tx = reg & 0x7f; -// HAL_SPI_TransmitReceive(&hspi4, &MPU_Tx, &MPU_Rx, 1, 55); -// MPU_Tx = data; -// HAL_SPI_TransmitReceive(&hspi4, &MPU_Tx, &MPU_Rx, 1, 55); -// RM_MPU9250_NSS_High(); -// return 0; -// } -// -// /****************************************************************************** -// Input -// reg register to be read -// Output -// Received data -// Description -// Read a register on MPU9250 -// Log -// 12/07/17 Nickel Liang First Draft -// *******************************************************************************/ -// static uint8_t RM_MPU9250_Read_Reg(uint8_t const reg) { -// static uint8_t MPU_Rx, MPU_Tx; -// RM_MPU9250_NSS_Low(); -// MPU_Tx = reg | 0x80; -// HAL_SPI_TransmitReceive(&hspi4, &MPU_Tx, &MPU_Rx, 1, 55); -// HAL_SPI_TransmitReceive(&hspi4, &MPU_Tx, &MPU_Rx, 1, 55); -// RM_MPU9250_NSS_High(); -// return MPU_Rx; -// } -// -// /****************************************************************************** -// Input -// regAddr start addr -// pData received data -// len how many consecutive registers -// Output -// 0 -// Description -// Read registers on MPU9250 -// Log -// 12/07/17 Nickel Liang First Draft -// *******************************************************************************/ -// static uint8_t RM_MPU9250_Read_Regs(uint8_t const regAddr, uint8_t *pData, uint8_t len) { -// static uint8_t MPU_Rx, MPU_Tx, MPU_Tx_buff[14] = {0xff}; -// RM_MPU9250_NSS_Low(); -// MPU_Tx = regAddr | 0x80; -// MPU_Tx_buff[0] = MPU_Tx; -// HAL_SPI_TransmitReceive(&hspi4, &MPU_Tx, &MPU_Rx, 1, 55); -// HAL_SPI_TransmitReceive(&hspi4, MPU_Tx_buff, pData, len, 55); -// RM_MPU9250_NSS_High(); -// return 0; -// } -// -// /****************************************************************************** -// Input -// percent PWM parameter -// Output -// 0 -// Description -// Read registers on MPU9250 -// Log -// 12/07/17 Nickel Liang First Draft -// *******************************************************************************/ -// // static void RM_MPU9250_Heater(uint8_t percent) { -// // -// // } -// -// /****************************************************************************** -// Input -// Output -// 0 - Success -// Other - Error -// Description -// Initialize Onboard IST8310 Magnetic Sensor -// Log -// 12/07/17 Nickel Liang First Draft -// 12/30/17 Nickel Liang Comments and fix -// NOTE -// Do not use IST8310, hardware bug. -// *******************************************************************************/ -// /*uint8_t RM_IMU_IST8310_Init(void) { -// RM_MPU9250_Write_Reg(MPU9250_USER_CTRL, 0x30); // Enable the I2C Master I/F module, Reset I2C Slave module -// HAL_Delay(10); -// RM_MPU9250_Write_Reg(MPU9250_I2C_MST_CTRL, 0x0d); // 400kHz -// HAL_Delay(10); -// -// RM_MPU9250_Write_Reg(MPU9250_I2C_SLV1_ADDR, IST8310_ADDRESS); // Write from slave 1 -// HAL_Delay(10); -// RM_MPU9250_Write_Reg(MPU9250_I2C_SLV4_ADDR, 0x80 | IST8310_ADDRESS); // Read from slave 4 -// HAL_Delay(10); -// -// RM_IST8310_Write_Reg(IST8310_R_CONFB, 0x01); -// if(IST8310_DEVICE_ID_A != RM_IST8310_Read_Reg(IST8310_WHO_AM_I)) -// return 1; //error -// HAL_Delay(10); -// -// RM_IST8310_Write_Reg(IST8310_R_CONFA, 0x00); -// if(RM_IST8310_Read_Reg(IST8310_R_CONFA) != 0x00) -// return 2; -// HAL_Delay(10); -// -// RM_IST8310_Write_Reg(IST8310_R_CONFB, 0x00); -// if(RM_IST8310_Read_Reg(IST8310_R_CONFB) != 0x00) -// return 3; -// HAL_Delay(10); -// -// RM_IST8310_Write_Reg(IST8310_AVGCNTL, 0x24); -// if(RM_IST8310_Read_Reg(IST8310_AVGCNTL) != 0x24) -// return 4; -// HAL_Delay(10); -// -// RM_IST8310_Write_Reg(IST8310_PDCNTL, 0xc0); -// if(RM_IST8310_Read_Reg(IST8310_PDCNTL) != 0xc0) -// return 5; -// HAL_Delay(10); -// -// RM_MPU9250_Write_Reg(MPU9250_I2C_SLV1_CTRL, 0x00); -// HAL_Delay(10); -// RM_MPU9250_Write_Reg(MPU9250_I2C_SLV4_CTRL, 0x00); -// HAL_Delay(10); -// -// RM_MPU_Read_IST_Init(IST8310_ADDRESS, IST8310_R_XL, 0x06); -// HAL_Delay(100); -// return 0; -// } -// */ -// -// /****************************************************************************** -// Input -// addr register to be written -// data data to be written -// Output -// 0 -// Description -// Write a IST8310 register through MPU9250 -// Log -// 12/07/17 Nickel Liang First Draft -// *******************************************************************************/ -// /*static uint8_t RM_IST8310_Write_Reg(uint8_t addr, uint8_t data) { -// RM_MPU9250_Write_Reg(MPU9250_I2C_SLV1_CTRL, 0x00); -// HAL_Delay(2); -// RM_MPU9250_Write_Reg(MPU9250_I2C_SLV1_REG, addr); -// HAL_Delay(2); -// RM_MPU9250_Write_Reg(MPU9250_I2C_SLV1_DO, data); -// HAL_Delay(2); -// -// RM_MPU9250_Write_Reg(MPU9250_I2C_SLV1_CTRL, 0x080 | 0x01); -// HAL_Delay(10); -// return 0; -// } -// */ -// -// /****************************************************************************** -// Input -// addr register to be written -// Output -// received data -// Description -// Read a IST8310 register through MPU9250 -// Log -// 12/07/17 Nickel Liang First Draft -// *******************************************************************************/ -// /*static uint8_t RM_IST8310_Read_Reg(uint8_t addr) { -// RM_MPU9250_Write_Reg(MPU9250_I2C_SLV4_REG, addr); -// HAL_Delay(10); -// RM_MPU9250_Write_Reg(MPU9250_I2C_SLV4_CTRL, 0x80); -// HAL_Delay(10); -// uint8_t data = RM_MPU9250_Read_Reg(MPU9250_I2C_SLV4_DI); -// RM_MPU9250_Write_Reg(MPU9250_I2C_SLV4_CTRL, 0x00); -// HAL_Delay(10); -// return data; -// } -// */ -// -// /****************************************************************************** -// Input -// device_address -// reg_base_addr -// data_num number of data -// Output -// Description -// Configure MPU9250 to read data from IST8310 -// Initialize the MPU9250 I2C Slave0 for I2C reading -// Log -// 12/07/17 Nickel Liang First Draft -// *******************************************************************************/ -// /*static void RM_MPU_Read_IST_Init(uint8_t device_address, uint8_t reg_base_addr, uint8_t data_num) { -// RM_MPU9250_Write_Reg(MPU9250_I2C_SLV1_ADDR, device_address); -// HAL_Delay(2); -// RM_MPU9250_Write_Reg(MPU9250_I2C_SLV1_REG, IST8310_R_CONFA); -// HAL_Delay(2); -// RM_MPU9250_Write_Reg(MPU9250_I2C_SLV1_DO, IST8310_ODR_MODE); -// HAL_Delay(2); -// -// RM_MPU9250_Write_Reg(MPU9250_I2C_SLV0_ADDR, 0x80 | device_address); -// HAL_Delay(2); -// RM_MPU9250_Write_Reg(MPU9250_I2C_SLV0_REG, reg_base_addr); -// HAL_Delay(2); -// -// RM_MPU9250_Write_Reg(MPU9250_I2C_SLV4_CTRL, 0x03); -// HAL_Delay(2); -// -// RM_MPU9250_Write_Reg(MPU9250_I2C_MST_DELAY_CTRL, 0x01 | 0x02); -// HAL_Delay(2); -// -// RM_MPU9250_Write_Reg(MPU9250_I2C_SLV1_CTRL, 0x80 | 0x01); -// HAL_Delay(6); -// -// RM_MPU9250_Write_Reg(MPU9250_I2C_SLV0_CTRL, 0x80 | data_num); -// HAL_Delay(7); -// } -// */ -// -// /****************************************************************************** -// Input -// Output -// Description -// Set MPU9250 Acceleration data resolution -// Log -// 12/07/17 Nickel Liang First Draft -// *******************************************************************************/ -// /*static uint8_t RM_MPU9250_Set_Accel_Fsr(uint8_t fsr) { -// return RM_MPU9250_Write_Reg(MPU9250_ACCEL_CONFIG, fsr<<3); -// } -// */ -// -// /****************************************************************************** -// Input -// Output -// Description -// Set MPU9250 Gyro data resolution -// Log -// 12/07/17 Nickel Liang First Draft -// *******************************************************************************/ -// /*static uint8_t RM_MPU9250_Set_Gyro_Fsr(uint8_t fsr) { -// return RM_MPU9250_Write_Reg(MPU9250_GYRO_CONFIG, fsr<<3); -// } -// */ diff --git a/Old_Src/Old/imu_mpu9250.h b/Old_Src/Old/imu_mpu9250.h deleted file mode 100644 index ae12459..0000000 --- a/Old_Src/Old/imu_mpu9250.h +++ /dev/null @@ -1,190 +0,0 @@ -// /****************************************************************************** -// Description -// Seperate MPU9250 IMU Utility -// Log -// 02/01/17 Nickel Liang First Draft -// *******************************************************************************/ -// -// #ifndef __IMU_MPU9250 -// #define __IMU_MPU9250 -// -// #include "imu_onboard.h" -// #include "mytype.h" -// #include "math.h" -// -// -// /* Register Table of MPU9250 */ -// typedef enum { -// MPU9250_SELF_TEST_XG = 0x00, -// MPU9250_SELF_TEST_YG = 0x01, -// MPU9250_SELF_TEST_ZG = 0x02, -// MPU9250_SELF_TEST_XA = 0x0D, -// MPU9250_SELF_TEST_YA = 0x0E, -// MPU9250_SELF_TEST_ZA = 0x0F, -// MPU9250_XG_OFFSET_H = 0x13, -// MPU9250_XG_OFFSET_L = 0x14, -// MPU9250_YG_OFFSET_H = 0x15, -// MPU9250_YG_OFFSET_L = 0x16, -// MPU9250_ZG_OFFSET_H = 0x17, -// MPU9250_ZG_OFFSET_L = 0x18, -// MPU9250_SMPLRT_DIV = 0x19, -// MPU9250_CONFIG = 0x1A, -// MPU9250_GYRO_CONFIG = 0x1B, -// MPU9250_ACCEL_CONFIG = 0x1C, -// MPU9250_ACCEL_CONFIG_2 = 0x1D, -// MPU9250_LP_ACCEL_ODR = 0x1E, -// MPU9250_WOM_THR = 0x1F, -// MPU9250_FIFO_EN = 0x23, -// MPU9250_I2C_MST_CTRL = 0x24, -// MPU9250_I2C_SLV0_ADDR = 0x25, -// MPU9250_I2C_SLV0_REG = 0x26, -// MPU9250_I2C_SLV0_CTRL = 0x27, -// MPU9250_I2C_SLV1_ADDR = 0x28, -// MPU9250_I2C_SLV1_REG = 0x29, -// MPU9250_I2C_SLV1_CTRL = 0x2A, -// MPU9250_I2C_SLV2_ADDR = 0x2B, -// MPU9250_I2C_SLV2_REG = 0x2C, -// MPU9250_I2C_SLV2_CTRL = 0x2D, -// MPU9250_I2C_SLV3_ADDR = 0x2E, -// MPU9250_I2C_SLV3_REG = 0x2F, -// MPU9250_I2C_SLV3_CTRL = 0x30, -// MPU9250_I2C_SLV4_ADDR = 0x31, -// MPU9250_I2C_SLV4_REG = 0x32, -// MPU9250_I2C_SLV4_DO = 0x33, -// MPU9250_I2C_SLV4_CTRL = 0x34, -// MPU9250_I2C_SLV4_DI = 0x35, -// MPU9250_I2C_MST_STATUS = 0x36, -// MPU9250_INT_PIN_CFG = 0x37, -// MPU9250_INT_ENABLE = 0x38, -// MPU9250_INT_STATUS = 0x3A, -// MPU9250_ACCEL_XOUT_H = 0x3B, -// MPU9250_ACCEL_XOUT_L = 0x3C, -// MPU9250_ACCEL_YOUT_H = 0x3D, -// MPU9250_ACCEL_YOUT_L = 0x3E, -// MPU9250_ACCEL_ZOUT_H = 0x3F, -// MPU9250_ACCEL_ZOUT_L = 0x40, -// MPU9250_TEMP_OUT_H = 0x41, -// MPU9250_TEMP_OUT_L = 0x42, -// MPU9250_GYRO_XOUT_H = 0x43, -// MPU9250_GYRO_XOUT_L = 0x44, -// MPU9250_GYRO_YOUT_H = 0x45, -// MPU9250_GYRO_YOUT_L = 0x46, -// MPU9250_GYRO_ZOUT_H = 0x47, -// MPU9250_GYRO_ZOUT_L = 0x48, -// MPU9250_EXT_SENS_DATA_00 = 0x49, -// MPU9250_EXT_SENS_DATA_01 = 0x4A, -// MPU9250_EXT_SENS_DATA_02 = 0x4B, -// MPU9250_EXT_SENS_DATA_03 = 0x4C, -// MPU9250_EXT_SENS_DATA_04 = 0x4D, -// MPU9250_EXT_SENS_DATA_05 = 0x4E, -// MPU9250_EXT_SENS_DATA_06 = 0x4F, -// MPU9250_EXT_SENS_DATA_07 = 0x50, -// MPU9250_EXT_SENS_DATA_08 = 0x51, -// MPU9250_EXT_SENS_DATA_09 = 0x52, -// MPU9250_EXT_SENS_DATA_10 = 0x53, -// MPU9250_EXT_SENS_DATA_11 = 0x54, -// MPU9250_EXT_SENS_DATA_12 = 0x55, -// MPU9250_EXT_SENS_DATA_13 = 0x56, -// MPU9250_EXT_SENS_DATA_14 = 0x57, -// MPU9250_EXT_SENS_DATA_15 = 0x58, -// MPU9250_EXT_SENS_DATA_16 = 0x59, -// MPU9250_EXT_SENS_DATA_17 = 0x5A, -// MPU9250_EXT_SENS_DATA_18 = 0x5B, -// MPU9250_EXT_SENS_DATA_19 = 0x5C, -// MPU9250_EXT_SENS_DATA_20 = 0x5D, -// MPU9250_EXT_SENS_DATA_21 = 0x5E, -// MPU9250_EXT_SENS_DATA_22 = 0x5F, -// MPU9250_EXT_SENS_DATA_23 = 0x60, -// MPU9250_I2C_SLV0_DO = 0x63, -// MPU9250_I2C_SLV1_DO = 0x64, -// MPU9250_I2C_SLV2_DO = 0x65, -// MPU9250_I2C_SLV3_DO = 0x66, -// MPU9250_I2C_MST_DELAY_CTRL = 0x67, -// MPU9250_SIGNAL_PATH_RESET = 0x68, -// MPU9250_MOT_DETECT_CTRL = 0x69, -// MPU9250_USER_CTRL = 0x6A, -// MPU9250_PWR_MGMT_1 = 0x6B, -// MPU9250_PWR_MGMT_2 = 0x6C, -// MPU9250_FIFO_COUNTH = 0x72, -// MPU9250_FIFO_COUNTL = 0x73, -// MPU9250_FIFO_R_W = 0x74, -// MPU9250_WHO_AM_I = 0x75, -// MPU9250_XA_OFFSET_H = 0x77, -// MPU9250_XA_OFFSET_L = 0x78, -// MPU9250_YA_OFFSET_H = 0x7A, -// MPU9250_YA_OFFSET_L = 0x7B, -// MPU9250_ZA_OFFSET_H = 0x7D, -// MPU9250_ZA_OFFSET_L = 0x7E, -// -// MPU9250_ID = 0x71, -// } RM_MPU9250_REG; -// -// /* Register Table of MPU9250 Magnetometer AK8963 */ -// typedef enum { -// AK8963_WIA = 0x00, // Device ID 0x48 -// AK8963_INFO = 0x01, -// AK8963_ST1 = 0x02, -// AK8963_HXL = 0x03, -// AK8963_HXH = 0x04, -// AK8963_HYL = 0x05, -// AK8963_HYH = 0x06, -// AK8963_HZL = 0x07, -// AK8963_HZH = 0x08, -// AK8963_ST2 = 0x09, -// AK8963_CNTL = 0x0A, -// /* "B0000": Power-down mode -// "B0001": Single measurement mode -// "B0010": Continuous measurement mode 1 -// "B0110": Continuous measurement mode 2 -// "B0100": External trigger measurement mode -// "B1000": Self-test mode -// "B1111": Fuse ROM access mode -// B: "0": 14-bit output -// "1": 16-bit output */ -// AK8963_RSV = 0x0B, // DO NOT USE -// AK8963_ASTC = 0x0C, -// AK8963_TS1 = 0x0D, // DO NOT USE -// AK8963_TS2 = 0x0E, // DO NOT USE -// AK8963_I2CDIS = 0x0F, -// AK8963_ASAX = 0x10, -// AK8963_ASAY = 0x11, -// AK8963_ASAZ = 0x12, -// } RM_AK8963_REG; -// -// -// /* Rx Data Structure */ -// /* This structure definition is in imu_onboard.h */ -// -// /* Declaration */ -// extern imu_t imuChassis; -// // extern imu_t imuShoot; -// // extern imu_t imuCamera; -// -// /* Global Functions */ -// uint8_t RM_IMU_MPU9250_Init(void); -// void RM_IMU_MPU9250_GetData(void); -// // void RM_MPU9250_ANGLE_UPDATE(void); -// // uint8_t RM_IMU_IST8310_Init(void); -// -// /* Static Functions */ -// static void RM_MPU9250_NSS_Low(void); -// // static void RM_MPU9250_DISCRETE_INTEGRAL(const float gyro_k, float *angle_storage); -// static void RM_MPU9250_NSS_High(void); -// // static void RM_MPU9250_UPDATE_ACC_ANGLE(void); -// // static void RM_MPU9250_KALMAN_FILTER_UPDATE(const float gyro_k, const float acc_angle, float *a_angle, double p_k[2][2]); -// // static void GET_ACC_STATIC_ANGLE(float grav_x, float grav_y, float grav_z, float *ret_x, float *ret_y, float *ret_z); -// static uint8_t RM_MPU9250_Write_Reg(uint8_t const reg, uint8_t const data); -// static uint8_t RM_MPU9250_Read_Reg(uint8_t const reg); -// static uint8_t RM_MPU9250_Read_Regs(uint8_t const regAddr, uint8_t *pData, uint8_t len); -// // static uint8_t RM_IST8310_Write_Reg(uint8_t addr, uint8_t data); -// // static uint8_t RM_IST8310_Read_Reg(uint8_t addr); -// // static void RM_MPU_Read_IST_Init(uint8_t device_address, uint8_t reg_base_addr, uint8_t data_num); -// // static uint8_t RM_MPU9250_Set_Accel_Fsr(uint8_t fsr); -// // static uint8_t RM_MPU9250_Set_Gyro_Fsr(uint8_t fsr); -// -// /* Debug Utility Functions */ -// void RM_DEBUG_IMU9250_PrintRaw(void); -// void RM_DEBUG_IMU9250_PrintDecoded(void); -// void RM_DEBUG_IMU9250_PrintKalman(void); -// -// #endif diff --git a/Old_Src/Old/taskChassis.c b/Old_Src/Old/taskChassis.c deleted file mode 100644 index 1e652f4..0000000 --- a/Old_Src/Old/taskChassis.c +++ /dev/null @@ -1,158 +0,0 @@ -#include "taskChassis.h" -#include "mytype.h" -#include - - - -/****************************************************************************** - Input: - Output - Description - chassis task - this task is called by a timer for 10 ms period - Log - 2018/04/08 Ke Sun & Steven Chen -*******************************************************************************/ -void StartChassisTask(void const * argument){ - - curr_ang = imuBoard.corrected_angle_data.z; - desired_ang = curr_ang+PI/4-last_ang; - ang_velocity = curr_ang-last_ang; - forward_back_ref = left_right_ref=rotate_ref=0; - - // Mecanuum physics and linear algebra basis(Mechanical Engineering & MATH415) - RM_Chassis_ControlInstructions(); - set[0] = (forward_back_ref*0.75 - left_right_ref*0.75 - 0.75*rotate_ref)*sin(desired_ang)-1.0*ang_velocity; - set[1] = (-forward_back_ref*0.75 - left_right_ref*0.75 - 0.75*rotate_ref)*cos(desired_ang)+1.0*ang_velocity; - set[2] = (forward_back_ref*0.75 + left_right_ref*0.75 - 0.75*rotate_ref)*cos(desired_ang)+1.0*ang_velocity; - set[3] = (-forward_back_ref*0.75 + left_right_ref*0.75 - 0.75*rotate_ref)*sin(desired_ang)-2.0*ang_velocity; - - //power limit - //power_limit_control(set); - - // Motor PID calculation - for (int i = 0; i < 4; i++) { - pid_calc(&pid_spd[i], motorChassis[i].speedRPM, set[i]); - } - //RM_PRINTF("Start Chassis Task\r\n"); - - RM_CAN_SetMotor(&hcan1, CAN_3508_1_4_TX, (int16_t)(pid_spd[0].pos_out), (int16_t)(pid_spd[1].pos_out), (int16_t)(pid_spd[2].pos_out), (int16_t)(pid_spd[3].pos_out)); - last_ang = curr_ang; - ang_velocity=0; -} -/****************************************************************************** - Input: - Output - Description - RM Chassis Control Instructions - Log - 2018/04/08 Ke Sun -*******************************************************************************/ -void RM_Chassis_ControlInstructions(void){ - // init_ang=curr_ang=last_ang=imuBoard.corrected_angle_data.z; - // Motor PID calculation - // forward motion when pressing W - if(RM_W_Pressed){ - forward_back_ref += 2000; - left_right_ref += 0; - rotate_ref += 0; - } - // backward motion when pressing S - if(RM_S_Pressed){ - forward_back_ref += -2000; - left_right_ref += 0; - rotate_ref += 0; - } - // left motion when pressing A - if(RM_A_Pressed){ - forward_back_ref += 0; - left_right_ref += 2000; - rotate_ref += 0; - } - // right motion when pressing D - if(RM_D_Pressed){ - forward_back_ref += 0; - left_right_ref += -2000; - rotate_ref += 0; - } - if(RM_Q_Pressed){ - rot = true; - - } - if(RM_Z_Pressed){ - rrot = true; - } - if(RM_R_Pressed){ - rot = false; - rrot=false; - } - - // calculate PID for rotation read data from gimbal - pid_calc(&pid_rotation, motorYaw.angle, 7200); - if(rot){ - rotation=-pid_rotation.pos_out; - } - else if(rrot){ - rotation=pid_rotation.pos_out; - } - else{ - rotation=0; - } - rotate_ref += rotation; - // The next few states are high-speed states - // high-speed forward motion when pressing F - if(RM_F_Pressed){ - forward_back_ref += 4000; - left_right_ref += 0; - rotate_ref += 0; - } - - // high-speed backward motion when pressing V - if(RM_V_Pressed){ - forward_back_ref += -4000; - left_right_ref += 0; - rotate_ref += 0; - } - - // high-speed left motion when pressing C - if(RM_C_Pressed){ - forward_back_ref += 0; - left_right_ref += 4000; - rotate_ref += 0; - } - - // high-speed right motion when pressing B - if(RM_B_Pressed){ - forward_back_ref += 0; - left_right_ref += -4000; - rotate_ref += 0; - } -} - - -/****************************************************************************** - Input: speed - Output - Description - limit power on chassis - Log - 2018/04/08 Ke Sun & Yaorui Hao -*******************************************************************************/ -void power_limit_control(double* speed){ - - /*ramp control method (current limiting) based on judgement system - total_cur_limit needs to be defined according to the judgement system*/ - - motor_measure_t motor_measurement; - - double total_cur = motor_measurement.torqueCurrent; - - if (total_cur > total_cur_limit) - { - speed[0] = speed[0] / total_cur * total_cur_limit; - speed[1] = speed[1] / total_cur * total_cur_limit; - speed[2] = speed[2] / total_cur * total_cur_limit; - speed[3] = speed[3] / total_cur * total_cur_limit; - } - -} diff --git a/Old_Src/Old/taskChassis.h b/Old_Src/Old/taskChassis.h deleted file mode 100644 index 979cab6..0000000 --- a/Old_Src/Old/taskChassis.h +++ /dev/null @@ -1,61 +0,0 @@ -#ifndef TASKCHASSIS__H -#define TASKCHASSIS__H - -#include "mytype.h" -#include "chassis.h" -/* chassis control period (ms) */ -#define CHASSIS_PERIOD 10 -#define total_cur_limit 2 /*current value needs to be set*/ -/****************************************************************************** - VARIABLES -*******************************************************************************/ -static float curr_ang; -static float last_ang; -static float desired_ang; -static float ang_velocity; -static int set_v; -static int set_pit; //goal angle for pitch -static int set_yaw; // goal angle for yaw -static int rotation=0; -static bool rot=false; -static bool rrot=false; -static int forward_back_ref,left_right_ref,rotate_ref;// goal velocity for chassis -static double set[4]; - -/* The function achieve remote control on the robot by using ch0, ch1, ch3(may consider switching to ch2) - Outputs may be optimized by futher parametrization - ch0 = horizontal stick on the right - ch1 = vertical stick on the right - ch3 = vertical stick on the left(up = clockwise, down = counterclockwise) - chassis top view: - set_spd[0]-----------set_spd[1] - --- - --- - --- - set_spd[2]-----------set_spd[3] -*/ -void RM_Chassis_ControlInstructions(void); -void power_limit_control(double* speed); -void StartChassisTask(void const * argument); - -#endif -/****************************************************************************** - Description - keyboard Control instructions: - when no key is pressed, chassis does nothing - when W is pressed, chassis moves forward - when S is pressed, chassis moves backward - when A is pressed, chassis moves left - when D is pressed, chassis moves right - when Q is pressed, chassis moves counterclockwise - when R is pressed, chassis moves clockwise - when Z is pressed, chassis keeps going clockwise and counterclockwise in repeated cycles. Press X to stop this action. - when F is pressed, chassis moves forward in high speed - when V is pressed, chassis moves backward in high speed - when C is pressed, chassis moves left with high speed - when B is pressed, chassis moves right with high speed - Log - 12/09/17 Ke Sun, Ruihao Yao First Draft - 12/20/17 Ruihao Yao Second Draft - -*******************************************************************************/ diff --git a/Old_Src/Old/taskGimbal.c b/Old_Src/Old/taskGimbal.c deleted file mode 100644 index 0a0a3f3..0000000 --- a/Old_Src/Old/taskGimbal.c +++ /dev/null @@ -1,100 +0,0 @@ -#include -#include "taskGimbal.h" - -extern osThreadId launcherTaskHandle; -/****************************************************************************** - Input: - Output - Description - gimbal task - this task is called by a timer for 20 ms period - and set signal for launcherTask - Log - 2018/04/08 Ke Sun -*******************************************************************************/ -void StartGimbalTask(void const * argument){ - - //RM_PRINTF("Start Gimbal Task; \r\n"); - // for GPIO - uint8_t TARGET; - uint8_t true_target=4; - - // update key-press - static int key_sta, key_cnt; - switch (key_sta) { - case 0: //no key - if (RM_KEY_Pressed) { // active low for key pressing - key_sta = 1; // if KEY is pressed set next state to case 1 - } - break; - case 1: //key down wait release. - if (RM_KEY_Pressed) { // active low for key pressing - key_sta = 2; // if KEY is pressed set next state to case 2 - key_cnt++; // if KEY is pressed update counter - RM_LED_RedOn(); - } else { - key_sta = 0; // if KEY press is caused by noise, set next state to case 0 - } - break; - case 2: - if (RM_KEY_Released) { - key_sta = 0; // if KEY press is caused by noise, set next state to case 0 - RM_LED_RedOff(); - } - break; - } - if (key_cnt > 9) key_cnt = 0; - - - /* MOTOR RX MESSAGE - For 3508, - DATA[0]DATA[1] = Mechanical Angle in range 0 ~ 8191 - 0 ~ 360 degree - DATA[2]DATA[3] = Rotational Speed in rpm - DATA[4]DATA[5] = Actual torque current - DATA[6] = Motor temperature in Celcius - DATA[7] = NULL - For 6623, - DATA[0]DATA[1] = Mechanical Angle in range 0 ~ 8191 - 0 ~ 360 degree - DATA[2]DATA[3] = Actual torque current in range -13000 ~ 13000 - DATA[4]DATA[5] = Given torque current - DATA[6]DATA[7] = NULL - */ - // update chasis motor and gimbal motor according to the key-press - - // update chasis motor and gimbal motor according to the key-press - - - // when remoteData.rc.s1 change to 1 set mode to auto-shooting - if (remoteData.rc.s1 == 0) { - GimbalPIDSetMode(GIMBAL_AUTO_SHOOT); - - //RM_GPIO_WriteBin(key_cnt); - TARGET = RM_GPIO_ReadBin(); - if (TARGET != 14){ - true_target=TARGET; - } - //GimbalPIDAutoSet(true_target); - - GimbalPIDAutoSet(key_cnt); - } - // when remoteData.rc.s1 change to 0 set mode to mouse imu control on gimbal -// else if (remoteData.rc.s1 == 0) { -// GimbalPIDSetMode(GIMBAL_MOUSE_IMU_SHOOT); -// GimbalPIDMouseIMU(remoteData.mouse.x*20, -30*remoteData.mouse.y); -// } - - // when remoteData.rc.s1 change to 0 set mode to man-shooting with mouse - else if (remoteData.rc.s1 == 1) { - GimbalPIDSetMode(GIMBAL_MAN_SHOOT); - GimbalPIDManSet(remoteData.mouse.x*27, -35*remoteData.mouse.y); - } - // when remoteData.rc.s1 change to 0 set mode to man-shooting with remote controllor -// else if (remoteData.rc.s1 == 0) { -// GimbalPIDSetMode(GIMBAL_MAN_SHOOT); - // GimbalPIDManSet(remoteData.rc.ch0-1024, remoteData.rc.ch1-1024); -// } - osSignalSet(launcherTaskHandle, LAUNCHER_TASK_EXE_SIGNAL); - // RM_PRINTF("(%u, %u)pitch %d;yaw %d \r\n", motorPitch.angle, motorYaw.angle, (int) pid_pit.pos_out, (int) pid_yaw.pos_out); - - -} diff --git a/Old_Src/Old/taskGimbal.h b/Old_Src/Old/taskGimbal.h deleted file mode 100644 index 848a462..0000000 --- a/Old_Src/Old/taskGimbal.h +++ /dev/null @@ -1,9 +0,0 @@ -#ifndef TASKGIMBAL__H -#define TASKGIMBAL__H - -#include "mytype.h" -/* chassis gimbal period (ms) */ -#define GIMBAL_PERIOD 5 -void StartGimbalTask(void const * argument); - -#endif diff --git a/Old_Src/Old/taskIMU.c b/Old_Src/Old/taskIMU.c deleted file mode 100644 index b31265e..0000000 --- a/Old_Src/Old/taskIMU.c +++ /dev/null @@ -1,29 +0,0 @@ -#include "taskIMU.h" -/****************************************************************************** - Input: - Output - Description - IMU task, 20 ms - Log - 2018/04/08 Ke Sun -*******************************************************************************/ -void StartIMUTask(void const * argument) { - uint32_t imu_wake_time = osKernelSysTick(); - while(1){ - //RM_PRINTF("Start IMU Task; \r\n"); - // print onboard imu - // RM_DEBUG_IMU_PrintRaw(); - // RM_DEBUG_IMU_PrintDecoded(); - - RM_MPU6500_ANGLE_UPDATE(); //update angle - - // print imu - //RM_DEBUG_IMU_PrintKalman(); - // print raw magnetometer data - //RM_DEBUG_IMU_PrintRaw(); - //RaiseRequest(); - //HAL_Delay(1000); - - osDelayUntil(&imu_wake_time, 20); - } -} diff --git a/Old_Src/Old/taskIMU.h b/Old_Src/Old/taskIMU.h deleted file mode 100644 index c708f65..0000000 --- a/Old_Src/Old/taskIMU.h +++ /dev/null @@ -1,9 +0,0 @@ -#ifndef TASKIMU__H -#define TASKIMU__H - -#include "mytype.h" -#include "imu_mpu9250.h" - -void StartIMUTask(void const * argument); - -#endif diff --git a/Old_Src/Old/taskLauncher.c b/Old_Src/Old/taskLauncher.c deleted file mode 100644 index 2f8bd3e..0000000 --- a/Old_Src/Old/taskLauncher.c +++ /dev/null @@ -1,23 +0,0 @@ -#include "taskLauncher.h" - -int set_launcher = 469*19; // goal velocity for launcher - -void StartLauncherTask(void const * argument){ - osEvent event; - while (1){ - RM_PRINTF("launcher task\n"); - event = osSignalWait(LAUNCHER_TASK_EXE_SIGNAL, osWaitForever); - - if(event.status == osEventSignal){ - if(event.value.signals & LAUNCHER_TASK_EXE_SIGNAL){ - //RM_PRINTF("Start Launcher Task; \r\n"); - - if(RM_Left_Clicked){ - LauncherShoot(); //Shoot when button is pressed - // Launcher Spinning Wheel Speed (RPM) - } - LauncherPIDSet(set_launcher); - } - } - } -} diff --git a/Old_Src/Old/taskLauncher.h b/Old_Src/Old/taskLauncher.h deleted file mode 100644 index 7d75ae7..0000000 --- a/Old_Src/Old/taskLauncher.h +++ /dev/null @@ -1,8 +0,0 @@ -#ifndef TASKLAUNCHER_H -#define TASKLAUNCHER_H - -#include "mytype.h" - -void StartLauncherTask(void const * argument); - -#endif diff --git a/Old_Src/Old/taskProtocol.c b/Old_Src/Old/taskProtocol.c deleted file mode 100644 index 48a75aa..0000000 --- a/Old_Src/Old/taskProtocol.c +++ /dev/null @@ -1,60 +0,0 @@ -/****************************************************************************** - Description - STM32 TX2 protocol - Log - 12/02/17 Jeff Ma the thread to communicate with protocol -*******************************************************************************/ -#include "taskProtocol.h" - -uint16_t PRO_STATUS; - -void SetProStatusIvalid(void){ - PRO_STATUS = STATUS_DATA_INVALID; -} -void SetProStatusValid(void){ - PRO_STATUS = STATUS_DATA_VALID; -} -uint16_t CheckProStatus(void){ - return PRO_STATUS; -} -/****************************************************************************** - Input: - Output - Description - protocol task - Log - 2018/04/08 Ke Sun -*******************************************************************************/ -void StartProtocolTask(void const *argument) { - uint32_t protocol_wake_time = osKernelSysTick(); - uint8_t f = 0; - PRO_STATUS = STATUS_DATA_VALID; - // for protocol initialiation - protoInit(); - // protocol task formally start now! - //RM_PRINTF("START PROTOCOL TASK!! \n\r"); - // This function should be called from gimbal.c - // called here for debugging - while(1){ - // check if there is a request to process - if ( protoRequest() == REQUEST_SUCC ) { - // if yes, then a request message should already - // be sent to TX2 then should wait for valid data - // to set to the - f=0; - SetProStatusIvalid(); - // here, first set the protocol data's status to be invalid - // the interrupt-mode UART receiver should update the data - // status to be valid again - } - // - if(CheckProStatus()==STATUS_DATA_VALID && f==0) { - RM_PRINTF("%u %u\t", Yaw_data, Pitch_data); - f=1; - // This function should be called from gimbal.c - // called here for debugging - } - // - osDelayUntil(&protocol_wake_time, 20); - } -} diff --git a/Old_Src/Old/taskProtocol.h b/Old_Src/Old/taskProtocol.h deleted file mode 100644 index 745fd51..0000000 --- a/Old_Src/Old/taskProtocol.h +++ /dev/null @@ -1,16 +0,0 @@ -#ifndef TASKPROTO__H -#define TASKPROTO__H - -#include "mytype.h" -#include "protocol.h" - -#define STATUS_DATA_VALID 1 -#define STATUS_DATA_INVALID 0 - -extern uint16_t PRO_STATUS; - -// task declaration -void StartProtocolTask(void const *argument); -void SetProStatusValid(void); -uint16_t CheckProStatus(void); -#endif diff --git a/Old_Src/freertos.c b/Old_Src/freertos.c deleted file mode 100644 index 3994842..0000000 --- a/Old_Src/freertos.c +++ /dev/null @@ -1,157 +0,0 @@ -/****************************************************************************** - Description - FreeRTOS. Original code generated by CubeMX. - Log - 11/23/17 Nickel Liang First Draft - 11/25/17 Yuan Ma - 12/05/17 Nickel Liang Minor improvements - Add LED, Buzzer, Key press - Standard Function Naming: RM_{Peripheral}_{Name} - 12/06/17 Nickel Liang Add DBUS Support - 12/19/17 Ruihao Yao & Ke Sun Add functions to implement chassis control instructions - 01/02/18 Nickel Liang Include lib change - 01/03/18 Nickel Liang Add GPIO test - 01/20/18 Ke Sun & Haoyang Zhang Modified RTOS - 04/09/18 Ke Sun Add comment and function header, osTimer modified -******************************************************************************* - NOTE - Please add log above if you change anything in this file. - This file is expected to change very very frequently, - so please keep tracking what you modified so other people can have a - better idea when they debug their program -*******************************************************************************/ - -/****************************************************************************** - INCLUDES -*******************************************************************************/ -#include -#include -#include "FreeRTOS.h" -#include "task.h" -#include "cmsis_os.h" -/* USER INCLUDES */ -#include "mytype.h" -#include "taskIMU.h" -#include "taskGimbal.h" -#include "taskChassis.h" -#include "taskLauncher.h" -#include "taskProtocol.h" -// -#include -/****************************************************************************** - VARIABLES -*******************************************************************************/ -osThreadId initialTaskHandle; - -osThreadId IMUTaskHandle; -osThreadId launcherTaskHandle; -osThreadId ProrocolaskHandle; - -osTimerId chassis_timer_id; -osTimerId gimbal_timer_id; - -/****************************************************************************** - FUNCTIONS -*******************************************************************************/ -void StartInitialTask(void const * argument); -extern void MX_FATFS_Init(void); -void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */ - - - -/****************************************************************************** - Input: - Output - Description - FREERTOS INIT - Log - 2018/04/08 Ke Sun -*******************************************************************************/ -void MX_FREERTOS_Init(void) { - osTimerDef(ChassisTimer, StartChassisTask); // create chassis timer - chassis_timer_id = osTimerCreate(osTimer(ChassisTimer), osTimerPeriodic, NULL); - if(chassis_timer_id != NULL){ - RM_PRINTF("TIMER CREATE SUCCESSFUL!\r\n"); - } - osTimerDef(gimTimer, StartGimbalTask); // create gimbal timer - gimbal_timer_id = osTimerCreate(osTimer(gimTimer), osTimerPeriodic,NULL); - - /* Create the thread(s) */ - /* definition and creation of initialTask */ - osThreadDef(initialTask, StartInitialTask, osPriorityAboveNormal, 0, 128); - initialTaskHandle = osThreadCreate(osThread(initialTask), NULL); - - /* high priority task */ - osThreadDef(IMUTask, StartIMUTask, osPriorityNormal, 0, 128); - IMUTaskHandle = osThreadCreate(osThread(IMUTask), NULL); - - osThreadDef(ProtocolTask, StartProtocolTask, osPriorityNormal, 0, 128); - ProrocolaskHandle = osThreadCreate(osThread(ProtocolTask), NULL); - - //osThreadDef(chassisTask, StartChassisTask, osPriorityNormal, 0, 128); - //chassisTaskHandle = osThreadCreate(osThread(chassisTask), NULL); - //osThreadDef(gimbalTask, StartGimbalTask, osPriorityNormal, 0, 128); - //gimbalTaskHandle = osThreadCreate(osThread(gimbalTask), NULL); - osThreadDef(launcherTask, StartLauncherTask, osPriorityNormal, 0, 128); - launcherTaskHandle = osThreadCreate(osThread(launcherTask), NULL); - -} - -/****************************************************************************** - Input: - Output - Description - initialization task - this task will be suspended upon completition - Log - 2018/04/08 Ke Sun -*******************************************************************************/ -void StartInitialTask(void const * argument) { - /****************************** - OS INITIALIZATION - ******************************/ - MX_FATFS_Init(); - - /****************************** - PERIPHERALS INITIALIZATION - ******************************/ - RM_BUZZER_Init(); // BUZZER Initialization - RM_DBUS_Init(); // DBUS Initialization - RM_CAN_InitCAN1(); // CAN1 Initialization - RM_CAN_InitCAN2(); // CAN2 Initialization - RM_IMU_Onboard_Init(); // Onboard IMU Initialization - RM_PRINTF("Start Default Task\r\n"); - - /****************************** - TASKS INITIALIZATION - ******************************/ - /* PID Initialize */ - // for chassis - for (int i = 0; i < 4; i++) { - PID_struct_init(&pid_spd[i], POSITION_PID, 10000/2, 3000, 8.0f, 2.0f, 0.0f, 0.0f); //4 motors angular rate closeloop. - } - PID_struct_init(&pid_rotation, CHASSISS_ROTATE, 2000, 2000, 5.0f, 0.0f, 0.0f, 0.0f); //for rotation PID - // for gimbal Init - GimbalPIDSetMode(GIMBAL_AUTO_SHOOT); - GimbalPIDAutoSet(5); - // for launcher - LauncherPIDSetMode(true); - - - /****************************** - SYSTEM IS READY - ******************************/ - RM_LED_RedOff(); - RM_LED_GreenOn(); - RM_LASER_On(); - RM_BUZZER_SingSong(Startup, 1); - RM_PRINTF("System Ready.\r\n"); - osTimerStart(gimbal_timer_id, GIMBAL_PERIOD); - osTimerStart(chassis_timer_id, CHASSIS_PERIOD); - // RM_DEBUG_GPIO_Test(); - - - //create individial tasks - /* */ - osThreadSuspend(initialTaskHandle); -} diff --git a/save_serial.py b/save_serial.py deleted file mode 100644 index c370dcf..0000000 --- a/save_serial.py +++ /dev/null @@ -1,39 +0,0 @@ -#!/usr/bin/env python -import serial -import sys -import os -import time -import sys - -data_dir = "data_history/" -if not os.path.exists(data_dir): - os.makedirs(data_dir) - -device_loc = '/dev/cu.SLAB_USBtoUART' -save_path = data_dir + time.ctime() + '.txt' -connected = False - -try: - print "Trying to connect to " + device_loc - ser = serial.Serial(device_loc, 115200) -except serial.serialutil.SerialException: - print "Fatal error: Device not found" - sys.exit(-1) - -save_stream = open(save_path, "wb") - -while not connected: - data = ser.read() #blocking - connected = True - -try: - while True: - if ser.inWaiting(): - data = ser.read() - sys.stdout.write(data) - sys.stdout.flush() - save_stream.write(data) -except KeyboardInterrupt: - print "Cleaning up..." - save_stream.close() - ser.close()