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:
13:d971833e28ea
Parent:
11:49344285c82a
Child:
15:23d79bb4eaf2
--- a/Configuration/config.h	Mon Apr 25 00:22:54 2016 +0000
+++ b/Configuration/config.h	Mon Apr 25 06:04:26 2016 +0000
@@ -16,10 +16,10 @@
 #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
+#define imu_UpdateRateHz 100 // 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_UpdateRateHz 200 // 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
@@ -133,13 +133,13 @@
 // This PID controller adds/subtracts a percentage of command velocity based on the variations of
 // the pitch angle while controlling the pitch angle to a setpoint angle.
 
-#define AttPIDFf_kP    500.00
-#define AttPIDFf_kI    10.00
-#define AttPIDFf_kD    1.00
+#define AttPIDFf_kP    9.00
+#define AttPIDFf_kI    1.00
+#define AttPIDFf_kD    0.10
 #define AttPIDFf_kFf   0.00
 
-#define AttPIDFf_lim_P  0.3
-#define AttPIDFf_lim_I  0.4
+#define AttPIDFf_lim_P  0.6
+#define AttPIDFf_lim_I  0.1
 #define AttPIDFf_lim_D  0.3
 #define AttPIDFf_lim_Ff 0.0