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@0:bd4719e15f7e, 2015-08-30 (annotated)

Committer:
DeguNaoto
Date:
Sun Aug 30 10:49:16 2015 +0000
Revision:
0:bd4719e15f7e
Child:
1:a89e2a604dcf
????????

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