Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of 2015robot_main by
Diff: machine_ps3.h
- 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()