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:
6:690db8b5030b
Parent:
5:099cb2e76c7d
Child:
8:b4da70727fd4
--- a/Configuration/config.h	Mon Jan 25 07:28:40 2016 +0000
+++ b/Configuration/config.h	Thu Feb 04 02:11:36 2016 +0000
@@ -13,7 +13,7 @@
 //-------------------------------------------------------------------------------------------------
 
 //** Function exectution rates ********************************************************************
-#define Hearbeat_RateHz 10 //hearbeat loop run rate in Hz
+#define Hearbeat_RateHz 1 //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
@@ -25,7 +25,7 @@
 #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_RateHz 100 //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
@@ -34,10 +34,10 @@
 #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 20 //Communications loop run rate in Hz
+#define CommLoop_UpdateRateHz 10 //Communications loop run rate in Hz
 #define CommLoop_UpdatePeriodMS (1000/CommLoop_UpdateRateHz) //Communications loop run rate in ms
 
-#define CommPump_UpdateRateHz 500 //nRF network communications pump run rate in Hz
+#define CommPump_UpdateRateHz 250 //nRF network communications pump run rate in Hz
 #define CommPump_UpdatePeriodMS (1000/CommPump_UpdateRateHz) //nRF network communications pump run rate in ms
 
 #define nRfDefault_uploadRateMS 200 //default upload rate in ms for nRF wireless comm
@@ -51,24 +51,25 @@
 //** Motor **
 #define PWMfreq_khz 20
 
-
-#define motor_LimitRPM_min  -100.0
-#define motor_LimitRPM_max  300.0
-
-#define motor_Acc_limit     200.0
+#define PIDFf_kP    1.5
+#define PIDFf_kI    9.5
+#define PIDFf_kD    0.5
+#define PIDFf_kFf   1.5
 
-#define PIDFf_kP    0.500
-#define PIDFf_kI    0.900
-#define PIDFf_kD    0.010
-#define PIDFf_kFf   1.000
+#define PIDFf_lim_P    0.3
+#define PIDFf_lim_I    0.5
+#define PIDFf_lim_D    0.2
+#define PIDFf_lim_Ff   0.5
 
-#define PIDFf_TermsLimit_min    -0.3
-#define PIDFf_TermsLimit_max    0.9
-
-#define control_SummLimit_min   -0.5
-#define control_SummLimit_max   1.0
+#define PIDFf_range_min     -1.0
+#define PIDFf_range_max     1.0
 
 #define pid_MWindowSize 10
+
+//#define motor_LimitRPM_min  -100.0
+//#define motor_LimitRPM_max  300.0
+
+//#define motor_Acc_limit     200.0
 //-------------
 
 //** Encoder **
@@ -79,7 +80,6 @@
 
 
 
-
 //** Localization *********************************************************************************
 //** IMU **
 #define Pose_MWindowSize 10