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@15:49ed1bbf3c1d, 2015-09-16 (annotated)

Committer:
DeguNaoto
Date:
Wed Sep 16 09:35:19 2015 +0000
Revision:
15:49ed1bbf3c1d
Parent:
14:a99f81878336 
????;

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 14:a99f81878336 15 #include "MachineState.h"
DeguNaoto 0:bd4719e15f7e 16 #include "PinDefined_and_Setting_ps3.h"
DeguNaoto 0:bd4719e15f7e 17 #include "Parameters_ps3.h"
DeguNaoto 0:bd4719e15f7e 18
DeguNaoto 14:a99f81878336 19 Serial pc(USBTX, USBRX);
DeguNaoto 14:a99f81878336 20
DeguNaoto 0:bd4719e15f7e 21 /**
DeguNaoto 0:bd4719e15f7e 22 * Functions prototype.
DeguNaoto 0:bd4719e15f7e 23 */
DeguNaoto 0:bd4719e15f7e 24 inline void initializeMotors();
DeguNaoto 0:bd4719e15f7e 25 inline void initializeControllers();
DeguNaoto 0:bd4719e15f7e 26 inline void mesure_move_r_velocity();
DeguNaoto 0:bd4719e15f7e 27 inline void mesure_move_l_velocity();
DeguNaoto 0:bd4719e15f7e 28 inline void Move_r_speed_following();
DeguNaoto 0:bd4719e15f7e 29 inline void Move_l_speed_following();
DeguNaoto 14:a99f81878336 30 void Move_r( float speed1 );
DeguNaoto 14:a99f81878336 31 void Move_l( float speed2 );
DeguNaoto 0:bd4719e15f7e 32
DeguNaoto 0:bd4719e15f7e 33 /**
DeguNaoto 0:bd4719e15f7e 34 * include function header files.
DeguNaoto 0:bd4719e15f7e 35 */
DeguNaoto 0:bd4719e15f7e 36 #include "communicate.h"
DeguNaoto 2:cf8ca6742db9 37
DeguNaoto 0:bd4719e15f7e 38 /**
DeguNaoto 13:87794ce49b50 39 * Defines and const
DeguNaoto 0:bd4719e15f7e 40 */
DeguNaoto 13:87794ce49b50 41 #define RATE 0.01
DeguNaoto 13:87794ce49b50 42 #define PI 3.14159265359
DeguNaoto 11:ce3083681efa 43 #define speed 10000.0
DeguNaoto 14:a99f81878336 44 const double HalfTread = 375.0;
DeguNaoto 14:a99f81878336 45 const double WheelRadius = 34.0;
DeguNaoto 14:a99f81878336 46 const double PulsePerRev = 400.0;
DeguNaoto 14:a99f81878336 47 const double Xo = 0.0;
DeguNaoto 14:a99f81878336 48 const double Yo = 0.0;
DeguNaoto 14:a99f81878336 49 const double Sitao = PI / 4.0;
DeguNaoto 14:a99f81878336 50
DeguNaoto 14:a99f81878336 51 /**
DeguNaoto 14:a99f81878336 52 * Class prototype.
DeguNaoto 14:a99f81878336 53 */
DeguNaoto 14:a99f81878336 54 MachineState Machine( WheelRadius, HalfTread, PulsePerRev, ENCOD_R_A, ENCOD_R_B, ENCOD_L_A, ENCOD_L_B, RATE, Xo, Yo, Sitao );
DeguNaoto 0:bd4719e15f7e 55
DeguNaoto 0:bd4719e15f7e 56 /**
DeguNaoto 0:bd4719e15f7e 57 * Set functions.
DeguNaoto 0:bd4719e15f7e 58 */
DeguNaoto 2:cf8ca6742db9 59
DeguNaoto 0:bd4719e15f7e 60 /***PID Controller***/
DeguNaoto 15:49ed1bbf3c1d 61 PID velocity_controller( MOVE_VELOCITY_Kc, MOVE_VELOCITY_TAUi, MOVE_VELOCITY_TAUd, RATE );
DeguNaoto 15:49ed1bbf3c1d 62 PID direction_controller( MOVE_DIRECTION_Kc, MOVE_DIRECTION_TAUi, MOVE_DIRECTION_TAUd, RATE );
DeguNaoto 15:49ed1bbf3c1d 63 //PID direction_controller( 9.0, 2637.0, 0.0, RATE );
DeguNaoto 0:bd4719e15f7e 64
DeguNaoto 0:bd4719e15f7e 65 /***Variables***/
DeguNaoto 14:a99f81878336 66 double targ_velocity = 0.0;
DeguNaoto 14:a99f81878336 67 double targ_sita = 0.0;
DeguNaoto 14:a99f81878336 68 double targ_swing_velocity = 0.0;
DeguNaoto 14:a99f81878336 69 double x1 = 0.0;
DeguNaoto 14:a99f81878336 70 double x2 = 0.0;
DeguNaoto 14:a99f81878336 71 double Vr = 0.0;
DeguNaoto 14:a99f81878336 72 double Vl = 0.0;
DeguNaoto 15:49ed1bbf3c1d 73 int step = 0;
DeguNaoto 15:49ed1bbf3c1d 74 int cylinder_step = 0;
DeguNaoto 13:87794ce49b50 75
DeguNaoto 0:bd4719e15f7e 76
DeguNaoto 0:bd4719e15f7e 77 /**
DeguNaoto 0:bd4719e15f7e 78 * Functions.
DeguNaoto 0:bd4719e15f7e 79 */
DeguNaoto 0:bd4719e15f7e 80
DeguNaoto 0:bd4719e15f7e 81 /***The function is motors initialize.***/
DeguNaoto 2:cf8ca6742db9 82 inline void initializeMotors()
DeguNaoto 2:cf8ca6742db9 83 {
DeguNaoto 0:bd4719e15f7e 84 Move_r_Motor_PWM.period_us(MOVE_R_PERIOD_US);
DeguNaoto 0:bd4719e15f7e 85 Move_l_Motor_PWM.period_us(MOVE_L_PERIOD_US);
DeguNaoto 0:bd4719e15f7e 86 }
DeguNaoto 0:bd4719e15f7e 87
DeguNaoto 0:bd4719e15f7e 88 /***The function is PID controller initialize.***/
DeguNaoto 2:cf8ca6742db9 89 inline void initializeControllers()
DeguNaoto 2:cf8ca6742db9 90 {
DeguNaoto 0:bd4719e15f7e 91
DeguNaoto 15:49ed1bbf3c1d 92 velocity_controller.setInputLimits(MOVE_VELOCITY_INPUT_LIMIT_BOTTOM, MOVE_VELOCITY_INPUT_LIMIT_TOP); //x1
DeguNaoto 15:49ed1bbf3c1d 93 direction_controller.setInputLimits(MOVE_DIRECTION_INPUT_LIMIT_BOTTOM , MOVE_DIRECTION_INPUT_LIMIT_TOP); //x2
DeguNaoto 2:cf8ca6742db9 94
DeguNaoto 0:bd4719e15f7e 95 //Pwm output from MOVE_OUTPUT_LIMIT_BOTTOM to MOVE_OUTPUT_LIMIT_TOP
DeguNaoto 15:49ed1bbf3c1d 96 velocity_controller.setOutputLimits(MOVE_VELOCITY_OUTPUT_LIMIT_BOTTOM, MOVE_VELOCITY_OUTPUT_LIMIT_TOP);
DeguNaoto 15:49ed1bbf3c1d 97 direction_controller.setOutputLimits(MOVE_DIRECTION_OUTPUT_LIMIT_BOTTOM, MOVE_DIRECTION_OUTPUT_LIMIT_TOP);
DeguNaoto 11:ce3083681efa 98
DeguNaoto 11:ce3083681efa 99 //set bias. 初期値
DeguNaoto 15:49ed1bbf3c1d 100 velocity_controller.setBias(MOVE_VELOCITY_BIAS);
DeguNaoto 15:49ed1bbf3c1d 101 direction_controller.setBias(MOVE_DIRECTION_BIAS);
DeguNaoto 2:cf8ca6742db9 102
DeguNaoto 0:bd4719e15f7e 103 //set mode.
DeguNaoto 11:ce3083681efa 104 velocity_controller.setMode(AUTO_MODE);
DeguNaoto 11:ce3083681efa 105 direction_controller.setMode(AUTO_MODE);
DeguNaoto 0:bd4719e15f7e 106 }
DeguNaoto 0:bd4719e15f7e 107
DeguNaoto 2:cf8ca6742db9 108 /***The function is Move on field.***/
DeguNaoto 0:bd4719e15f7e 109 //Right
DeguNaoto 14:a99f81878336 110 void Move_r(float speed1)
DeguNaoto 2:cf8ca6742db9 111 {
DeguNaoto 14:a99f81878336 112 if(speed1<0) {
DeguNaoto 0:bd4719e15f7e 113 Move_r_Motor_CCW = 1;
DeguNaoto 0:bd4719e15f7e 114 Move_r_Motor_CW = 0;
DeguNaoto 14:a99f81878336 115 Move_r_Motor_PWM = abs(speed1);
DeguNaoto 2:cf8ca6742db9 116 } else {
DeguNaoto 0:bd4719e15f7e 117 Move_r_Motor_CCW = 0;
DeguNaoto 0:bd4719e15f7e 118 Move_r_Motor_CW = 1;
DeguNaoto 14:a99f81878336 119 Move_r_Motor_PWM = speed1;
DeguNaoto 0:bd4719e15f7e 120 }
DeguNaoto 0:bd4719e15f7e 121 }
DeguNaoto 0:bd4719e15f7e 122 //Left
DeguNaoto 14:a99f81878336 123 void Move_l(float speed2)
DeguNaoto 2:cf8ca6742db9 124 {
DeguNaoto 15:49ed1bbf3c1d 125 if(speed2<0) {
DeguNaoto 0:bd4719e15f7e 126 Move_l_Motor_CCW = 1;
DeguNaoto 0:bd4719e15f7e 127 Move_l_Motor_CW = 0;
DeguNaoto 14:a99f81878336 128 Move_l_Motor_PWM = abs(speed2);
DeguNaoto 2:cf8ca6742db9 129 } else {
DeguNaoto 0:bd4719e15f7e 130 Move_l_Motor_CCW = 0;
DeguNaoto 0:bd4719e15f7e 131 Move_l_Motor_CW = 1;
DeguNaoto 14:a99f81878336 132 Move_l_Motor_PWM = speed2;
DeguNaoto 0:bd4719e15f7e 133 }
DeguNaoto 0:bd4719e15f7e 134 }
DeguNaoto 0:bd4719e15f7e 135
DeguNaoto 11:ce3083681efa 136 /***Caluculate state.***/
DeguNaoto 11:ce3083681efa 137
DeguNaoto 0:bd4719e15f7e 138 /***The function is following move speed.***/
DeguNaoto 15:49ed1bbf3c1d 139 void velocity_following(double velocity,double target_v)
DeguNaoto 11:ce3083681efa 140 {
DeguNaoto 15:49ed1bbf3c1d 141 velocity_controller.setSetPoint( ( float ) target_v );
DeguNaoto 13:87794ce49b50 142 velocity_controller.setProcessValue( ( float ) velocity );
DeguNaoto 13:87794ce49b50 143 x1 = ( double )velocity_controller.compute();
DeguNaoto 11:ce3083681efa 144 }
DeguNaoto 11:ce3083681efa 145
DeguNaoto 15:49ed1bbf3c1d 146 void sita_following(double sita,double target_s)
DeguNaoto 2:cf8ca6742db9 147 {
DeguNaoto 15:49ed1bbf3c1d 148 direction_controller.setSetPoint( ( float ) target_s );
DeguNaoto 13:87794ce49b50 149 direction_controller.setProcessValue( ( float ) sita );
DeguNaoto 13:87794ce49b50 150 x2 = ( double )direction_controller.compute();
DeguNaoto 0:bd4719e15f7e 151 }
DeguNaoto 11:ce3083681efa 152
DeguNaoto 11:ce3083681efa 153 inline void move_following()
DeguNaoto 2:cf8ca6742db9 154 {
DeguNaoto 14:a99f81878336 155 velocity_following( Machine.getVelocity(), targ_velocity );
DeguNaoto 14:a99f81878336 156 sita_following( Machine.getSita(), targ_sita );
DeguNaoto 14:a99f81878336 157 Vr = ( x1 + x2 ) / 2.0;
DeguNaoto 14:a99f81878336 158 Vl = ( x1 - x2 ) / 2.0;
DeguNaoto 14:a99f81878336 159 if( abs( Vr ) < 0.02 ) Vr = 0.0;
DeguNaoto 14:a99f81878336 160 if( abs( Vl ) < 0.02 ) Vl = 0.0;
DeguNaoto 14:a99f81878336 161 Move_r( ( float ) Vr );
DeguNaoto 14:a99f81878336 162 Move_l( ( float ) Vl );
DeguNaoto 0:bd4719e15f7e 163 }
DeguNaoto 11:ce3083681efa 164
DeguNaoto 0:bd4719e15f7e 165 /***Emergency stop.***/
DeguNaoto 2:cf8ca6742db9 166 void Emergency_toggle()
DeguNaoto 2:cf8ca6742db9 167 {
DeguNaoto 4:51d87d2b698c 168 if(edge_circle) {
DeguNaoto 4:51d87d2b698c 169 edge_circle=0;
DeguNaoto 0:bd4719e15f7e 170 if(toggle) toggle=0,Emergency_stop=0;
DeguNaoto 0:bd4719e15f7e 171 else toggle=1,Emergency_stop=1;
DeguNaoto 0:bd4719e15f7e 172 }
DeguNaoto 0:bd4719e15f7e 173 }
DeguNaoto 0:bd4719e15f7e 174
DeguNaoto 0:bd4719e15f7e 175 #endif /*machine.h*/