Diff: machine_ps3.h

Revision:

12:24a444bed6a0

Parent:

11:ce3083681efa

Child:

13:87794ce49b50

Child:

18:526124eef0d1

--- a/machine_ps3.h	Sat Sep 12 10:35:51 2015 +0000
+++ b/machine_ps3.h	Tue Sep 15 08:32:03 2015 +0000
@@ -46,6 +46,7 @@
  */
 #define RATE 0.01
 #define PI 3.14159265359
+//#define speed 10000.0
 #define speed 10000.0
 
 /**
@@ -54,8 +55,7 @@
 
 /***PID Controller***/
 PID velocity_controller(18.0,5274.0 ,0.0,RATE);
-PID direction_controller(18.0,5274.0,0.0,RATE);
-PID swing_controller(SWING_Kc, SWING_TAUi, SWING_TAUd, RATE);
+PID direction_controller(9.0,2637.0,0.0,RATE);
 
 /***Variables***/
 double Pulses_move_r=0.0;
@@ -84,7 +84,6 @@
 {
     Move_r_Motor_PWM.period_us(MOVE_R_PERIOD_US);
     Move_l_Motor_PWM.period_us(MOVE_L_PERIOD_US);
-    Motor_swing_pwm.period_us(SWING_PERIOD_US);
 }
 
 /***The function is PID controller initialize.***/
@@ -93,22 +92,18 @@
 
     velocity_controller.setInputLimits(-200000.0, 200000.0); //x1
     direction_controller.setInputLimits(-10.0, 10.0); //x2
-    swing_controller.setInputLimits(SWING_INPUT_LIMIT_BOTTOM, SWING_INPUT_LIMIT_TOP);
 
     //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);
-    swing_controller.setOutputLimits(SWING_OUTPUT_LIMIT_BOTTOM, SWING_OUTPUT_LIMIT_TOP);
     
     //set bias. 初期値
     velocity_controller.setBias(0.0);
     direction_controller.setBias(0.0);
-    swing_controller.setBias(SWING_BIAS);
 
     //set mode.
     velocity_controller.setMode(AUTO_MODE);
     direction_controller.setMode(AUTO_MODE);
-    swing_controller.setMode(AUTO_MODE);
 }
 
 /***The function is Move on field.***/
@@ -156,13 +151,6 @@
     sita+=dsita,x+=dx,y+=dy;
 }
 
-inline void mesure_swing_velocity()
-{
-    Pulses_swing = (double)Swing_speed_sense.getPulses();
-    swing_velocity = Pulses_swing - PrefPulses_swing;
-    PrefPulses_swing = Pulses_swing;
-}
-
 /***The function is following move speed.***/
 float cont_swing=0.0;
 
@@ -194,15 +182,6 @@
     Move_l((float)Vl);
 }
 
-inline void Swing_speed_following()
-{
-    swing_controller.setSetPoint((float)targ_swing_velocity);
-    swing_controller.setProcessValue((float)swing_velocity);
-    cont_swing = swing_controller.compute();
-    Motor_swing_pwm = cont_swing;
-}
-
-
 /***Emergency stop.***/
 void Emergency_toggle()
 {