Diff: machine_ps3.h

Revision:

1:a89e2a604dcf

Parent:

0:bd4719e15f7e

Child:

2:cf8ca6742db9

--- a/machine_ps3.h	Sun Aug 30 10:49:16 2015 +0000
+++ b/machine_ps3.h	Mon Aug 31 00:25:25 2015 +0000
@@ -29,7 +29,6 @@
 inline void data_check();
 inline void SBDBT_interrupt();
 inline void initializeSBDBT();
-inline void calculate_diff();
 inline void Move_r_speed_following();
 inline void Move_l_speed_following();
 inline void Swing_speed_following();
@@ -53,14 +52,8 @@
 /***PID Controller***/
 PID move_r_controller(MOVE_R_Kc, MOVE_R_TAUi, MOVE_R_TAUd, RATE);
 PID move_l_controller(MOVE_L_Kc, MOVE_L_TAUi, MOVE_L_TAUd, RATE);
-//diff
-PID diff_controller(DIFF_Kc, DIFF_TAUi, DIFF_TAUd, RATE);
-//swing
 PID swing_controller(SWING_Kc, SWING_TAUi, SWING_TAUd, RATE);
 
-/***Ticker***/
-//Ticker interrupt;
-
 /***Variables***/
 double Pulses_move_r=0.0;
 double Pulses_move_l=0.0;
@@ -76,7 +69,6 @@
 double targ_move_r_velocity=0.0;
 double targ_move_l_velocity=0.0;
 double targ_swing_velocity=0.0;
-double diff=0.0;
 //const double alpha=0.9;
 
 /**
@@ -95,7 +87,6 @@
 
     move_r_controller.setInputLimits(MOVE_R_INPUT_LIMIT_BOTTOM, MOVE_R_INPUT_LIMIT_TOP);
     move_l_controller.setInputLimits(MOVE_L_INPUT_LIMIT_BOTTOM, MOVE_L_INPUT_LIMIT_TOP);
-    diff_controller.setInputLimits(DIFF_INPUT_LIMIT_BOTTOM, DIFF_INPUT_LIMIT_TOP);
     swing_controller.setInputLimits(SWING_INPUT_LIMIT_BOTTOM, SWING_INPUT_LIMIT_TOP);
     
     //Pwm output from MOVE_OUTPUT_LIMIT_BOTTOM to MOVE_OUTPUT_LIMIT_TOP
@@ -106,17 +97,12 @@
     //set bias.
     move_r_controller.setBias(MOVE_R_BIAS);
     move_l_controller.setBias(MOVE_L_BIAS);
-    diff_controller.setBias(DIFF_BIAS);
     swing_controller.setBias(SWING_BIAS);
     
     //set mode.
     move_r_controller.setMode(AUTO_MODE);
     move_l_controller.setMode(AUTO_MODE);
-    diff_controller.setMode(AUTO_MODE);
     swing_controller.setMode(AUTO_MODE);
-    
-    //set point.
-    diff_controller.setSetPoint(0.0);
 }
 
 /***The function is Move on field.***/ 
@@ -172,27 +158,16 @@
 float cont_move_r=0.0,cont_move_l=0.0,cont_swing=0.0;
 short r=0,l=0;
 
-inline void calculate_diff(){
-    diff=move_r_velocity-move_l_velocity;
-    diff_controller.setProcessValue(diff);
-    if(diff<0) r=1,l=0;
-    else r=0,l=1;
-}
-
 inline void Move_r_speed_following(){
     move_r_controller.setSetPoint((float)targ_move_r_velocity);
     move_r_controller.setProcessValue((float)move_r_velocity);
     cont_move_r = move_r_controller.compute();
-    //if(r) cont_move_r=move_r_controller.compute()+diff_controller.compute();
-    //else cont_move_r=move_r_controller.compute();
     Move_r(cont_move_r);
 }
 inline void Move_l_speed_following(){
     move_l_controller.setSetPoint((float)targ_move_l_velocity);
     move_l_controller.setProcessValue((float)move_l_velocity);
     cont_move_l = move_l_controller.compute();
-    //if(l) cont_move_l=move_l_controller.compute()+diff_controller.compute();
-    //else cont_move_l=move_l_controller.compute();
     Move_l(cont_move_l);
 }
 inline void Swing_speed_following(){