2015_robocon_bteam / Mbed 2 deprecated 2015robot_main

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.

Dependencies:   PID QEI mbed

Fork of 2015robot_main by Naoto Deguchi

machine_ps3.h@13:87794ce49b50, 2015-09-16 (annotated)

Committer:
DeguNaoto
Date:
Wed Sep 16 00:47:46 2015 +0000
Revision:
13:87794ce49b50
Parent:
12:24a444bed6a0
Child:
14:a99f81878336
??? class ??

Who changed what in which revision?

UserRevisionLine numberNew contents of line
DeguNaoto 0:bd4719e15f7e 1 /**
DeguNaoto 2:cf8ca6742db9 2 * This library is included main.cpp.
DeguNaoto 0:bd4719e15f7e 3 * class "machine" whitch machine functions are structured.
DeguNaoto 0:bd4719e15f7e 4 */
DeguNaoto 0:bd4719e15f7e 5
DeguNaoto 0:bd4719e15f7e 6 #ifndef MACHINE_H
DeguNaoto 0:bd4719e15f7e 7 #define MACHINE_H
DeguNaoto 0:bd4719e15f7e 8
DeguNaoto 0:bd4719e15f7e 9 /**
DeguNaoto 0:bd4719e15f7e 10 * Includes
DeguNaoto 0:bd4719e15f7e 11 */
DeguNaoto 0:bd4719e15f7e 12 #include "mbed.h"
DeguNaoto 0:bd4719e15f7e 13 #include "QEI.h"
DeguNaoto 0:bd4719e15f7e 14 #include "PID.h"
DeguNaoto 0:bd4719e15f7e 15 #include "PinDefined_and_Setting_ps3.h"
DeguNaoto 0:bd4719e15f7e 16 #include "Parameters_ps3.h"
DeguNaoto 0:bd4719e15f7e 17
DeguNaoto 0:bd4719e15f7e 18 /**
DeguNaoto 0:bd4719e15f7e 19 * Functions prototype.
DeguNaoto 0:bd4719e15f7e 20 */
DeguNaoto 0:bd4719e15f7e 21 inline void initializeMotors();
DeguNaoto 0:bd4719e15f7e 22 inline void initializeControllers();
DeguNaoto 0:bd4719e15f7e 23 inline void mesure_move_r_velocity();
DeguNaoto 0:bd4719e15f7e 24 inline void mesure_move_l_velocity();
DeguNaoto 0:bd4719e15f7e 25 inline void data_clear();
DeguNaoto 0:bd4719e15f7e 26 inline void data_check();
DeguNaoto 0:bd4719e15f7e 27 inline void Move_r_speed_following();
DeguNaoto 0:bd4719e15f7e 28 inline void Move_l_speed_following();
DeguNaoto 13:87794ce49b50 29 extern void Move_r( float speed1 );
DeguNaoto 13:87794ce49b50 30 extern void Move_l( float speed2 );
DeguNaoto 13:87794ce49b50 31 extern void Emergency_toggle();
DeguNaoto 13:87794ce49b50 32
DeguNaoto 13:87794ce49b50 33 /**
DeguNaoto 13:87794ce49b50 34 * Class prototype.
DeguNaoto 13:87794ce49b50 35 */
DeguNaoto 13:87794ce49b50 36
DeguNaoto 0:bd4719e15f7e 37
DeguNaoto 0:bd4719e15f7e 38 /**
DeguNaoto 0:bd4719e15f7e 39 * include function header files.
DeguNaoto 0:bd4719e15f7e 40 */
DeguNaoto 0:bd4719e15f7e 41 #include "communicate.h"
DeguNaoto 2:cf8ca6742db9 42
DeguNaoto 0:bd4719e15f7e 43 /**
DeguNaoto 13:87794ce49b50 44 * Defines and const
DeguNaoto 0:bd4719e15f7e 45 */
DeguNaoto 13:87794ce49b50 46 #define RATE 0.01
DeguNaoto 13:87794ce49b50 47 #define PI 3.14159265359
DeguNaoto 11:ce3083681efa 48 #define speed 10000.0
DeguNaoto 13:87794ce49b50 49 const double d = 375.0;
DeguNaoto 13:87794ce49b50 50 const double r_wheel = 34.0;
DeguNaoto 13:87794ce49b50 51 const double ppr = 400.0;
DeguNaoto 0:bd4719e15f7e 52
DeguNaoto 0:bd4719e15f7e 53 /**
DeguNaoto 0:bd4719e15f7e 54 * Set functions.
DeguNaoto 0:bd4719e15f7e 55 */
DeguNaoto 2:cf8ca6742db9 56
DeguNaoto 0:bd4719e15f7e 57 /***PID Controller***/
DeguNaoto 13:87794ce49b50 58 PID velocity_controller( 18.0, 5274.0, 0.0, RATE );
DeguNaoto 13:87794ce49b50 59 PID direction_controller( 9.0, 2637.0, 0.0, RATE );
DeguNaoto 0:bd4719e15f7e 60
DeguNaoto 0:bd4719e15f7e 61 /***Variables***/
DeguNaoto 13:87794ce49b50 62
DeguNaoto 13:87794ce49b50 63 class Cstate {
DeguNaoto 13:87794ce49b50 64 private:
DeguNaoto 13:87794ce49b50 65 double _Pulses_r;
DeguNaoto 13:87794ce49b50 66 double _Pulses_l;
DeguNaoto 13:87794ce49b50 67 double _PrefPulses_r;
DeguNaoto 13:87794ce49b50 68 double _PrefPulses_l;
DeguNaoto 13:87794ce49b50 69 double _dsita;
DeguNaoto 13:87794ce49b50 70 double _dx;
DeguNaoto 13:87794ce49b50 71 double _dy;
DeguNaoto 13:87794ce49b50 72 double _sita;
DeguNaoto 13:87794ce49b50 73 double _x;
DeguNaoto 13:87794ce49b50 74 double _y;
DeguNaoto 13:87794ce49b50 75 double _dlr;
DeguNaoto 13:87794ce49b50 76 double _dll;
DeguNaoto 13:87794ce49b50 77 double _vr;
DeguNaoto 13:87794ce49b50 78 double _vl;
DeguNaoto 13:87794ce49b50 79 double _velocity;
DeguNaoto 13:87794ce49b50 80 Cstate() {
DeguNaoto 13:87794ce49b50 81 _PrefPulses_r = 0.0;
DeguNaoto 13:87794ce49b50 82 _PrefPulses_l = 0.0;
DeguNaoto 13:87794ce49b50 83 _sita = 0.0;
DeguNaoto 13:87794ce49b50 84 _x = 0.0;
DeguNaoto 13:87794ce49b50 85 _y = 0.0;
DeguNaoto 13:87794ce49b50 86 }
DeguNaoto 13:87794ce49b50 87 void mesure_state() {
DeguNaoto 13:87794ce49b50 88 _Pulses_r = ( double ) Move_r_sense.getPulses();
DeguNaoto 13:87794ce49b50 89 _Pulses_l = ( double ) Move_l_sense.getPulses();
DeguNaoto 13:87794ce49b50 90 _dlr = 2.0 * PI * r_wheel * ( ( _Pulses_r - _PrefPulses_r ) / ppr );
DeguNaoto 13:87794ce49b50 91 _dll = 2.0 * PI * r_wheel * ( ( _Pulses_l - _PrefPulses_l ) / ppr );
DeguNaoto 13:87794ce49b50 92 _PrefPulses_r = _Pulses_r;
DeguNaoto 13:87794ce49b50 93 _PrefPulses_l = _Pulses_l;
DeguNaoto 13:87794ce49b50 94 _vr = _dlr / RATE;
DeguNaoto 13:87794ce49b50 95 _vl = _dll / RATE;
DeguNaoto 13:87794ce49b50 96 _velocity = ( _vr + _vl ) / 2.0;
DeguNaoto 13:87794ce49b50 97 _dsita = ( _dlr - _dll ) / ( 2.0 * d );
DeguNaoto 13:87794ce49b50 98 _dx = ( ( _dlr + _dll ) / 2.0 ) * cos( _sita + _dsita / 2.0 );
DeguNaoto 13:87794ce49b50 99 _dy = ( ( _dlr + _dll ) / 2.0 ) * sin( _sita + _dsita / 2.0 );
DeguNaoto 13:87794ce49b50 100 _sita += _dsita;
DeguNaoto 13:87794ce49b50 101 _x += _dx;
DeguNaoto 13:87794ce49b50 102 _y += _dy;
DeguNaoto 13:87794ce49b50 103 }
DeguNaoto 13:87794ce49b50 104 public:
DeguNaoto 13:87794ce49b50 105 double velocity() { return _velocity; }
DeguNaoto 13:87794ce49b50 106 double sita() { return _sita; }
DeguNaoto 13:87794ce49b50 107 double x() { return _x; }
DeguNaoto 13:87794ce49b50 108 double y() { return _y; }
DeguNaoto 13:87794ce49b50 109 };
DeguNaoto 13:87794ce49b50 110
DeguNaoto 13:87794ce49b50 111 extern double targ_velocity=0.0;
DeguNaoto 13:87794ce49b50 112 extern double targ_sita=0.0;
DeguNaoto 13:87794ce49b50 113 extern double targ_swing_velocity=0.0;
DeguNaoto 13:87794ce49b50 114 extern double x1=0.0;
DeguNaoto 13:87794ce49b50 115 extern double x2=0.0;
DeguNaoto 13:87794ce49b50 116 extern double Vr=0.0;
DeguNaoto 13:87794ce49b50 117 extern double Vl=0.0;
DeguNaoto 13:87794ce49b50 118 extern int step = 0;
DeguNaoto 13:87794ce49b50 119 extern int cylinder_step = 0;
DeguNaoto 13:87794ce49b50 120
DeguNaoto 0:bd4719e15f7e 121
DeguNaoto 0:bd4719e15f7e 122 /**
DeguNaoto 0:bd4719e15f7e 123 * Functions.
DeguNaoto 0:bd4719e15f7e 124 */
DeguNaoto 0:bd4719e15f7e 125
DeguNaoto 0:bd4719e15f7e 126 /***The function is motors initialize.***/
DeguNaoto 2:cf8ca6742db9 127 inline void initializeMotors()
DeguNaoto 2:cf8ca6742db9 128 {
DeguNaoto 0:bd4719e15f7e 129 Move_r_Motor_PWM.period_us(MOVE_R_PERIOD_US);
DeguNaoto 0:bd4719e15f7e 130 Move_l_Motor_PWM.period_us(MOVE_L_PERIOD_US);
DeguNaoto 0:bd4719e15f7e 131 }
DeguNaoto 0:bd4719e15f7e 132
DeguNaoto 0:bd4719e15f7e 133 /***The function is PID controller initialize.***/
DeguNaoto 2:cf8ca6742db9 134 inline void initializeControllers()
DeguNaoto 2:cf8ca6742db9 135 {
DeguNaoto 0:bd4719e15f7e 136
DeguNaoto 11:ce3083681efa 137 velocity_controller.setInputLimits(-200000.0, 200000.0); //x1
DeguNaoto 11:ce3083681efa 138 direction_controller.setInputLimits(-10.0, 10.0); //x2
DeguNaoto 2:cf8ca6742db9 139
DeguNaoto 0:bd4719e15f7e 140 //Pwm output from MOVE_OUTPUT_LIMIT_BOTTOM to MOVE_OUTPUT_LIMIT_TOP
DeguNaoto 11:ce3083681efa 141 velocity_controller.setOutputLimits(-1.0, 1.0);
DeguNaoto 11:ce3083681efa 142 direction_controller.setOutputLimits(-1.0, 1.0);
DeguNaoto 11:ce3083681efa 143
DeguNaoto 11:ce3083681efa 144 //set bias. 初期値
DeguNaoto 11:ce3083681efa 145 velocity_controller.setBias(0.0);
DeguNaoto 11:ce3083681efa 146 direction_controller.setBias(0.0);
DeguNaoto 2:cf8ca6742db9 147
DeguNaoto 0:bd4719e15f7e 148 //set mode.
DeguNaoto 11:ce3083681efa 149 velocity_controller.setMode(AUTO_MODE);
DeguNaoto 11:ce3083681efa 150 direction_controller.setMode(AUTO_MODE);
DeguNaoto 0:bd4719e15f7e 151 }
DeguNaoto 0:bd4719e15f7e 152
DeguNaoto 2:cf8ca6742db9 153 /***The function is Move on field.***/
DeguNaoto 0:bd4719e15f7e 154 //Right
DeguNaoto 13:87794ce49b50 155 void Move_r(float speed)
DeguNaoto 2:cf8ca6742db9 156 {
DeguNaoto 13:87794ce49b50 157 if(speed<0) {
DeguNaoto 0:bd4719e15f7e 158 Move_r_Motor_CCW = 1;
DeguNaoto 0:bd4719e15f7e 159 Move_r_Motor_CW = 0;
DeguNaoto 13:87794ce49b50 160 Move_r_Motor_PWM = abs(speed);
DeguNaoto 2:cf8ca6742db9 161 } else {
DeguNaoto 0:bd4719e15f7e 162 Move_r_Motor_CCW = 0;
DeguNaoto 0:bd4719e15f7e 163 Move_r_Motor_CW = 1;
DeguNaoto 13:87794ce49b50 164 Move_r_Motor_PWM = speed;
DeguNaoto 0:bd4719e15f7e 165 }
DeguNaoto 0:bd4719e15f7e 166 }
DeguNaoto 0:bd4719e15f7e 167 //Left
DeguNaoto 13:87794ce49b50 168 void Move_l(float speed)
DeguNaoto 2:cf8ca6742db9 169 {
DeguNaoto 13:87794ce49b50 170 if(speed<0) {
DeguNaoto 0:bd4719e15f7e 171 Move_l_Motor_CCW = 1;
DeguNaoto 0:bd4719e15f7e 172 Move_l_Motor_CW = 0;
DeguNaoto 13:87794ce49b50 173 Move_l_Motor_PWM = abs(speed);
DeguNaoto 2:cf8ca6742db9 174 } else {
DeguNaoto 0:bd4719e15f7e 175 Move_l_Motor_CCW = 0;
DeguNaoto 0:bd4719e15f7e 176 Move_l_Motor_CW = 1;
DeguNaoto 13:87794ce49b50 177 Move_l_Motor_PWM = speed;
DeguNaoto 0:bd4719e15f7e 178 }
DeguNaoto 0:bd4719e15f7e 179 }
DeguNaoto 0:bd4719e15f7e 180
DeguNaoto 11:ce3083681efa 181 /***Caluculate state.***/
DeguNaoto 11:ce3083681efa 182
DeguNaoto 0:bd4719e15f7e 183 /***The function is following move speed.***/
DeguNaoto 13:87794ce49b50 184 void velocity_following(double velocity,double target)
DeguNaoto 11:ce3083681efa 185 {
DeguNaoto 13:87794ce49b50 186 velocity_controller.setSetPoint( ( float ) target );
DeguNaoto 13:87794ce49b50 187 velocity_controller.setProcessValue( ( float ) velocity );
DeguNaoto 13:87794ce49b50 188 x1 = ( double )velocity_controller.compute();
DeguNaoto 11:ce3083681efa 189 }
DeguNaoto 11:ce3083681efa 190
DeguNaoto 13:87794ce49b50 191 void sita_following(double sita,double target)
DeguNaoto 2:cf8ca6742db9 192 {
DeguNaoto 13:87794ce49b50 193 direction_controller.setSetPoint( ( float ) target );
DeguNaoto 13:87794ce49b50 194 direction_controller.setProcessValue( ( float ) sita );
DeguNaoto 13:87794ce49b50 195 x2 = ( double )direction_controller.compute();
DeguNaoto 0:bd4719e15f7e 196 }
DeguNaoto 11:ce3083681efa 197
DeguNaoto 11:ce3083681efa 198 inline void move_following()
DeguNaoto 2:cf8ca6742db9 199 {
DeguNaoto 11:ce3083681efa 200 velocity_following();
DeguNaoto 11:ce3083681efa 201 sita_following();
DeguNaoto 11:ce3083681efa 202 Vr=(x1+x2)/2.0;
DeguNaoto 11:ce3083681efa 203 Vl=(x1-x2)/2.0;
DeguNaoto 11:ce3083681efa 204 if(abs(Vr)<0.05) Vr=0.0;
DeguNaoto 11:ce3083681efa 205 if(abs(Vl)<0.05) Vl=0.0;
DeguNaoto 11:ce3083681efa 206 Move_r((float)Vr);
DeguNaoto 11:ce3083681efa 207 Move_l((float)Vl);
DeguNaoto 0:bd4719e15f7e 208 }
DeguNaoto 11:ce3083681efa 209
DeguNaoto 0:bd4719e15f7e 210 /***Emergency stop.***/
DeguNaoto 2:cf8ca6742db9 211 void Emergency_toggle()
DeguNaoto 2:cf8ca6742db9 212 {
DeguNaoto 4:51d87d2b698c 213 if(edge_circle) {
DeguNaoto 4:51d87d2b698c 214 edge_circle=0;
DeguNaoto 0:bd4719e15f7e 215 if(toggle) toggle=0,Emergency_stop=0;
DeguNaoto 0:bd4719e15f7e 216 else toggle=1,Emergency_stop=1;
DeguNaoto 0:bd4719e15f7e 217 }
DeguNaoto 0:bd4719e15f7e 218 }
DeguNaoto 0:bd4719e15f7e 219
DeguNaoto 0:bd4719e15f7e 220 #endif /*machine.h*/