Diff: machine_ps3.h

Revision:

15:49ed1bbf3c1d

Parent:

14:a99f81878336

diff -r a99f81878336 -r 49ed1bbf3c1d machine_ps3.h
--- a/machine_ps3.h	Wed Sep 16 08:11:06 2015 +0000
+++ b/machine_ps3.h	Wed Sep 16 09:35:19 2015 +0000
@@ -58,8 +58,9 @@
  */
 
 /***PID Controller***/
-PID velocity_controller( 18.0, 5274.0, 0.0, RATE );
-PID direction_controller( 9.0, 2637.0, 0.0, RATE );
+PID velocity_controller( MOVE_VELOCITY_Kc, MOVE_VELOCITY_TAUi, MOVE_VELOCITY_TAUd, RATE );
+PID direction_controller( MOVE_DIRECTION_Kc, MOVE_DIRECTION_TAUi, MOVE_DIRECTION_TAUd, RATE );
+//PID direction_controller( 9.0, 2637.0, 0.0, RATE );
 
 /***Variables***/
 double targ_velocity        = 0.0;
@@ -69,8 +70,8 @@
 double x2                   = 0.0;
 double Vr                   = 0.0;
 double Vl                   = 0.0;
-int    step            = 0;
-int    cylinder_step   = 0;
+int    step                 = 0;
+int    cylinder_step        = 0;
 
 
 /**
@@ -88,16 +89,16 @@
 inline void initializeControllers()
 {
 
-    velocity_controller.setInputLimits(-200000.0, 200000.0); //x1
-    direction_controller.setInputLimits(-10.0, 10.0); //x2
+    velocity_controller.setInputLimits(MOVE_VELOCITY_INPUT_LIMIT_BOTTOM, MOVE_VELOCITY_INPUT_LIMIT_TOP); //x1
+    direction_controller.setInputLimits(MOVE_DIRECTION_INPUT_LIMIT_BOTTOM , MOVE_DIRECTION_INPUT_LIMIT_TOP); //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);
+    velocity_controller.setOutputLimits(MOVE_VELOCITY_OUTPUT_LIMIT_BOTTOM, MOVE_VELOCITY_OUTPUT_LIMIT_TOP);
+    direction_controller.setOutputLimits(MOVE_DIRECTION_OUTPUT_LIMIT_BOTTOM, MOVE_DIRECTION_OUTPUT_LIMIT_TOP);
     
     //set bias. 初期値
-    velocity_controller.setBias(0.0);
-    direction_controller.setBias(0.0);
+    velocity_controller.setBias(MOVE_VELOCITY_BIAS);
+    direction_controller.setBias(MOVE_DIRECTION_BIAS);
 
     //set mode.
     velocity_controller.setMode(AUTO_MODE);
@@ -121,7 +122,7 @@
 //Left
 void Move_l(float speed2)
 {
-    if(speed<0) {
+    if(speed2<0) {
         Move_l_Motor_CCW = 1;
         Move_l_Motor_CW = 0;
         Move_l_Motor_PWM = abs(speed2);
@@ -135,16 +136,16 @@
 /***Caluculate state.***/
 
 /***The function is following move speed.***/
-void velocity_following(double velocity,double target)
+void velocity_following(double velocity,double target_v)
 {
-    velocity_controller.setSetPoint( ( float ) target );
+    velocity_controller.setSetPoint( ( float ) target_v );
     velocity_controller.setProcessValue( ( float ) velocity );
     x1 = ( double )velocity_controller.compute();
 }
 
-void sita_following(double sita,double target)
+void sita_following(double sita,double target_s)
 {
-    direction_controller.setSetPoint( ( float ) target );
+    direction_controller.setSetPoint( ( float ) target_s );
     direction_controller.setProcessValue( ( float ) sita );
     x2 = ( double )direction_controller.compute();
 }