Diff: machine_ps3.h

Revision:

21:79b94cb922f0

Parent:

20:22efb19bb82f

Child:

22:3996c3f41922

--- a/machine_ps3.h	Thu Sep 17 07:01:31 2015 +0000
+++ b/machine_ps3.h	Fri Sep 18 01:49:16 2015 +0000
@@ -7,74 +7,26 @@
 #define MACHINE_H
 
 /**
+ * Functions prototype.
+ */
+
+
+/**
  * Includes
  */
 #include "mbed.h"
 #include "QEI.h"
 #include "PID.h"
 #include "PinDefined_and_Setting_ps3.h"
+#include "prototype.h"
 #include "Parameters_ps3.h"
 #include "communicate.h"
-
-/**
- * Functions prototype.
- */
-inline void initializeMotors();
-inline void initializeControllers();
-void Move_r(float speed1);
-void Move_l(float speed2);
-inline void mesure_move_r_velocity();
-inline void mesure_move_l_velocity();
-inline void mesure_swing_velocity();
-inline void Move_r_speed_following();
-inline void Move_l_speed_following();
-inline void Swing_speed_following();
-void Emergency_toggle();
-inline void initialize_interrupter();
-void shootEsaka();
-
-/**
- * Defines
- */
-#define RATE 0.01
-#define PI 3.14159265359
-//#define speed 10000.0
-#define speed 10000.0
+#include "autoMode.h"
 
 /**
  * Set functions.
  */
 
-/***PID Controller***/
-PID velocity_controller(18.0,5274.0 ,0.0,RATE);
-PID direction_controller(9.0,2637.0,0.0,RATE);
-
-/***Variables***/
-double Pulses_move_r        = 0.0;
-double Pulses_move_l        = 0.0;
-double Pulses_swing         = 0.0;
-double PrefPulses_move_r    = 0.0;
-double PrefPulses_move_l    = 0.0;
-double PrefPulses_swing     = 0.0;
-double swing_velocity       = 0.0;
-double velocity             = 0.0;
-double targ_velocity        = 0.0;
-double targ_sita            = 0.0;
-double targ_swing_velocity  = 0.0;
-double dsita                = 0.0;
-double dx                   = 0.0;
-double dy                   = 0.0;
-double sita                 = 0.0;
-double x                    = 0.0;
-double y                    = 0.0;
-double dlr                  = 0.0;
-double dll                  = 0.0;
-double vr                   = 0.0;
-double vl                   = 0.0; 
-double x1                   = 0.0;
-double x2                   = 0.0;
-double Vr                   = 0.0;
-double Vl                   = 0.0;
 int step                    = 0;
 int cylinder_step           = 0;
 
@@ -89,51 +41,31 @@
     Move_l_Motor_PWM.period_us(MOVE_L_PERIOD_US);
 }
 
-/***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 Move on field.***/
 //Right
 void Move_r(float speed1)
 {
     if(speed1<0) {
-        Move_r_Motor_CCW = 1;
-        Move_r_Motor_CW = 0;
-        Move_r_Motor_PWM = abs(speed1);
+        Move_r_Motor_CCW    = 1;
+        Move_r_Motor_CW     = 0;
+        Move_r_Motor_PWM    = abs(speed1);
     } else {
-        Move_r_Motor_CCW = 0;
-        Move_r_Motor_CW = 1;
-        Move_r_Motor_PWM = speed1;
+        Move_r_Motor_CCW    = 0;
+        Move_r_Motor_CW     = 1;
+        Move_r_Motor_PWM    = speed1;
     }
 }
 //Left
 void Move_l(float speed2)
 {
     if(speed2<0) {
-        Move_l_Motor_CCW = 1;
-        Move_l_Motor_CW = 0;
-        Move_l_Motor_PWM = abs(speed2);
+        Move_l_Motor_CCW    = 1;
+        Move_l_Motor_CW     = 0;
+        Move_l_Motor_PWM    = abs(speed2);
     } else {
-        Move_l_Motor_CCW = 0;
-        Move_l_Motor_CW = 1;
-        Move_l_Motor_PWM = speed2;
+        Move_l_Motor_CCW    = 0;
+        Move_l_Motor_CW     = 1;
+        Move_l_Motor_PWM    = speed2;
     }
 }
 
@@ -157,42 +89,12 @@
     y                   += dy;
 }
 
-/***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);
-}
-
 /***Emergency stop.***/
 void Emergency_toggle()
 {
     if(edge_circle) {
-        edge_circle=0;
-        Emergency_stop = !Emergency_stop;
+        edge_circle     = 0;
+        Emergency_stop  = !Emergency_stop;
     }
 }