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@10:36f81cc33202, 2015-09-09 (annotated)

Committer:
DeguNaoto
Date:
Wed Sep 09 11:20:07 2015 +0000
Revision:
10:36f81cc33202
Parent:
4:51d87d2b698c
Child:
11:ce3083681efa
20150909???;

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 0:bd4719e15f7e 48
DeguNaoto 0:bd4719e15f7e 49 /**
DeguNaoto 0:bd4719e15f7e 50 * Set functions.
DeguNaoto 0:bd4719e15f7e 51 */
DeguNaoto 2:cf8ca6742db9 52
DeguNaoto 0:bd4719e15f7e 53 /***PID Controller***/
DeguNaoto 0:bd4719e15f7e 54 PID move_r_controller(MOVE_R_Kc, MOVE_R_TAUi, MOVE_R_TAUd, RATE);
DeguNaoto 0:bd4719e15f7e 55 PID move_l_controller(MOVE_L_Kc, MOVE_L_TAUi, MOVE_L_TAUd, RATE);
DeguNaoto 0:bd4719e15f7e 56 PID swing_controller(SWING_Kc, SWING_TAUi, SWING_TAUd, RATE);
DeguNaoto 0:bd4719e15f7e 57
DeguNaoto 0:bd4719e15f7e 58 /***Variables***/
DeguNaoto 0:bd4719e15f7e 59 double Pulses_move_r=0.0;
DeguNaoto 0:bd4719e15f7e 60 double Pulses_move_l=0.0;
DeguNaoto 0:bd4719e15f7e 61 double Pulses_swing=0.0;
DeguNaoto 0:bd4719e15f7e 62 double PrefPulses_move_r=0.0;
DeguNaoto 0:bd4719e15f7e 63 double PrefPulses_move_l=0.0;
DeguNaoto 0:bd4719e15f7e 64 double PrefPulses_swing=0.0;
DeguNaoto 0:bd4719e15f7e 65 double move_r_velocity=0.0;
DeguNaoto 0:bd4719e15f7e 66 double move_l_velocity=0.0;
DeguNaoto 0:bd4719e15f7e 67 double swing_velocity=0.0;
DeguNaoto 0:bd4719e15f7e 68 //double Prefmove_r_velocity=0.0;
DeguNaoto 0:bd4719e15f7e 69 //double Prefmove_l_velocity=0.0;
DeguNaoto 0:bd4719e15f7e 70 double targ_move_r_velocity=0.0;
DeguNaoto 0:bd4719e15f7e 71 double targ_move_l_velocity=0.0;
DeguNaoto 0:bd4719e15f7e 72 double targ_swing_velocity=0.0;
DeguNaoto 0:bd4719e15f7e 73 //const double alpha=0.9;
DeguNaoto 0:bd4719e15f7e 74
DeguNaoto 0:bd4719e15f7e 75 /**
DeguNaoto 0:bd4719e15f7e 76 * Functions.
DeguNaoto 0:bd4719e15f7e 77 */
DeguNaoto 0:bd4719e15f7e 78
DeguNaoto 0:bd4719e15f7e 79 /***The function is motors initialize.***/
DeguNaoto 2:cf8ca6742db9 80 inline void initializeMotors()
DeguNaoto 2:cf8ca6742db9 81 {
DeguNaoto 0:bd4719e15f7e 82 Move_r_Motor_PWM.period_us(MOVE_R_PERIOD_US);
DeguNaoto 0:bd4719e15f7e 83 Move_l_Motor_PWM.period_us(MOVE_L_PERIOD_US);
DeguNaoto 0:bd4719e15f7e 84 Motor_swing_pwm.period_us(SWING_PERIOD_US);
DeguNaoto 0:bd4719e15f7e 85 }
DeguNaoto 0:bd4719e15f7e 86
DeguNaoto 0:bd4719e15f7e 87 /***The function is PID controller initialize.***/
DeguNaoto 2:cf8ca6742db9 88 inline void initializeControllers()
DeguNaoto 2:cf8ca6742db9 89 {
DeguNaoto 0:bd4719e15f7e 90
DeguNaoto 0:bd4719e15f7e 91 move_r_controller.setInputLimits(MOVE_R_INPUT_LIMIT_BOTTOM, MOVE_R_INPUT_LIMIT_TOP);
DeguNaoto 0:bd4719e15f7e 92 move_l_controller.setInputLimits(MOVE_L_INPUT_LIMIT_BOTTOM, MOVE_L_INPUT_LIMIT_TOP);
DeguNaoto 0:bd4719e15f7e 93 swing_controller.setInputLimits(SWING_INPUT_LIMIT_BOTTOM, SWING_INPUT_LIMIT_TOP);
DeguNaoto 2:cf8ca6742db9 94
DeguNaoto 0:bd4719e15f7e 95 //Pwm output from MOVE_OUTPUT_LIMIT_BOTTOM to MOVE_OUTPUT_LIMIT_TOP
DeguNaoto 0:bd4719e15f7e 96 move_r_controller.setOutputLimits(MOVE_R_OUTPUT_LIMIT_BOTTOM, MOVE_R_OUTPUT_LIMIT_TOP);
DeguNaoto 0:bd4719e15f7e 97 move_l_controller.setOutputLimits(MOVE_L_OUTPUT_LIMIT_BOTTOM, MOVE_L_OUTPUT_LIMIT_TOP);
DeguNaoto 0:bd4719e15f7e 98 swing_controller.setOutputLimits(SWING_OUTPUT_LIMIT_BOTTOM, SWING_OUTPUT_LIMIT_TOP);
DeguNaoto 2:cf8ca6742db9 99
DeguNaoto 0:bd4719e15f7e 100 //set bias.
DeguNaoto 0:bd4719e15f7e 101 move_r_controller.setBias(MOVE_R_BIAS);
DeguNaoto 0:bd4719e15f7e 102 move_l_controller.setBias(MOVE_L_BIAS);
DeguNaoto 0:bd4719e15f7e 103 swing_controller.setBias(SWING_BIAS);
DeguNaoto 2:cf8ca6742db9 104
DeguNaoto 0:bd4719e15f7e 105 //set mode.
DeguNaoto 0:bd4719e15f7e 106 move_r_controller.setMode(AUTO_MODE);
DeguNaoto 0:bd4719e15f7e 107 move_l_controller.setMode(AUTO_MODE);
DeguNaoto 0:bd4719e15f7e 108 swing_controller.setMode(AUTO_MODE);
DeguNaoto 0:bd4719e15f7e 109 }
DeguNaoto 0:bd4719e15f7e 110
DeguNaoto 2:cf8ca6742db9 111 /***The function is Move on field.***/
DeguNaoto 0:bd4719e15f7e 112 //Right
DeguNaoto 2:cf8ca6742db9 113 void Move_r(float speed1)
DeguNaoto 2:cf8ca6742db9 114 {
DeguNaoto 2:cf8ca6742db9 115 if(speed1<0) {
DeguNaoto 0:bd4719e15f7e 116 Move_r_Motor_CCW = 1;
DeguNaoto 0:bd4719e15f7e 117 Move_r_Motor_CW = 0;
DeguNaoto 0:bd4719e15f7e 118 Move_r_Motor_PWM = abs(speed1);
DeguNaoto 2:cf8ca6742db9 119 } else {
DeguNaoto 0:bd4719e15f7e 120 Move_r_Motor_CCW = 0;
DeguNaoto 0:bd4719e15f7e 121 Move_r_Motor_CW = 1;
DeguNaoto 0:bd4719e15f7e 122 Move_r_Motor_PWM = speed1;
DeguNaoto 0:bd4719e15f7e 123 }
DeguNaoto 0:bd4719e15f7e 124 }
DeguNaoto 0:bd4719e15f7e 125 //Left
DeguNaoto 2:cf8ca6742db9 126 void Move_l(float speed2)
DeguNaoto 2:cf8ca6742db9 127 {
DeguNaoto 2:cf8ca6742db9 128 if(speed2<0) {
DeguNaoto 0:bd4719e15f7e 129 Move_l_Motor_CCW = 1;
DeguNaoto 0:bd4719e15f7e 130 Move_l_Motor_CW = 0;
DeguNaoto 0:bd4719e15f7e 131 Move_l_Motor_PWM = abs(speed2);
DeguNaoto 2:cf8ca6742db9 132 } else {
DeguNaoto 0:bd4719e15f7e 133 Move_l_Motor_CCW = 0;
DeguNaoto 0:bd4719e15f7e 134 Move_l_Motor_CW = 1;
DeguNaoto 0:bd4719e15f7e 135 Move_l_Motor_PWM = speed2;
DeguNaoto 0:bd4719e15f7e 136 }
DeguNaoto 0:bd4719e15f7e 137 }
DeguNaoto 0:bd4719e15f7e 138
DeguNaoto 0:bd4719e15f7e 139 /***Caluculate move velocity.***/
DeguNaoto 2:cf8ca6742db9 140 inline void mesure_move_r_velocity()
DeguNaoto 2:cf8ca6742db9 141 {
DeguNaoto 0:bd4719e15f7e 142 Pulses_move_r = (double)Move_r_sense.getPulses();
DeguNaoto 0:bd4719e15f7e 143 move_r_velocity = Pulses_move_r - PrefPulses_move_r;
DeguNaoto 0:bd4719e15f7e 144 //move_r_velocity += (1.0-alpha)*Prefmove_r_velocity;
DeguNaoto 0:bd4719e15f7e 145 //Prefmove_r_velocity = move_r_velocity;
DeguNaoto 0:bd4719e15f7e 146 PrefPulses_move_r = Pulses_move_r;
DeguNaoto 0:bd4719e15f7e 147 }
DeguNaoto 2:cf8ca6742db9 148 inline void mesure_move_l_velocity()
DeguNaoto 2:cf8ca6742db9 149 {
DeguNaoto 0:bd4719e15f7e 150 Pulses_move_l = (double)Move_l_sense.getPulses();
DeguNaoto 0:bd4719e15f7e 151 move_l_velocity = Pulses_move_l - PrefPulses_move_l;
DeguNaoto 0:bd4719e15f7e 152 //move_l_velocity += (1.0-alpha)*Prefmove_l_velocity;
DeguNaoto 0:bd4719e15f7e 153 //Prefmove_l_velocity = move_l_velocity;
DeguNaoto 0:bd4719e15f7e 154 PrefPulses_move_l = Pulses_move_l;
DeguNaoto 0:bd4719e15f7e 155 }
DeguNaoto 2:cf8ca6742db9 156 inline void mesure_swing_velocity()
DeguNaoto 2:cf8ca6742db9 157 {
DeguNaoto 4:51d87d2b698c 158 Pulses_swing = (double)Swing_speed_sense.getPulses();
DeguNaoto 0:bd4719e15f7e 159 swing_velocity = Pulses_swing - PrefPulses_swing;
DeguNaoto 0:bd4719e15f7e 160 PrefPulses_swing = Pulses_swing;
DeguNaoto 0:bd4719e15f7e 161 }
DeguNaoto 0:bd4719e15f7e 162
DeguNaoto 0:bd4719e15f7e 163 /***The function is following move speed.***/
DeguNaoto 0:bd4719e15f7e 164 float cont_move_r=0.0,cont_move_l=0.0,cont_swing=0.0;
DeguNaoto 0:bd4719e15f7e 165 short r=0,l=0;
DeguNaoto 0:bd4719e15f7e 166
DeguNaoto 2:cf8ca6742db9 167 inline void Move_r_speed_following()
DeguNaoto 2:cf8ca6742db9 168 {
DeguNaoto 10:36f81cc33202 169 //move_r_controller.setSetPoint((float)targ_move_r_velocity);
DeguNaoto 10:36f81cc33202 170 //move_r_controller.setProcessValue((float)move_r_velocity);
DeguNaoto 10:36f81cc33202 171 move_r_controller.setSetPoint(0.0);
DeguNaoto 10:36f81cc33202 172 move_r_controller.setProcessValue(0.0);
DeguNaoto 0:bd4719e15f7e 173 cont_move_r = move_r_controller.compute();
DeguNaoto 0:bd4719e15f7e 174 Move_r(cont_move_r);
DeguNaoto 0:bd4719e15f7e 175 }
DeguNaoto 2:cf8ca6742db9 176 inline void Move_l_speed_following()
DeguNaoto 2:cf8ca6742db9 177 {
DeguNaoto 10:36f81cc33202 178 //move_l_controller.setSetPoint((float)targ_move_l_velocity);
DeguNaoto 10:36f81cc33202 179 //move_l_controller.setProcessValue((float)move_l_velocity);
DeguNaoto 10:36f81cc33202 180 move_l_controller.setSetPoint(0.0);
DeguNaoto 10:36f81cc33202 181 move_l_controller.setProcessValue(0.0);
DeguNaoto 0:bd4719e15f7e 182 cont_move_l = move_l_controller.compute();
DeguNaoto 0:bd4719e15f7e 183 Move_l(cont_move_l);
DeguNaoto 0:bd4719e15f7e 184 }
DeguNaoto 2:cf8ca6742db9 185 inline void Swing_speed_following()
DeguNaoto 2:cf8ca6742db9 186 {
DeguNaoto 0:bd4719e15f7e 187 swing_controller.setSetPoint((float)targ_swing_velocity);
DeguNaoto 0:bd4719e15f7e 188 swing_controller.setProcessValue((float)swing_velocity);
DeguNaoto 0:bd4719e15f7e 189 cont_swing = swing_controller.compute();
DeguNaoto 0:bd4719e15f7e 190 Motor_swing_pwm = cont_swing;
DeguNaoto 0:bd4719e15f7e 191 }
DeguNaoto 0:bd4719e15f7e 192
DeguNaoto 4:51d87d2b698c 193 /***move step.***/
DeguNaoto 4:51d87d2b698c 194 void move_step(){
DeguNaoto 4:51d87d2b698c 195 if(edge_triangle) {
DeguNaoto 4:51d87d2b698c 196 edge_triangle=0;
DeguNaoto 4:51d87d2b698c 197 if(step==7) step=0;
DeguNaoto 4:51d87d2b698c 198 else step++;
DeguNaoto 4:51d87d2b698c 199 }
DeguNaoto 4:51d87d2b698c 200 }
DeguNaoto 4:51d87d2b698c 201
DeguNaoto 0:bd4719e15f7e 202 /***Emergency stop.***/
DeguNaoto 2:cf8ca6742db9 203 void Emergency_toggle()
DeguNaoto 2:cf8ca6742db9 204 {
DeguNaoto 4:51d87d2b698c 205 if(edge_circle) {
DeguNaoto 4:51d87d2b698c 206 edge_circle=0;
DeguNaoto 0:bd4719e15f7e 207 if(toggle) toggle=0,Emergency_stop=0;
DeguNaoto 0:bd4719e15f7e 208 else toggle=1,Emergency_stop=1;
DeguNaoto 0:bd4719e15f7e 209 }
DeguNaoto 0:bd4719e15f7e 210 }
DeguNaoto 0:bd4719e15f7e 211
DeguNaoto 0:bd4719e15f7e 212 #endif /*machine.h*/