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@20:22efb19bb82f, 2015-09-17 (annotated)

Committer:
DeguNaoto
Date:
Thu Sep 17 07:01:31 2015 +0000
Revision:
20:22efb19bb82f
Parent:
19:598211462097
Child:
21:79b94cb922f0
Enable????????

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