Diff: autoMode.h

Revision:

21:79b94cb922f0

Child:

22:3996c3f41922

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/autoMode.h	Fri Sep 18 01:49:16 2015 +0000
@@ -0,0 +1,57 @@
+#ifndef AUTOMODE_H
+#define AUTOMODE_H
+
+/***PID Controller***/
+PID velocity_controller(18.0,5274.0 ,0.0,RATE);
+PID direction_controller(9.0,2637.0,0.0,RATE);
+
+/***The function is PID controller initialize.***/
+inline void initializeControllers()
+{
+
+    velocity_controller.setInputLimits(-200000.0, 200000.0); //x1
+    direction_controller.setInputLimits(-10.0, 10.0); //x2
+
+    //Pwm output from MOVE_OUTPUT_LIMIT_BOTTOM to MOVE_OUTPUT_LIMIT_TOP
+    velocity_controller.setOutputLimits(-1.0, 1.0);
+    direction_controller.setOutputLimits(-1.0, 1.0);
+    
+    //set bias. 初期値
+    velocity_controller.setBias(0.0);
+    direction_controller.setBias(0.0);
+
+    //set mode.
+    velocity_controller.setMode(AUTO_MODE);
+    direction_controller.setMode(AUTO_MODE);
+}
+
+/***The function is following move speed.***/
+inline void velocity_following()
+{
+    velocity_controller.setSetPoint((float)targ_velocity);
+    velocity_controller.setProcessValue((float)velocity);
+    x1 = (double)velocity_controller.compute();
+}
+
+inline void sita_following()
+{
+    direction_controller.setSetPoint((float)targ_sita);
+    direction_controller.setProcessValue((float)sita);
+    //direction_controller.setSetPoint(0.0);  //目標値とのずれをなくす
+    //direction_controller.setProcessValue(-y);
+    x2 = (double)direction_controller.compute();
+}
+
+inline void move_following()
+{
+    velocity_following();
+    sita_following();
+    Vr                  = ( x1 + x2 ) / 2.0;
+    Vl                  = ( x1 - x2 ) / 2.0;
+    if( abs(Vr) < 0.05 ) Vr = 0.0;
+    if( abs(Vl) < 0.05 ) Vl = 0.0;
+    Move_r( ( float ) Vr );
+    Move_l( ( float ) Vl );
+}
+
+#endif /*autoMode.h*/
\ No newline at end of file