2015_robocon_bteam / Mbed 2 deprecated 2015robot_main

Dependencies:   PID QEI mbed

Fork of 2015robot_main by Naoto Deguchi

Revision:
13:87794ce49b50
Parent:
12:24a444bed6a0
Child:
14:a99f81878336
--- a/machine_ps3.h	Tue Sep 15 08:32:03 2015 +0000
+++ b/machine_ps3.h	Wed Sep 16 00:47:46 2015 +0000
@@ -20,21 +20,20 @@
  */
 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 data_clear();
 inline void data_check();
-inline void SBDBT_interrupt();
-inline void initializeSBDBT();
 inline void Move_r_speed_following();
 inline void Move_l_speed_following();
-inline void Swing_speed_following();
-void Emergency_toggle();
-short toggle=0,edge_circle=0,edge_triangle=0,edge_r1=0,edge_l1=0,edge_l_up=0,edge_l_down=0,edge_r_up=0,edge_r_down=0,edge_right=0,edge_left=0,edge_up=0;
-int step=0,cylinder_step=0;
+extern void Move_r( float speed1 );
+extern void Move_l( float speed2 );
+extern void Emergency_toggle();
+
+/**
+ * Class prototype.
+ */
+
 
 /**
  * include function header files.
@@ -42,38 +41,83 @@
 #include "communicate.h"
 
 /**
- * Defines
+ * Defines and const
  */
-#define RATE 0.01
-#define PI 3.14159265359
-//#define speed 10000.0
+#define RATE  0.01
+#define PI    3.14159265359
 #define speed 10000.0
+const double d       = 375.0;
+const double r_wheel = 34.0;
+const double ppr     = 400.0;
 
 /**
  * Set functions.
  */
 
 /***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( 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,dx=0.0,dy=0.0,sita=0.0,x=0.0,y=0.0,dlr=0.0,dll=0.0,vr=0.0,vl=0.0; //state
-double x1=0.0,x2=0.0;
-double Vr=0.0,Vl=0.0;
-const double d=375.0;
-const double r_wheel=34.0;
+
+class Cstate {
+private:
+    double _Pulses_r;
+    double _Pulses_l;
+    double _PrefPulses_r;
+    double _PrefPulses_l;
+    double _dsita;
+    double _dx;
+    double _dy;
+    double _sita;
+    double _x;
+    double _y;
+    double _dlr;
+    double _dll;
+    double _vr;
+    double _vl;
+    double _velocity;
+    Cstate() {
+        _PrefPulses_r = 0.0;
+        _PrefPulses_l = 0.0;
+        _sita         = 0.0;
+        _x            = 0.0;
+        _y            = 0.0;
+    }
+    void mesure_state() {
+        _Pulses_r     = ( double ) Move_r_sense.getPulses();
+        _Pulses_l     = ( double ) Move_l_sense.getPulses();
+        _dlr          = 2.0 * PI * r_wheel * ( ( _Pulses_r - _PrefPulses_r ) / ppr );
+        _dll          = 2.0 * PI * r_wheel * ( ( _Pulses_l - _PrefPulses_l ) / ppr );
+        _PrefPulses_r = _Pulses_r;
+        _PrefPulses_l = _Pulses_l;
+        _vr           = _dlr / RATE;
+        _vl           = _dll / RATE;
+        _velocity     = ( _vr + _vl ) / 2.0;
+        _dsita        = ( _dlr - _dll ) / ( 2.0 * d );
+        _dx           = ( ( _dlr + _dll ) / 2.0 ) * cos( _sita + _dsita / 2.0 );
+        _dy           = ( ( _dlr + _dll ) / 2.0 ) * sin( _sita + _dsita / 2.0 );
+        _sita        += _dsita;
+        _x           += _dx;
+        _y           += _dy;
+    }
+public:   
+    double velocity() { return _velocity; }
+    double sita()     { return _sita;     }
+    double x()        { return _x;        }
+    double y()        { return _y;        }
+};
+
+extern double targ_velocity=0.0;
+extern double targ_sita=0.0;
+extern double targ_swing_velocity=0.0;
+extern double x1=0.0;
+extern double x2=0.0;
+extern double Vr=0.0;
+extern double Vl=0.0;
+extern int    step            = 0;
+extern int    cylinder_step   = 0;
+
 
 /**
  * Functions.
@@ -108,66 +152,47 @@
 
 /***The function is Move on field.***/
 //Right
-void Move_r(float speed1)
+void Move_r(float speed)
 {
-    if(speed1<0) {
+    if(speed<0) {
         Move_r_Motor_CCW = 1;
         Move_r_Motor_CW = 0;
-        Move_r_Motor_PWM = abs(speed1);
+        Move_r_Motor_PWM = abs(speed);
     } else {
         Move_r_Motor_CCW = 0;
         Move_r_Motor_CW = 1;
-        Move_r_Motor_PWM = speed1;
+        Move_r_Motor_PWM = speed;
     }
 }
 //Left
-void Move_l(float speed2)
+void Move_l(float speed)
 {
-    if(speed2<0) {
+    if(speed<0) {
         Move_l_Motor_CCW = 1;
         Move_l_Motor_CW = 0;
-        Move_l_Motor_PWM = abs(speed2);
+        Move_l_Motor_PWM = abs(speed);
     } else {
         Move_l_Motor_CCW = 0;
         Move_l_Motor_CW = 1;
-        Move_l_Motor_PWM = speed2;
+        Move_l_Motor_PWM = speed;
     }
 }
 
 /***Caluculate state.***/
-inline void mesure_state()
-{
-    Pulses_move_r = (double)Move_r_sense.getPulses();
-    dlr = (((Pulses_move_r - PrefPulses_move_r)/400.0)*2.0*PI)*r_wheel;
-    PrefPulses_move_r = Pulses_move_r;
-    Pulses_move_l = (double)Move_l_sense.getPulses();
-    dll = (((Pulses_move_l - PrefPulses_move_l)/400.0)*2.0*PI)*r_wheel;
-    PrefPulses_move_l = Pulses_move_l;
-    vr=dlr/RATE,vl=dll/RATE;
-    velocity=(vr+vl)/2.0;
-    dsita=(dlr - dll)/(2.0*d);
-    dx=((dlr+dll)/2.0)*cos(sita+dsita/2.0);
-    dy=((dlr+dll)/2.0)*sin(sita+dsita/2.0);
-    sita+=dsita,x+=dx,y+=dy;
-}
 
 /***The function is following move speed.***/
-float cont_swing=0.0;
-
-inline void velocity_following()
+void velocity_following(double velocity,double target)
 {
-    velocity_controller.setSetPoint((float)targ_velocity);
-    velocity_controller.setProcessValue((float)velocity);
-    x1 = (double)velocity_controller.compute();
+    velocity_controller.setSetPoint( ( float ) target );
+    velocity_controller.setProcessValue( ( float ) velocity );
+    x1 = ( double )velocity_controller.compute();
 }
 
-inline void sita_following()
+void sita_following(double sita,double target)
 {
-    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();
+    direction_controller.setSetPoint( ( float ) target );
+    direction_controller.setProcessValue( ( float ) sita );
+    x2 = ( double )direction_controller.compute();
 }
 
 inline void move_following()