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@11:ce3083681efa, 2015-09-12 (annotated)

Committer:
DeguNaoto
Date:
Sat Sep 12 10:35:51 2015 +0000
Revision:
11:ce3083681efa
Parent:
10:36f81cc33202
Child:
12:24a444bed6a0
??????????????

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 void Move_r(float speed1);
DeguNaoto 0:bd4719e15f7e 24 void Move_l(float speed2);
DeguNaoto 0:bd4719e15f7e 25 inline void mesure_move_r_velocity();
DeguNaoto 0:bd4719e15f7e 26 inline void mesure_move_l_velocity();
DeguNaoto 0:bd4719e15f7e 27 inline void mesure_swing_velocity();
DeguNaoto 0:bd4719e15f7e 28 inline void data_clear();
DeguNaoto 0:bd4719e15f7e 29 inline void data_check();
DeguNaoto 0:bd4719e15f7e 30 inline void SBDBT_interrupt();
DeguNaoto 0:bd4719e15f7e 31 inline void initializeSBDBT();
DeguNaoto 0:bd4719e15f7e 32 inline void Move_r_speed_following();
DeguNaoto 0:bd4719e15f7e 33 inline void Move_l_speed_following();
DeguNaoto 0:bd4719e15f7e 34 inline void Swing_speed_following();
DeguNaoto 0:bd4719e15f7e 35 void Emergency_toggle();
DeguNaoto 4:51d87d2b698c 36 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;
DeguNaoto 4:51d87d2b698c 37 int step=0,cylinder_step=0;
DeguNaoto 0:bd4719e15f7e 38
DeguNaoto 0:bd4719e15f7e 39 /**
DeguNaoto 0:bd4719e15f7e 40 * include function header files.
DeguNaoto 0:bd4719e15f7e 41 */
DeguNaoto 0:bd4719e15f7e 42 #include "communicate.h"
DeguNaoto 2:cf8ca6742db9 43
DeguNaoto 0:bd4719e15f7e 44 /**
DeguNaoto 0:bd4719e15f7e 45 * Defines
DeguNaoto 0:bd4719e15f7e 46 */
DeguNaoto 0:bd4719e15f7e 47 #define RATE 0.01
DeguNaoto 11:ce3083681efa 48 #define PI 3.14159265359
DeguNaoto 11:ce3083681efa 49 #define speed 10000.0
DeguNaoto 0:bd4719e15f7e 50
DeguNaoto 0:bd4719e15f7e 51 /**
DeguNaoto 0:bd4719e15f7e 52 * Set functions.
DeguNaoto 0:bd4719e15f7e 53 */
DeguNaoto 2:cf8ca6742db9 54
DeguNaoto 0:bd4719e15f7e 55 /***PID Controller***/
DeguNaoto 11:ce3083681efa 56 PID velocity_controller(18.0,5274.0 ,0.0,RATE);
DeguNaoto 11:ce3083681efa 57 PID direction_controller(18.0,5274.0,0.0,RATE);
DeguNaoto 0:bd4719e15f7e 58 PID swing_controller(SWING_Kc, SWING_TAUi, SWING_TAUd, RATE);
DeguNaoto 0:bd4719e15f7e 59
DeguNaoto 0:bd4719e15f7e 60 /***Variables***/
DeguNaoto 0:bd4719e15f7e 61 double Pulses_move_r=0.0;
DeguNaoto 0:bd4719e15f7e 62 double Pulses_move_l=0.0;
DeguNaoto 0:bd4719e15f7e 63 double Pulses_swing=0.0;
DeguNaoto 0:bd4719e15f7e 64 double PrefPulses_move_r=0.0;
DeguNaoto 0:bd4719e15f7e 65 double PrefPulses_move_l=0.0;
DeguNaoto 0:bd4719e15f7e 66 double PrefPulses_swing=0.0;
DeguNaoto 0:bd4719e15f7e 67 double swing_velocity=0.0;
DeguNaoto 11:ce3083681efa 68 double velocity=0.0;
DeguNaoto 11:ce3083681efa 69 double targ_velocity=0.0;
DeguNaoto 11:ce3083681efa 70 double targ_sita=0.0;
DeguNaoto 0:bd4719e15f7e 71 double targ_swing_velocity=0.0;
DeguNaoto 11:ce3083681efa 72 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
DeguNaoto 11:ce3083681efa 73 double x1=0.0,x2=0.0;
DeguNaoto 11:ce3083681efa 74 double Vr=0.0,Vl=0.0;
DeguNaoto 11:ce3083681efa 75 const double d=375.0;
DeguNaoto 11:ce3083681efa 76 const double r_wheel=34.0;
DeguNaoto 0:bd4719e15f7e 77
DeguNaoto 0:bd4719e15f7e 78 /**
DeguNaoto 0:bd4719e15f7e 79 * Functions.
DeguNaoto 0:bd4719e15f7e 80 */
DeguNaoto 0:bd4719e15f7e 81
DeguNaoto 0:bd4719e15f7e 82 /***The function is motors initialize.***/
DeguNaoto 2:cf8ca6742db9 83 inline void initializeMotors()
DeguNaoto 2:cf8ca6742db9 84 {
DeguNaoto 0:bd4719e15f7e 85 Move_r_Motor_PWM.period_us(MOVE_R_PERIOD_US);
DeguNaoto 0:bd4719e15f7e 86 Move_l_Motor_PWM.period_us(MOVE_L_PERIOD_US);
DeguNaoto 0:bd4719e15f7e 87 Motor_swing_pwm.period_us(SWING_PERIOD_US);
DeguNaoto 0:bd4719e15f7e 88 }
DeguNaoto 0:bd4719e15f7e 89
DeguNaoto 0:bd4719e15f7e 90 /***The function is PID controller initialize.***/
DeguNaoto 2:cf8ca6742db9 91 inline void initializeControllers()
DeguNaoto 2:cf8ca6742db9 92 {
DeguNaoto 0:bd4719e15f7e 93
DeguNaoto 11:ce3083681efa 94 velocity_controller.setInputLimits(-200000.0, 200000.0); //x1
DeguNaoto 11:ce3083681efa 95 direction_controller.setInputLimits(-10.0, 10.0); //x2
DeguNaoto 0:bd4719e15f7e 96 swing_controller.setInputLimits(SWING_INPUT_LIMIT_BOTTOM, SWING_INPUT_LIMIT_TOP);
DeguNaoto 2:cf8ca6742db9 97
DeguNaoto 0:bd4719e15f7e 98 //Pwm output from MOVE_OUTPUT_LIMIT_BOTTOM to MOVE_OUTPUT_LIMIT_TOP
DeguNaoto 11:ce3083681efa 99 velocity_controller.setOutputLimits(-1.0, 1.0);
DeguNaoto 11:ce3083681efa 100 direction_controller.setOutputLimits(-1.0, 1.0);
DeguNaoto 0:bd4719e15f7e 101 swing_controller.setOutputLimits(SWING_OUTPUT_LIMIT_BOTTOM, SWING_OUTPUT_LIMIT_TOP);
DeguNaoto 11:ce3083681efa 102
DeguNaoto 11:ce3083681efa 103 //set bias. 初期値
DeguNaoto 11:ce3083681efa 104 velocity_controller.setBias(0.0);
DeguNaoto 11:ce3083681efa 105 direction_controller.setBias(0.0);
DeguNaoto 0:bd4719e15f7e 106 swing_controller.setBias(SWING_BIAS);
DeguNaoto 2:cf8ca6742db9 107
DeguNaoto 0:bd4719e15f7e 108 //set mode.
DeguNaoto 11:ce3083681efa 109 velocity_controller.setMode(AUTO_MODE);
DeguNaoto 11:ce3083681efa 110 direction_controller.setMode(AUTO_MODE);
DeguNaoto 0:bd4719e15f7e 111 swing_controller.setMode(AUTO_MODE);
DeguNaoto 0:bd4719e15f7e 112 }
DeguNaoto 0:bd4719e15f7e 113
DeguNaoto 2:cf8ca6742db9 114 /***The function is Move on field.***/
DeguNaoto 0:bd4719e15f7e 115 //Right
DeguNaoto 2:cf8ca6742db9 116 void Move_r(float speed1)
DeguNaoto 2:cf8ca6742db9 117 {
DeguNaoto 2:cf8ca6742db9 118 if(speed1<0) {
DeguNaoto 0:bd4719e15f7e 119 Move_r_Motor_CCW = 1;
DeguNaoto 0:bd4719e15f7e 120 Move_r_Motor_CW = 0;
DeguNaoto 0:bd4719e15f7e 121 Move_r_Motor_PWM = abs(speed1);
DeguNaoto 2:cf8ca6742db9 122 } else {
DeguNaoto 0:bd4719e15f7e 123 Move_r_Motor_CCW = 0;
DeguNaoto 0:bd4719e15f7e 124 Move_r_Motor_CW = 1;
DeguNaoto 0:bd4719e15f7e 125 Move_r_Motor_PWM = speed1;
DeguNaoto 0:bd4719e15f7e 126 }
DeguNaoto 0:bd4719e15f7e 127 }
DeguNaoto 0:bd4719e15f7e 128 //Left
DeguNaoto 2:cf8ca6742db9 129 void Move_l(float speed2)
DeguNaoto 2:cf8ca6742db9 130 {
DeguNaoto 2:cf8ca6742db9 131 if(speed2<0) {
DeguNaoto 0:bd4719e15f7e 132 Move_l_Motor_CCW = 1;
DeguNaoto 0:bd4719e15f7e 133 Move_l_Motor_CW = 0;
DeguNaoto 0:bd4719e15f7e 134 Move_l_Motor_PWM = abs(speed2);
DeguNaoto 2:cf8ca6742db9 135 } else {
DeguNaoto 0:bd4719e15f7e 136 Move_l_Motor_CCW = 0;
DeguNaoto 0:bd4719e15f7e 137 Move_l_Motor_CW = 1;
DeguNaoto 0:bd4719e15f7e 138 Move_l_Motor_PWM = speed2;
DeguNaoto 0:bd4719e15f7e 139 }
DeguNaoto 0:bd4719e15f7e 140 }
DeguNaoto 0:bd4719e15f7e 141
DeguNaoto 11:ce3083681efa 142 /***Caluculate state.***/
DeguNaoto 11:ce3083681efa 143 inline void mesure_state()
DeguNaoto 2:cf8ca6742db9 144 {
DeguNaoto 0:bd4719e15f7e 145 Pulses_move_r = (double)Move_r_sense.getPulses();
DeguNaoto 11:ce3083681efa 146 dlr = (((Pulses_move_r - PrefPulses_move_r)/400.0)*2.0*PI)*r_wheel;
DeguNaoto 0:bd4719e15f7e 147 PrefPulses_move_r = Pulses_move_r;
DeguNaoto 11:ce3083681efa 148 Pulses_move_l = (double)Move_l_sense.getPulses();
DeguNaoto 11:ce3083681efa 149 dll = (((Pulses_move_l - PrefPulses_move_l)/400.0)*2.0*PI)*r_wheel;
DeguNaoto 11:ce3083681efa 150 PrefPulses_move_l = Pulses_move_l;
DeguNaoto 11:ce3083681efa 151 vr=dlr/RATE,vl=dll/RATE;
DeguNaoto 11:ce3083681efa 152 velocity=(vr+vl)/2.0;
DeguNaoto 11:ce3083681efa 153 dsita=(dlr - dll)/(2.0*d);
DeguNaoto 11:ce3083681efa 154 dx=((dlr+dll)/2.0)*cos(sita+dsita/2.0);
DeguNaoto 11:ce3083681efa 155 dy=((dlr+dll)/2.0)*sin(sita+dsita/2.0);
DeguNaoto 11:ce3083681efa 156 sita+=dsita,x+=dx,y+=dy;
DeguNaoto 0:bd4719e15f7e 157 }
DeguNaoto 11:ce3083681efa 158
DeguNaoto 2:cf8ca6742db9 159 inline void mesure_swing_velocity()
DeguNaoto 2:cf8ca6742db9 160 {
DeguNaoto 4:51d87d2b698c 161 Pulses_swing = (double)Swing_speed_sense.getPulses();
DeguNaoto 0:bd4719e15f7e 162 swing_velocity = Pulses_swing - PrefPulses_swing;
DeguNaoto 0:bd4719e15f7e 163 PrefPulses_swing = Pulses_swing;
DeguNaoto 0:bd4719e15f7e 164 }
DeguNaoto 0:bd4719e15f7e 165
DeguNaoto 0:bd4719e15f7e 166 /***The function is following move speed.***/
DeguNaoto 11:ce3083681efa 167 float cont_swing=0.0;
DeguNaoto 0:bd4719e15f7e 168
DeguNaoto 11:ce3083681efa 169 inline void velocity_following()
DeguNaoto 11:ce3083681efa 170 {
DeguNaoto 11:ce3083681efa 171 velocity_controller.setSetPoint((float)targ_velocity);
DeguNaoto 11:ce3083681efa 172 velocity_controller.setProcessValue((float)velocity);
DeguNaoto 11:ce3083681efa 173 x1 = (double)velocity_controller.compute();
DeguNaoto 11:ce3083681efa 174 }
DeguNaoto 11:ce3083681efa 175
DeguNaoto 11:ce3083681efa 176 inline void sita_following()
DeguNaoto 2:cf8ca6742db9 177 {
DeguNaoto 11:ce3083681efa 178 direction_controller.setSetPoint((float)targ_sita);
DeguNaoto 11:ce3083681efa 179 direction_controller.setProcessValue((float)sita);
DeguNaoto 11:ce3083681efa 180 //direction_controller.setSetPoint(0.0); //目標値とのずれをなくす
DeguNaoto 11:ce3083681efa 181 //direction_controller.setProcessValue(-y);
DeguNaoto 11:ce3083681efa 182 x2 = (double)direction_controller.compute();
DeguNaoto 0:bd4719e15f7e 183 }
DeguNaoto 11:ce3083681efa 184
DeguNaoto 11:ce3083681efa 185 inline void move_following()
DeguNaoto 2:cf8ca6742db9 186 {
DeguNaoto 11:ce3083681efa 187 velocity_following();
DeguNaoto 11:ce3083681efa 188 sita_following();
DeguNaoto 11:ce3083681efa 189 Vr=(x1+x2)/2.0;
DeguNaoto 11:ce3083681efa 190 Vl=(x1-x2)/2.0;
DeguNaoto 11:ce3083681efa 191 if(abs(Vr)<0.05) Vr=0.0;
DeguNaoto 11:ce3083681efa 192 if(abs(Vl)<0.05) Vl=0.0;
DeguNaoto 11:ce3083681efa 193 Move_r((float)Vr);
DeguNaoto 11:ce3083681efa 194 Move_l((float)Vl);
DeguNaoto 0:bd4719e15f7e 195 }
DeguNaoto 11:ce3083681efa 196
DeguNaoto 2:cf8ca6742db9 197 inline void Swing_speed_following()
DeguNaoto 2:cf8ca6742db9 198 {
DeguNaoto 0:bd4719e15f7e 199 swing_controller.setSetPoint((float)targ_swing_velocity);
DeguNaoto 0:bd4719e15f7e 200 swing_controller.setProcessValue((float)swing_velocity);
DeguNaoto 0:bd4719e15f7e 201 cont_swing = swing_controller.compute();
DeguNaoto 0:bd4719e15f7e 202 Motor_swing_pwm = cont_swing;
DeguNaoto 0:bd4719e15f7e 203 }
DeguNaoto 0:bd4719e15f7e 204
DeguNaoto 4:51d87d2b698c 205
DeguNaoto 0:bd4719e15f7e 206 /***Emergency stop.***/
DeguNaoto 2:cf8ca6742db9 207 void Emergency_toggle()
DeguNaoto 2:cf8ca6742db9 208 {
DeguNaoto 4:51d87d2b698c 209 if(edge_circle) {
DeguNaoto 4:51d87d2b698c 210 edge_circle=0;
DeguNaoto 0:bd4719e15f7e 211 if(toggle) toggle=0,Emergency_stop=0;
DeguNaoto 0:bd4719e15f7e 212 else toggle=1,Emergency_stop=1;
DeguNaoto 0:bd4719e15f7e 213 }
DeguNaoto 0:bd4719e15f7e 214 }
DeguNaoto 0:bd4719e15f7e 215
DeguNaoto 0:bd4719e15f7e 216 #endif /*machine.h*/