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@18:526124eef0d1, 2015-09-17 (annotated)

Committer:
DeguNaoto
Date:
Thu Sep 17 02:58:36 2015 +0000
Revision:
18:526124eef0d1
Parent:
12:24a444bed6a0
Child:
19:598211462097
???????????????IM920?????????????

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