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@12:24a444bed6a0, 2015-09-15 (annotated)

Committer:
DeguNaoto
Date:
Tue Sep 15 08:32:03 2015 +0000
Revision:
12:24a444bed6a0
Parent:
11:ce3083681efa
Child:
13:87794ce49b50
Child:
18:526124eef0d1
20150913 ???????

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 12:24a444bed6a0 49 //#define speed 10000.0
DeguNaoto 11:ce3083681efa 50 #define speed 10000.0
DeguNaoto 0:bd4719e15f7e 51
DeguNaoto 0:bd4719e15f7e 52 /**
DeguNaoto 0:bd4719e15f7e 53 * Set functions.
DeguNaoto 0:bd4719e15f7e 54 */
DeguNaoto 2:cf8ca6742db9 55
DeguNaoto 0:bd4719e15f7e 56 /***PID Controller***/
DeguNaoto 11:ce3083681efa 57 PID velocity_controller(18.0,5274.0 ,0.0,RATE);
DeguNaoto 12:24a444bed6a0 58 PID direction_controller(9.0,2637.0,0.0,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 }
DeguNaoto 0:bd4719e15f7e 88
DeguNaoto 0:bd4719e15f7e 89 /***The function is PID controller initialize.***/
DeguNaoto 2:cf8ca6742db9 90 inline void initializeControllers()
DeguNaoto 2:cf8ca6742db9 91 {
DeguNaoto 0:bd4719e15f7e 92
DeguNaoto 11:ce3083681efa 93 velocity_controller.setInputLimits(-200000.0, 200000.0); //x1
DeguNaoto 11:ce3083681efa 94 direction_controller.setInputLimits(-10.0, 10.0); //x2
DeguNaoto 2:cf8ca6742db9 95
DeguNaoto 0:bd4719e15f7e 96 //Pwm output from MOVE_OUTPUT_LIMIT_BOTTOM to MOVE_OUTPUT_LIMIT_TOP
DeguNaoto 11:ce3083681efa 97 velocity_controller.setOutputLimits(-1.0, 1.0);
DeguNaoto 11:ce3083681efa 98 direction_controller.setOutputLimits(-1.0, 1.0);
DeguNaoto 11:ce3083681efa 99
DeguNaoto 11:ce3083681efa 100 //set bias. 初期値
DeguNaoto 11:ce3083681efa 101 velocity_controller.setBias(0.0);
DeguNaoto 11:ce3083681efa 102 direction_controller.setBias(0.0);
DeguNaoto 2:cf8ca6742db9 103
DeguNaoto 0:bd4719e15f7e 104 //set mode.
DeguNaoto 11:ce3083681efa 105 velocity_controller.setMode(AUTO_MODE);
DeguNaoto 11:ce3083681efa 106 direction_controller.setMode(AUTO_MODE);
DeguNaoto 0:bd4719e15f7e 107 }
DeguNaoto 0:bd4719e15f7e 108
DeguNaoto 2:cf8ca6742db9 109 /***The function is Move on field.***/
DeguNaoto 0:bd4719e15f7e 110 //Right
DeguNaoto 2:cf8ca6742db9 111 void Move_r(float speed1)
DeguNaoto 2:cf8ca6742db9 112 {
DeguNaoto 2:cf8ca6742db9 113 if(speed1<0) {
DeguNaoto 0:bd4719e15f7e 114 Move_r_Motor_CCW = 1;
DeguNaoto 0:bd4719e15f7e 115 Move_r_Motor_CW = 0;
DeguNaoto 0:bd4719e15f7e 116 Move_r_Motor_PWM = abs(speed1);
DeguNaoto 2:cf8ca6742db9 117 } else {
DeguNaoto 0:bd4719e15f7e 118 Move_r_Motor_CCW = 0;
DeguNaoto 0:bd4719e15f7e 119 Move_r_Motor_CW = 1;
DeguNaoto 0:bd4719e15f7e 120 Move_r_Motor_PWM = speed1;
DeguNaoto 0:bd4719e15f7e 121 }
DeguNaoto 0:bd4719e15f7e 122 }
DeguNaoto 0:bd4719e15f7e 123 //Left
DeguNaoto 2:cf8ca6742db9 124 void Move_l(float speed2)
DeguNaoto 2:cf8ca6742db9 125 {
DeguNaoto 2:cf8ca6742db9 126 if(speed2<0) {
DeguNaoto 0:bd4719e15f7e 127 Move_l_Motor_CCW = 1;
DeguNaoto 0:bd4719e15f7e 128 Move_l_Motor_CW = 0;
DeguNaoto 0:bd4719e15f7e 129 Move_l_Motor_PWM = abs(speed2);
DeguNaoto 2:cf8ca6742db9 130 } else {
DeguNaoto 0:bd4719e15f7e 131 Move_l_Motor_CCW = 0;
DeguNaoto 0:bd4719e15f7e 132 Move_l_Motor_CW = 1;
DeguNaoto 0:bd4719e15f7e 133 Move_l_Motor_PWM = speed2;
DeguNaoto 0:bd4719e15f7e 134 }
DeguNaoto 0:bd4719e15f7e 135 }
DeguNaoto 0:bd4719e15f7e 136
DeguNaoto 11:ce3083681efa 137 /***Caluculate state.***/
DeguNaoto 11:ce3083681efa 138 inline void mesure_state()
DeguNaoto 2:cf8ca6742db9 139 {
DeguNaoto 0:bd4719e15f7e 140 Pulses_move_r = (double)Move_r_sense.getPulses();
DeguNaoto 11:ce3083681efa 141 dlr = (((Pulses_move_r - PrefPulses_move_r)/400.0)*2.0*PI)*r_wheel;
DeguNaoto 0:bd4719e15f7e 142 PrefPulses_move_r = Pulses_move_r;
DeguNaoto 11:ce3083681efa 143 Pulses_move_l = (double)Move_l_sense.getPulses();
DeguNaoto 11:ce3083681efa 144 dll = (((Pulses_move_l - PrefPulses_move_l)/400.0)*2.0*PI)*r_wheel;
DeguNaoto 11:ce3083681efa 145 PrefPulses_move_l = Pulses_move_l;
DeguNaoto 11:ce3083681efa 146 vr=dlr/RATE,vl=dll/RATE;
DeguNaoto 11:ce3083681efa 147 velocity=(vr+vl)/2.0;
DeguNaoto 11:ce3083681efa 148 dsita=(dlr - dll)/(2.0*d);
DeguNaoto 11:ce3083681efa 149 dx=((dlr+dll)/2.0)*cos(sita+dsita/2.0);
DeguNaoto 11:ce3083681efa 150 dy=((dlr+dll)/2.0)*sin(sita+dsita/2.0);
DeguNaoto 11:ce3083681efa 151 sita+=dsita,x+=dx,y+=dy;
DeguNaoto 0:bd4719e15f7e 152 }
DeguNaoto 11:ce3083681efa 153
DeguNaoto 0:bd4719e15f7e 154 /***The function is following move speed.***/
DeguNaoto 11:ce3083681efa 155 float cont_swing=0.0;
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 0:bd4719e15f7e 195 #endif /*machine.h*/