Implement new controller

Dependencies:   mbed-rtos mbed QEI BNO055 MPU6050_DMP_Nucleo-I2Cdev virgo3_imuHandler_Orion_PCB MAX17048 Servo

Fork of Orion_newPCB_test by Team Virgo v3

Revision:
4:315716ef8178
Parent:
2:761e3c932ce0
Child:
5:099cb2e76c7d
--- a/Configuration/config.h	Mon Jan 18 09:07:39 2016 +0000
+++ b/Configuration/config.h	Wed Jan 20 04:01:37 2016 +0000
@@ -14,32 +14,40 @@
 
 //** Function exectution rates ********************************************************************
 #define Hearbeat_RateHz 10 //hearbeat loop run rate in Hz
+#define Hearbeat_RateMS (1000/Hearbeat_RateHz) //hearbeat loop run rate in ms
+
 #define imu_UpdateRateHz 200 // imu update rate in Hz
+#define imu_UpdatePeriodMS (1000/imu_UpdateRateHz) // imu update rate in ms
+
 #define motorControl_UpdateRateHz 100 // motorControl update rate in Hz
+#define motorControl_UpdatePeriodMS (1000/motorControl_UpdateRateHz) // motorControl update rate in ms
+
 #define odometry_UpdateRateHz 125 //odometry update rate in Hz
+#define odometry_UpdatePeriodMS (1000/odometry_UpdateRateHz) //odometry update rate in ms
+
 #define PrintLoop_RateHz 10 //print loop run rate in Hz
+#define PrintLoop_PeriodMS (1000/PrintLoop_RateHz) //print loop run rate in ms
+
 #define PurePursuit_UpdateRateHz 20//Pure pursuit update run rate in Hz
+#define PurePursuit_UpdatePeriodMS (1000/PurePursuit_UpdateRateHz)//Pure pursuit update run rate in ms
+
 #define WaypointCmd_UpdateRateHz 10 //waypoint commander run rate in Hz
+#define WaypointCmd_UpdatePeriodMS (1000/WaypointCmd_UpdateRateHz) //waypoint commander run rate in ms
+
 #define CommLoop_UpdateRateHz 100 //Communications loop run rate in Hz
+#define CommLoop_UpdatePeriodMS (1000/CommLoop_UpdateRateHz) //Communications loop run rate in ms
+
 #define nRfDefault_uploadRateMS 200 //default upload rate in ms for nRF wireless comm
 #define camera_OnRateMinutes 15 //default camera switch on frequency in minutes
 #define camera_CaptureTimeS 30 //default camera capture time in seconds
 //-------------------------------------------------------------------------------------------------
 
-//** IMU MPU9150 **********************************************************************************
-#define IMU_FIFO_RATE_DIVIDER 0x01 // FIFO rate = 200Hz / (1 + this value), range 0x00 to 0x09. Here, 0x01 implies 100Hz.
-#define IMU_SAMPLE_RATE_DIVIDER 1 // Sample rate = 1kHz / (1 + this value), Here, 1 implies 500Hz.
-#define MPU6050_GYRO_FS MPU6050_GYRO_FS_2000 // measuring range of gyroscope (±n deg/s)
-#define MPU6050_ACCEL_FS MPU6050_ACCEL_FS_2 // measuring range of acceleration sensor (±n g)
-#define MPU6050_StabilizationReadings 10 //number of non deviating data points to determine if imu has stabilized
+
+//** Drivetrain ********************************************************************************
+//** Motor **
+#define PWMfreq_khz 20
 
 
-
-#define GyroV_MWindowSize 25
-
-//-------------------------------------------------------------------------------------------------
-
-//** Motor Control ********************************************************************************
 #define motor_LimitRPM_min  -100.0
 #define motor_LimitRPM_max  300.0
 
@@ -57,16 +65,36 @@
 #define control_SummLimit_max   1.0
 
 #define pid_MWindowSize 10
+//-------------
 
+//** Encoder **
+#define encoder_resolution 104
+#define encoder_MWindowSize 12
+//-------------
 //-------------------------------------------------------------------------------------------------
 
 
-//** Encoder **************************************************************************************
-#define encoder_resolution 104
-#define encoder_MWindowSize 12
-//-------------------------------------------------------------------------------------------------
+
 
 //** Localization *********************************************************************************
+//** IMU **
+#define Pose_MWindowSize 10
+#define GyroAcc_MWindowSize 25
+
+//* MPU9150 *
+#define MPU6050_StabilizationReadings 10 //number of non deviating data points to determine if imu has stabilized
+#define IMU_FIFO_RATE_DIVIDER 0x01 // FIFO rate = 200Hz / (1 + this value), range 0x00 to 0x09. Here, 0x01 implies 100Hz.
+#define IMU_SAMPLE_RATE_DIVIDER 1 // Sample rate = 1kHz / (1 + this value), Here, 1 implies 500Hz.
+#define MPU6050_GYRO_FS MPU6050_GYRO_FS_2000 // measuring range of gyroscope (±n deg/s)
+#define MPU6050_ACCEL_FS MPU6050_ACCEL_FS_2 // measuring range of acceleration sensor (±n g)
+//*   **   *
+
+//* BNO055 *
+#define BNO055_StabilizationReadings 10 //number of non deviating data points to determine if imu has stabilized
+
+//*   **   *
+
+//-------------
 #define wheel_dia 10 //wheel dia in mm
 
 //-------------------------------------------------------------------------------------------------