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@1:a89e2a604dcf, 2015-08-31 (annotated)

Committer:
DeguNaoto
Date:
Mon Aug 31 00:25:25 2015 +0000
Revision:
1:a89e2a604dcf
Parent:
0:bd4719e15f7e
Child:
2:cf8ca6742db9
diff ????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
DeguNaoto 0:bd4719e15f7e 1 /**
DeguNaoto 0:bd4719e15f7e 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 0:bd4719e15f7e 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 0:bd4719e15f7e 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 0:bd4719e15f7e 79 inline void initializeMotors(){
DeguNaoto 0:bd4719e15f7e 80 Move_r_Motor_PWM.period_us(MOVE_R_PERIOD_US);
DeguNaoto 0:bd4719e15f7e 81 Move_l_Motor_PWM.period_us(MOVE_L_PERIOD_US);
DeguNaoto 0:bd4719e15f7e 82 Motor_swing_pwm.period_us(SWING_PERIOD_US);
DeguNaoto 0:bd4719e15f7e 83 }
DeguNaoto 0:bd4719e15f7e 84
DeguNaoto 0:bd4719e15f7e 85 /***The function is PID controller initialize.***/
DeguNaoto 0:bd4719e15f7e 86 inline void initializeControllers(){
DeguNaoto 0:bd4719e15f7e 87
DeguNaoto 0:bd4719e15f7e 88 move_r_controller.setInputLimits(MOVE_R_INPUT_LIMIT_BOTTOM, MOVE_R_INPUT_LIMIT_TOP);
DeguNaoto 0:bd4719e15f7e 89 move_l_controller.setInputLimits(MOVE_L_INPUT_LIMIT_BOTTOM, MOVE_L_INPUT_LIMIT_TOP);
DeguNaoto 0:bd4719e15f7e 90 swing_controller.setInputLimits(SWING_INPUT_LIMIT_BOTTOM, SWING_INPUT_LIMIT_TOP);
DeguNaoto 0:bd4719e15f7e 91
DeguNaoto 0:bd4719e15f7e 92 //Pwm output from MOVE_OUTPUT_LIMIT_BOTTOM to MOVE_OUTPUT_LIMIT_TOP
DeguNaoto 0:bd4719e15f7e 93 move_r_controller.setOutputLimits(MOVE_R_OUTPUT_LIMIT_BOTTOM, MOVE_R_OUTPUT_LIMIT_TOP);
DeguNaoto 0:bd4719e15f7e 94 move_l_controller.setOutputLimits(MOVE_L_OUTPUT_LIMIT_BOTTOM, MOVE_L_OUTPUT_LIMIT_TOP);
DeguNaoto 0:bd4719e15f7e 95 swing_controller.setOutputLimits(SWING_OUTPUT_LIMIT_BOTTOM, SWING_OUTPUT_LIMIT_TOP);
DeguNaoto 0:bd4719e15f7e 96
DeguNaoto 0:bd4719e15f7e 97 //set bias.
DeguNaoto 0:bd4719e15f7e 98 move_r_controller.setBias(MOVE_R_BIAS);
DeguNaoto 0:bd4719e15f7e 99 move_l_controller.setBias(MOVE_L_BIAS);
DeguNaoto 0:bd4719e15f7e 100 swing_controller.setBias(SWING_BIAS);
DeguNaoto 0:bd4719e15f7e 101
DeguNaoto 0:bd4719e15f7e 102 //set mode.
DeguNaoto 0:bd4719e15f7e 103 move_r_controller.setMode(AUTO_MODE);
DeguNaoto 0:bd4719e15f7e 104 move_l_controller.setMode(AUTO_MODE);
DeguNaoto 0:bd4719e15f7e 105 swing_controller.setMode(AUTO_MODE);
DeguNaoto 0:bd4719e15f7e 106 }
DeguNaoto 0:bd4719e15f7e 107
DeguNaoto 0:bd4719e15f7e 108 /***The function is Move on field.***/
DeguNaoto 0:bd4719e15f7e 109 //Right
DeguNaoto 0:bd4719e15f7e 110 void Move_r(float speed1){
DeguNaoto 0:bd4719e15f7e 111 if(speed1<0){
DeguNaoto 0:bd4719e15f7e 112 Move_r_Motor_CCW = 1;
DeguNaoto 0:bd4719e15f7e 113 Move_r_Motor_CW = 0;
DeguNaoto 0:bd4719e15f7e 114 Move_r_Motor_PWM = abs(speed1);
DeguNaoto 0:bd4719e15f7e 115 }
DeguNaoto 0:bd4719e15f7e 116 else{
DeguNaoto 0:bd4719e15f7e 117 Move_r_Motor_CCW = 0;
DeguNaoto 0:bd4719e15f7e 118 Move_r_Motor_CW = 1;
DeguNaoto 0:bd4719e15f7e 119 Move_r_Motor_PWM = speed1;
DeguNaoto 0:bd4719e15f7e 120 }
DeguNaoto 0:bd4719e15f7e 121 }
DeguNaoto 0:bd4719e15f7e 122 //Left
DeguNaoto 0:bd4719e15f7e 123 void Move_l(float speed2){
DeguNaoto 0:bd4719e15f7e 124 if(speed2<0){
DeguNaoto 0:bd4719e15f7e 125 Move_l_Motor_CCW = 1;
DeguNaoto 0:bd4719e15f7e 126 Move_l_Motor_CW = 0;
DeguNaoto 0:bd4719e15f7e 127 Move_l_Motor_PWM = abs(speed2);
DeguNaoto 0:bd4719e15f7e 128 }
DeguNaoto 0:bd4719e15f7e 129 else{
DeguNaoto 0:bd4719e15f7e 130 Move_l_Motor_CCW = 0;
DeguNaoto 0:bd4719e15f7e 131 Move_l_Motor_CW = 1;
DeguNaoto 0:bd4719e15f7e 132 Move_l_Motor_PWM = speed2;
DeguNaoto 0:bd4719e15f7e 133 }
DeguNaoto 0:bd4719e15f7e 134 }
DeguNaoto 0:bd4719e15f7e 135
DeguNaoto 0:bd4719e15f7e 136 /***Caluculate move velocity.***/
DeguNaoto 0:bd4719e15f7e 137 inline void mesure_move_r_velocity(){
DeguNaoto 0:bd4719e15f7e 138 Pulses_move_r = (double)Move_r_sense.getPulses();
DeguNaoto 0:bd4719e15f7e 139 move_r_velocity = Pulses_move_r - PrefPulses_move_r;
DeguNaoto 0:bd4719e15f7e 140 //move_r_velocity += (1.0-alpha)*Prefmove_r_velocity;
DeguNaoto 0:bd4719e15f7e 141 //Prefmove_r_velocity = move_r_velocity;
DeguNaoto 0:bd4719e15f7e 142 PrefPulses_move_r = Pulses_move_r;
DeguNaoto 0:bd4719e15f7e 143 }
DeguNaoto 0:bd4719e15f7e 144 inline void mesure_move_l_velocity(){
DeguNaoto 0:bd4719e15f7e 145 Pulses_move_l = (double)Move_l_sense.getPulses();
DeguNaoto 0:bd4719e15f7e 146 move_l_velocity = Pulses_move_l - PrefPulses_move_l;
DeguNaoto 0:bd4719e15f7e 147 //move_l_velocity += (1.0-alpha)*Prefmove_l_velocity;
DeguNaoto 0:bd4719e15f7e 148 //Prefmove_l_velocity = move_l_velocity;
DeguNaoto 0:bd4719e15f7e 149 PrefPulses_move_l = Pulses_move_l;
DeguNaoto 0:bd4719e15f7e 150 }
DeguNaoto 0:bd4719e15f7e 151 inline void mesure_swing_velocity(){
DeguNaoto 0:bd4719e15f7e 152 Pulses_swing = (double)Swing_speed_sens.getPulses();
DeguNaoto 0:bd4719e15f7e 153 swing_velocity = Pulses_swing - PrefPulses_swing;
DeguNaoto 0:bd4719e15f7e 154 PrefPulses_swing = Pulses_swing;
DeguNaoto 0:bd4719e15f7e 155 }
DeguNaoto 0:bd4719e15f7e 156
DeguNaoto 0:bd4719e15f7e 157 /***The function is following move speed.***/
DeguNaoto 0:bd4719e15f7e 158 float cont_move_r=0.0,cont_move_l=0.0,cont_swing=0.0;
DeguNaoto 0:bd4719e15f7e 159 short r=0,l=0;
DeguNaoto 0:bd4719e15f7e 160
DeguNaoto 0:bd4719e15f7e 161 inline void Move_r_speed_following(){
DeguNaoto 0:bd4719e15f7e 162 move_r_controller.setSetPoint((float)targ_move_r_velocity);
DeguNaoto 0:bd4719e15f7e 163 move_r_controller.setProcessValue((float)move_r_velocity);
DeguNaoto 0:bd4719e15f7e 164 cont_move_r = move_r_controller.compute();
DeguNaoto 0:bd4719e15f7e 165 Move_r(cont_move_r);
DeguNaoto 0:bd4719e15f7e 166 }
DeguNaoto 0:bd4719e15f7e 167 inline void Move_l_speed_following(){
DeguNaoto 0:bd4719e15f7e 168 move_l_controller.setSetPoint((float)targ_move_l_velocity);
DeguNaoto 0:bd4719e15f7e 169 move_l_controller.setProcessValue((float)move_l_velocity);
DeguNaoto 0:bd4719e15f7e 170 cont_move_l = move_l_controller.compute();
DeguNaoto 0:bd4719e15f7e 171 Move_l(cont_move_l);
DeguNaoto 0:bd4719e15f7e 172 }
DeguNaoto 0:bd4719e15f7e 173 inline void Swing_speed_following(){
DeguNaoto 0:bd4719e15f7e 174 swing_controller.setSetPoint((float)targ_swing_velocity);
DeguNaoto 0:bd4719e15f7e 175 swing_controller.setProcessValue((float)swing_velocity);
DeguNaoto 0:bd4719e15f7e 176 cont_swing = swing_controller.compute();
DeguNaoto 0:bd4719e15f7e 177 Motor_swing_pwm = cont_swing;
DeguNaoto 0:bd4719e15f7e 178 }
DeguNaoto 0:bd4719e15f7e 179
DeguNaoto 0:bd4719e15f7e 180 /***Emergency stop.***/
DeguNaoto 0:bd4719e15f7e 181 void Emergency_toggle(){
DeguNaoto 0:bd4719e15f7e 182 if(edge){
DeguNaoto 0:bd4719e15f7e 183 edge=0;
DeguNaoto 0:bd4719e15f7e 184 if(toggle) toggle=0,Emergency_stop=0;
DeguNaoto 0:bd4719e15f7e 185 else toggle=1,Emergency_stop=1;
DeguNaoto 0:bd4719e15f7e 186 }
DeguNaoto 0:bd4719e15f7e 187 }
DeguNaoto 0:bd4719e15f7e 188
DeguNaoto 0:bd4719e15f7e 189 #endif /*machine.h*/