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@3:5266af49834f, 2015-09-02 (annotated)

Committer:
DeguNaoto
Date:
Wed Sep 02 05:12:26 2015 +0000
Revision:
3:5266af49834f
Parent:
2:cf8ca6742db9 
???????????????; stop()???????

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 0:bd4719e15f7e 36 short toggle=0,edge=0;
DeguNaoto 0:bd4719e15f7e 37
DeguNaoto 0:bd4719e15f7e 38 /**
DeguNaoto 0:bd4719e15f7e 39 * include function header files.
DeguNaoto 0:bd4719e15f7e 40 */
DeguNaoto 0:bd4719e15f7e 41 #include "communicate.h"
DeguNaoto 2:cf8ca6742db9 42
DeguNaoto 0:bd4719e15f7e 43 /**
DeguNaoto 0:bd4719e15f7e 44 * Defines
DeguNaoto 0:bd4719e15f7e 45 */
DeguNaoto 0:bd4719e15f7e 46 #define RATE 0.01
DeguNaoto 0:bd4719e15f7e 47
DeguNaoto 0:bd4719e15f7e 48 /**
DeguNaoto 0:bd4719e15f7e 49 * Set functions.
DeguNaoto 0:bd4719e15f7e 50 */
DeguNaoto 2:cf8ca6742db9 51
DeguNaoto 0:bd4719e15f7e 52 /***PID Controller***/
DeguNaoto 0:bd4719e15f7e 53 PID move_r_controller(MOVE_R_Kc, MOVE_R_TAUi, MOVE_R_TAUd, RATE);
DeguNaoto 0:bd4719e15f7e 54 PID move_l_controller(MOVE_L_Kc, MOVE_L_TAUi, MOVE_L_TAUd, RATE);
DeguNaoto 0:bd4719e15f7e 55 PID swing_controller(SWING_Kc, SWING_TAUi, SWING_TAUd, RATE);
DeguNaoto 0:bd4719e15f7e 56
DeguNaoto 0:bd4719e15f7e 57 /***Variables***/
DeguNaoto 0:bd4719e15f7e 58 double Pulses_move_r=0.0;
DeguNaoto 0:bd4719e15f7e 59 double Pulses_move_l=0.0;
DeguNaoto 0:bd4719e15f7e 60 double Pulses_swing=0.0;
DeguNaoto 0:bd4719e15f7e 61 double PrefPulses_move_r=0.0;
DeguNaoto 0:bd4719e15f7e 62 double PrefPulses_move_l=0.0;
DeguNaoto 0:bd4719e15f7e 63 double PrefPulses_swing=0.0;
DeguNaoto 0:bd4719e15f7e 64 double move_r_velocity=0.0;
DeguNaoto 0:bd4719e15f7e 65 double move_l_velocity=0.0;
DeguNaoto 0:bd4719e15f7e 66 double swing_velocity=0.0;
DeguNaoto 0:bd4719e15f7e 67 //double Prefmove_r_velocity=0.0;
DeguNaoto 0:bd4719e15f7e 68 //double Prefmove_l_velocity=0.0;
DeguNaoto 0:bd4719e15f7e 69 double targ_move_r_velocity=0.0;
DeguNaoto 0:bd4719e15f7e 70 double targ_move_l_velocity=0.0;
DeguNaoto 0:bd4719e15f7e 71 double targ_swing_velocity=0.0;
DeguNaoto 0:bd4719e15f7e 72 //const double alpha=0.9;
DeguNaoto 0:bd4719e15f7e 73
DeguNaoto 0:bd4719e15f7e 74 /**
DeguNaoto 0:bd4719e15f7e 75 * Functions.
DeguNaoto 0:bd4719e15f7e 76 */
DeguNaoto 0:bd4719e15f7e 77
DeguNaoto 0:bd4719e15f7e 78 /***The function is motors initialize.***/
DeguNaoto 2:cf8ca6742db9 79 inline void initializeMotors()
DeguNaoto 2:cf8ca6742db9 80 {
DeguNaoto 0:bd4719e15f7e 81 Move_r_Motor_PWM.period_us(MOVE_R_PERIOD_US);
DeguNaoto 0:bd4719e15f7e 82 Move_l_Motor_PWM.period_us(MOVE_L_PERIOD_US);
DeguNaoto 0:bd4719e15f7e 83 Motor_swing_pwm.period_us(SWING_PERIOD_US);
DeguNaoto 0:bd4719e15f7e 84 }
DeguNaoto 0:bd4719e15f7e 85
DeguNaoto 0:bd4719e15f7e 86 /***The function is PID controller initialize.***/
DeguNaoto 2:cf8ca6742db9 87 inline void initializeControllers()
DeguNaoto 2:cf8ca6742db9 88 {
DeguNaoto 0:bd4719e15f7e 89
DeguNaoto 0:bd4719e15f7e 90 move_r_controller.setInputLimits(MOVE_R_INPUT_LIMIT_BOTTOM, MOVE_R_INPUT_LIMIT_TOP);
DeguNaoto 0:bd4719e15f7e 91 move_l_controller.setInputLimits(MOVE_L_INPUT_LIMIT_BOTTOM, MOVE_L_INPUT_LIMIT_TOP);
DeguNaoto 0:bd4719e15f7e 92 swing_controller.setInputLimits(SWING_INPUT_LIMIT_BOTTOM, SWING_INPUT_LIMIT_TOP);
DeguNaoto 2:cf8ca6742db9 93
DeguNaoto 0:bd4719e15f7e 94 //Pwm output from MOVE_OUTPUT_LIMIT_BOTTOM to MOVE_OUTPUT_LIMIT_TOP
DeguNaoto 0:bd4719e15f7e 95 move_r_controller.setOutputLimits(MOVE_R_OUTPUT_LIMIT_BOTTOM, MOVE_R_OUTPUT_LIMIT_TOP);
DeguNaoto 0:bd4719e15f7e 96 move_l_controller.setOutputLimits(MOVE_L_OUTPUT_LIMIT_BOTTOM, MOVE_L_OUTPUT_LIMIT_TOP);
DeguNaoto 0:bd4719e15f7e 97 swing_controller.setOutputLimits(SWING_OUTPUT_LIMIT_BOTTOM, SWING_OUTPUT_LIMIT_TOP);
DeguNaoto 2:cf8ca6742db9 98
DeguNaoto 0:bd4719e15f7e 99 //set bias.
DeguNaoto 0:bd4719e15f7e 100 move_r_controller.setBias(MOVE_R_BIAS);
DeguNaoto 0:bd4719e15f7e 101 move_l_controller.setBias(MOVE_L_BIAS);
DeguNaoto 0:bd4719e15f7e 102 swing_controller.setBias(SWING_BIAS);
DeguNaoto 2:cf8ca6742db9 103
DeguNaoto 0:bd4719e15f7e 104 //set mode.
DeguNaoto 0:bd4719e15f7e 105 move_r_controller.setMode(AUTO_MODE);
DeguNaoto 0:bd4719e15f7e 106 move_l_controller.setMode(AUTO_MODE);
DeguNaoto 0:bd4719e15f7e 107 swing_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 3:5266af49834f 138 void Stop(){
DeguNaoto 3:5266af49834f 139 Move_l_Motor_CCW = 0;
DeguNaoto 3:5266af49834f 140 Move_l_Motor_CW = 0;
DeguNaoto 3:5266af49834f 141 Move_l_Motor_PWM = 0;
DeguNaoto 3:5266af49834f 142 }
DeguNaoto 3:5266af49834f 143
DeguNaoto 0:bd4719e15f7e 144 /***Caluculate move velocity.***/
DeguNaoto 2:cf8ca6742db9 145 inline void mesure_move_r_velocity()
DeguNaoto 2:cf8ca6742db9 146 {
DeguNaoto 0:bd4719e15f7e 147 Pulses_move_r = (double)Move_r_sense.getPulses();
DeguNaoto 0:bd4719e15f7e 148 move_r_velocity = Pulses_move_r - PrefPulses_move_r;
DeguNaoto 0:bd4719e15f7e 149 //move_r_velocity += (1.0-alpha)*Prefmove_r_velocity;
DeguNaoto 0:bd4719e15f7e 150 //Prefmove_r_velocity = move_r_velocity;
DeguNaoto 0:bd4719e15f7e 151 PrefPulses_move_r = Pulses_move_r;
DeguNaoto 0:bd4719e15f7e 152 }
DeguNaoto 2:cf8ca6742db9 153 inline void mesure_move_l_velocity()
DeguNaoto 2:cf8ca6742db9 154 {
DeguNaoto 0:bd4719e15f7e 155 Pulses_move_l = (double)Move_l_sense.getPulses();
DeguNaoto 0:bd4719e15f7e 156 move_l_velocity = Pulses_move_l - PrefPulses_move_l;
DeguNaoto 0:bd4719e15f7e 157 //move_l_velocity += (1.0-alpha)*Prefmove_l_velocity;
DeguNaoto 0:bd4719e15f7e 158 //Prefmove_l_velocity = move_l_velocity;
DeguNaoto 0:bd4719e15f7e 159 PrefPulses_move_l = Pulses_move_l;
DeguNaoto 0:bd4719e15f7e 160 }
DeguNaoto 2:cf8ca6742db9 161 inline void mesure_swing_velocity()
DeguNaoto 2:cf8ca6742db9 162 {
DeguNaoto 0:bd4719e15f7e 163 Pulses_swing = (double)Swing_speed_sens.getPulses();
DeguNaoto 0:bd4719e15f7e 164 swing_velocity = Pulses_swing - PrefPulses_swing;
DeguNaoto 0:bd4719e15f7e 165 PrefPulses_swing = Pulses_swing;
DeguNaoto 0:bd4719e15f7e 166 }
DeguNaoto 0:bd4719e15f7e 167
DeguNaoto 0:bd4719e15f7e 168 /***The function is following move speed.***/
DeguNaoto 0:bd4719e15f7e 169 float cont_move_r=0.0,cont_move_l=0.0,cont_swing=0.0;
DeguNaoto 0:bd4719e15f7e 170 short r=0,l=0;
DeguNaoto 0:bd4719e15f7e 171
DeguNaoto 2:cf8ca6742db9 172 inline void Move_r_speed_following()
DeguNaoto 2:cf8ca6742db9 173 {
DeguNaoto 0:bd4719e15f7e 174 move_r_controller.setSetPoint((float)targ_move_r_velocity);
DeguNaoto 0:bd4719e15f7e 175 move_r_controller.setProcessValue((float)move_r_velocity);
DeguNaoto 0:bd4719e15f7e 176 cont_move_r = move_r_controller.compute();
DeguNaoto 0:bd4719e15f7e 177 Move_r(cont_move_r);
DeguNaoto 0:bd4719e15f7e 178 }
DeguNaoto 2:cf8ca6742db9 179 inline void Move_l_speed_following()
DeguNaoto 2:cf8ca6742db9 180 {
DeguNaoto 0:bd4719e15f7e 181 move_l_controller.setSetPoint((float)targ_move_l_velocity);
DeguNaoto 0:bd4719e15f7e 182 move_l_controller.setProcessValue((float)move_l_velocity);
DeguNaoto 0:bd4719e15f7e 183 cont_move_l = move_l_controller.compute();
DeguNaoto 0:bd4719e15f7e 184 Move_l(cont_move_l);
DeguNaoto 0:bd4719e15f7e 185 }
DeguNaoto 2:cf8ca6742db9 186 inline void Swing_speed_following()
DeguNaoto 2:cf8ca6742db9 187 {
DeguNaoto 0:bd4719e15f7e 188 swing_controller.setSetPoint((float)targ_swing_velocity);
DeguNaoto 0:bd4719e15f7e 189 swing_controller.setProcessValue((float)swing_velocity);
DeguNaoto 0:bd4719e15f7e 190 cont_swing = swing_controller.compute();
DeguNaoto 0:bd4719e15f7e 191 Motor_swing_pwm = cont_swing;
DeguNaoto 0:bd4719e15f7e 192 }
DeguNaoto 0:bd4719e15f7e 193
DeguNaoto 0:bd4719e15f7e 194 /***Emergency stop.***/
DeguNaoto 2:cf8ca6742db9 195 void Emergency_toggle()
DeguNaoto 2:cf8ca6742db9 196 {
DeguNaoto 2:cf8ca6742db9 197 if(edge) {
DeguNaoto 0:bd4719e15f7e 198 edge=0;
DeguNaoto 0:bd4719e15f7e 199 if(toggle) toggle=0,Emergency_stop=0;
DeguNaoto 0:bd4719e15f7e 200 else toggle=1,Emergency_stop=1;
DeguNaoto 0:bd4719e15f7e 201 }
DeguNaoto 0:bd4719e15f7e 202 }
DeguNaoto 0:bd4719e15f7e 203
DeguNaoto 0:bd4719e15f7e 204 #endif /*machine.h*/