Diff: machine_ps3.h

Revision:

14:a99f81878336

Parent:

13:87794ce49b50

Child:

15:49ed1bbf3c1d

--- a/machine_ps3.h	Wed Sep 16 00:47:46 2015 +0000
+++ b/machine_ps3.h	Wed Sep 16 08:11:06 2015 +0000
@@ -12,9 +12,12 @@
 #include "mbed.h"
 #include "QEI.h"
 #include "PID.h"
+#include "MachineState.h"
 #include "PinDefined_and_Setting_ps3.h"
 #include "Parameters_ps3.h"
 
+Serial pc(USBTX, USBRX);
+
 /**
  * Functions prototype.
  */
@@ -22,18 +25,10 @@
 inline void initializeControllers();
 inline void mesure_move_r_velocity();
 inline void mesure_move_l_velocity();
-inline void data_clear();
-inline void data_check();
 inline void Move_r_speed_following();
 inline void Move_l_speed_following();
-extern void Move_r( float speed1 );
-extern void Move_l( float speed2 );
-extern void Emergency_toggle();
-
-/**
- * Class prototype.
- */
-
+void        Move_r( float speed1 );
+void        Move_l( float speed2 );
 
 /**
  * include function header files.
@@ -46,9 +41,17 @@
 #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;
+const double HalfTread   = 375.0;
+const double WheelRadius = 34.0;
+const double PulsePerRev = 400.0;
+const double Xo          = 0.0;
+const double Yo          = 0.0;
+const double Sitao       = PI / 4.0;
+
+/**
+ * Class prototype.
+ */
+MachineState Machine( WheelRadius, HalfTread, PulsePerRev, ENCOD_R_A, ENCOD_R_B, ENCOD_L_A, ENCOD_L_B, RATE, Xo, Yo, Sitao );
 
 /**
  * Set functions.
@@ -59,64 +62,15 @@
 PID direction_controller( 9.0, 2637.0, 0.0, RATE );
 
 /***Variables***/
-
-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;
+double targ_velocity        = 0.0;
+double targ_sita            = 0.0;
+double targ_swing_velocity  = 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;
 
 
 /**
@@ -152,29 +106,29 @@
 
 /***The function is Move on field.***/
 //Right
-void Move_r(float speed)
+void Move_r(float speed1)
 {
-    if(speed<0) {
+    if(speed1<0) {
         Move_r_Motor_CCW = 1;
         Move_r_Motor_CW = 0;
-        Move_r_Motor_PWM = abs(speed);
+        Move_r_Motor_PWM = abs(speed1);
     } else {
         Move_r_Motor_CCW = 0;
         Move_r_Motor_CW = 1;
-        Move_r_Motor_PWM = speed;
+        Move_r_Motor_PWM = speed1;
     }
 }
 //Left
-void Move_l(float speed)
+void Move_l(float speed2)
 {
     if(speed<0) {
         Move_l_Motor_CCW = 1;
         Move_l_Motor_CW = 0;
-        Move_l_Motor_PWM = abs(speed);
+        Move_l_Motor_PWM = abs(speed2);
     } else {
         Move_l_Motor_CCW = 0;
         Move_l_Motor_CW = 1;
-        Move_l_Motor_PWM = speed;
+        Move_l_Motor_PWM = speed2;
     }
 }
 
@@ -197,14 +151,14 @@
 
 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);
+    velocity_following( Machine.getVelocity(), targ_velocity );
+    sita_following( Machine.getSita(), targ_sita );
+    Vr = ( x1 + x2 ) / 2.0;
+    Vl = ( x1 - x2 ) / 2.0;
+    if( abs( Vr ) < 0.02 ) Vr = 0.0;
+    if( abs( Vl ) < 0.02 ) Vl = 0.0;
+    Move_r( ( float ) Vr );
+    Move_l( ( float ) Vl );
 }
 
 /***Emergency stop.***/