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

autoMode.h@73:86b7b82ba997, 2015-10-10 (annotated)

Committer:
DeguNaoto
Date:
Sat Oct 10 01:44:30 2015 +0000
Revision:
73:86b7b82ba997
Parent:
71:4ebc1c0fcb4e
Child:
75:4cdcd50ee666
?????????????????????;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
DeguNaoto 21:79b94cb922f0 1 #ifndef AUTOMODE_H
DeguNaoto 21:79b94cb922f0 2 #define AUTOMODE_H
DeguNaoto 21:79b94cb922f0 3
DeguNaoto 21:79b94cb922f0 4 /***PID Controller***/
DeguNaoto 66:14df82661dfa 5 //PID velocity_controller(36.0,5274.0 ,0.0,RATE);
DeguNaoto 66:14df82661dfa 6 //PID direction_controller(36.0,3.0,0.0,RATE);
DeguNaoto 66:14df82661dfa 7 PID velocity_controller(9.0,5274.0 ,0.0,RATE);
DeguNaoto 34:f9ef622f4376 8 PID direction_controller(36.0,3.0,0.0,RATE);
DeguNaoto 21:79b94cb922f0 9
DeguNaoto 57:3fbd487e055e 10 #ifdef IM920
DeguNaoto 52:d9e1629da852 11 /***IM920 correspondence***/
DeguNaoto 52:d9e1629da852 12 void autoIM920() {
DeguNaoto 52:d9e1629da852 13 if(b==7){ /*mode change*/
DeguNaoto 52:d9e1629da852 14 if(edge7){
DeguNaoto 52:d9e1629da852 15 edge7=0;
DeguNaoto 52:d9e1629da852 16 autoflag=0;
DeguNaoto 52:d9e1629da852 17 Indicator4=0;
DeguNaoto 52:d9e1629da852 18 IndicatorAuto=1;
DeguNaoto 52:d9e1629da852 19 }
DeguNaoto 52:d9e1629da852 20 }
DeguNaoto 52:d9e1629da852 21 else if(b==6){ /*start*/
DeguNaoto 73:86b7b82ba997 22 resetState();
DeguNaoto 64:a98fe602c26d 23 flagf=1;
DeguNaoto 52:d9e1629da852 24 targ_velocity=speed;
DeguNaoto 66:14df82661dfa 25 sendData(7,defoR);
DeguNaoto 52:d9e1629da852 26 stateR = defoR;
DeguNaoto 52:d9e1629da852 27 stateL = defoL;
DeguNaoto 61:e018207ae860 28 step = 0;
DeguNaoto 61:e018207ae860 29 CMode = 0;
DeguNaoto 52:d9e1629da852 30 }
DeguNaoto 61:e018207ae860 31 else if(b==8){ /*middle start*/
DeguNaoto 73:86b7b82ba997 32 resetState();
DeguNaoto 64:a98fe602c26d 33 flagf=1;
DeguNaoto 61:e018207ae860 34 targ_velocity=speed;
DeguNaoto 66:14df82661dfa 35 sendData(7,17);
DeguNaoto 65:cebdfc0184d7 36 stateR = 17;
DeguNaoto 65:cebdfc0184d7 37 stateL = 17;
DeguNaoto 61:e018207ae860 38 step = 4;
DeguNaoto 61:e018207ae860 39 CMode = 6;
DeguNaoto 61:e018207ae860 40 }
DeguNaoto 52:d9e1629da852 41 else if(b==5){ /*reset*/
DeguNaoto 66:14df82661dfa 42 sendData(7,31);
DeguNaoto 52:d9e1629da852 43 }
DeguNaoto 52:d9e1629da852 44 else if(b==9){ /*stop*/
DeguNaoto 29:03bb1ddbe456 45 Motor_swing=0;
DeguNaoto 22:3996c3f41922 46 sita=PI/4.0,x=0.0,y=0.0;
DeguNaoto 22:3996c3f41922 47 targ_velocity=0.0;
DeguNaoto 22:3996c3f41922 48 targ_sita=PI/4.0;
DeguNaoto 22:3996c3f41922 49 velocity_controller.reset();
DeguNaoto 22:3996c3f41922 50 direction_controller.reset();
DeguNaoto 52:d9e1629da852 51 }
DeguNaoto 52:d9e1629da852 52 else if(b==1){ /*L down*/
DeguNaoto 52:d9e1629da852 53 if(edge1){
DeguNaoto 52:d9e1629da852 54 edge1=0;
DeguNaoto 52:d9e1629da852 55 if(stateL!=1) stateL--;
DeguNaoto 57:3fbd487e055e 56 sendData(4,stateL);
DeguNaoto 52:d9e1629da852 57 }
DeguNaoto 52:d9e1629da852 58 }
DeguNaoto 52:d9e1629da852 59 else if(b==2){ /*L up*/
DeguNaoto 52:d9e1629da852 60 if(edge2){
DeguNaoto 52:d9e1629da852 61 edge2=0;
DeguNaoto 52:d9e1629da852 62 if(stateL!=28) stateL++;
DeguNaoto 57:3fbd487e055e 63 sendData(4,stateL);
DeguNaoto 22:3996c3f41922 64 }
DeguNaoto 52:d9e1629da852 65 }
DeguNaoto 52:d9e1629da852 66 else if(b==3){ /*R down*/
DeguNaoto 52:d9e1629da852 67 if(edge3){
DeguNaoto 52:d9e1629da852 68 edge3=0;
DeguNaoto 52:d9e1629da852 69 if(stateR!=1) stateR--;
DeguNaoto 57:3fbd487e055e 70 sendData(5,stateR);
DeguNaoto 52:d9e1629da852 71 }
DeguNaoto 52:d9e1629da852 72 }
DeguNaoto 52:d9e1629da852 73 else if(b==4){ /*R up*/
DeguNaoto 52:d9e1629da852 74 if(edge4){
DeguNaoto 52:d9e1629da852 75 edge4=0;
DeguNaoto 52:d9e1629da852 76 if(stateR!=28) stateR++;
DeguNaoto 57:3fbd487e055e 77 sendData(5,stateR);
DeguNaoto 52:d9e1629da852 78 }
DeguNaoto 52:d9e1629da852 79 }
DeguNaoto 52:d9e1629da852 80 if(b!=7) edge7=1;
DeguNaoto 52:d9e1629da852 81 if(b!=1) edge1=1;
DeguNaoto 52:d9e1629da852 82 if(b!=2) edge2=1;
DeguNaoto 52:d9e1629da852 83 if(b!=3) edge3=1;
DeguNaoto 52:d9e1629da852 84 if(b!=4) edge4=1;
DeguNaoto 22:3996c3f41922 85 }
DeguNaoto 57:3fbd487e055e 86 #else
DeguNaoto 57:3fbd487e055e 87 void autoPS3(){
DeguNaoto 57:3fbd487e055e 88 if(circle){ /*mode change*/
DeguNaoto 57:3fbd487e055e 89 if(edge_circle){
DeguNaoto 57:3fbd487e055e 90 edge_circle=0;
DeguNaoto 57:3fbd487e055e 91 autoflag=0;
DeguNaoto 57:3fbd487e055e 92 Indicator4=0;
DeguNaoto 57:3fbd487e055e 93 IndicatorAuto=1;
DeguNaoto 57:3fbd487e055e 94 }
DeguNaoto 57:3fbd487e055e 95 }
DeguNaoto 57:3fbd487e055e 96 else if(up){ /*start*/
DeguNaoto 73:86b7b82ba997 97 resetState();
DeguNaoto 64:a98fe602c26d 98 flagf=1;
DeguNaoto 57:3fbd487e055e 99 targ_velocity=speed;
DeguNaoto 66:14df82661dfa 100 sendData(7,defoR);
DeguNaoto 57:3fbd487e055e 101 stateR = defoR;
DeguNaoto 57:3fbd487e055e 102 stateL = defoL;
DeguNaoto 61:e018207ae860 103 step = 0;
DeguNaoto 61:e018207ae860 104 CStep = 1;
DeguNaoto 61:e018207ae860 105 }
DeguNaoto 61:e018207ae860 106 else if(right){ /*middle start*/
DeguNaoto 73:86b7b82ba997 107 resetState();
DeguNaoto 64:a98fe602c26d 108 flagf = 1;
DeguNaoto 61:e018207ae860 109 targ_velocity=speed;
DeguNaoto 66:14df82661dfa 110 sendData(7,17);
DeguNaoto 65:cebdfc0184d7 111 stateR = 17;
DeguNaoto 65:cebdfc0184d7 112 stateL = 17;
DeguNaoto 61:e018207ae860 113 step = 4;
DeguNaoto 61:e018207ae860 114 CStep = 6;
DeguNaoto 57:3fbd487e055e 115 }
DeguNaoto 57:3fbd487e055e 116 else if(square){ /*reset*/
DeguNaoto 66:14df82661dfa 117 sendData(7,31);
DeguNaoto 57:3fbd487e055e 118 }
DeguNaoto 71:4ebc1c0fcb4e 119 else if(triangle){
DeguNaoto 71:4ebc1c0fcb4e 120 // sendData(7,defoR);
DeguNaoto 71:4ebc1c0fcb4e 121 skip = 1;
DeguNaoto 71:4ebc1c0fcb4e 122 }
DeguNaoto 71:4ebc1c0fcb4e 123 else if(!triangle){
DeguNaoto 71:4ebc1c0fcb4e 124 skip = 0;
DeguNaoto 57:3fbd487e055e 125 }
DeguNaoto 57:3fbd487e055e 126 else if(cross){ /*stop*/
DeguNaoto 57:3fbd487e055e 127 Motor_swing=0;
DeguNaoto 64:a98fe602c26d 128 // sita=PI/4.0,x=0.0,y=0.0;
DeguNaoto 57:3fbd487e055e 129 targ_velocity=0.0;
DeguNaoto 57:3fbd487e055e 130 targ_sita=PI/4.0;
DeguNaoto 57:3fbd487e055e 131 velocity_controller.reset();
DeguNaoto 57:3fbd487e055e 132 direction_controller.reset();
DeguNaoto 57:3fbd487e055e 133 }
DeguNaoto 57:3fbd487e055e 134 else if(l1){ /*L up*/
DeguNaoto 57:3fbd487e055e 135 if(edge_l1){
DeguNaoto 57:3fbd487e055e 136 edge_l1=0;
DeguNaoto 61:e018207ae860 137 if(stateL!=1) stateL--;
DeguNaoto 57:3fbd487e055e 138 sendData(4,stateL);
DeguNaoto 57:3fbd487e055e 139 }
DeguNaoto 57:3fbd487e055e 140 }
DeguNaoto 57:3fbd487e055e 141 else if(l2){ /*L down*/
DeguNaoto 57:3fbd487e055e 142 if(edge_l2){
DeguNaoto 57:3fbd487e055e 143 edge_l2=0;
DeguNaoto 57:3fbd487e055e 144 if(stateL!=28) stateL++;
DeguNaoto 57:3fbd487e055e 145 sendData(4,stateL);
DeguNaoto 57:3fbd487e055e 146 }
DeguNaoto 57:3fbd487e055e 147 }
DeguNaoto 57:3fbd487e055e 148 else if(r1){ /*R up*/
DeguNaoto 57:3fbd487e055e 149 if(edge_r1){
DeguNaoto 57:3fbd487e055e 150 edge_r1=0;
DeguNaoto 61:e018207ae860 151 if(stateR!=1) stateR--;
DeguNaoto 57:3fbd487e055e 152 sendData(5,stateR);
DeguNaoto 57:3fbd487e055e 153 }
DeguNaoto 57:3fbd487e055e 154 }
DeguNaoto 57:3fbd487e055e 155 else if(r2){ /*R down*/
DeguNaoto 57:3fbd487e055e 156 if(edge_r2){
DeguNaoto 57:3fbd487e055e 157 edge_r2=0;
DeguNaoto 57:3fbd487e055e 158 if(stateR!=28) stateR++;
DeguNaoto 57:3fbd487e055e 159 sendData(5,stateR);
DeguNaoto 57:3fbd487e055e 160 }
DeguNaoto 57:3fbd487e055e 161 }
DeguNaoto 57:3fbd487e055e 162 }
DeguNaoto 57:3fbd487e055e 163 #endif
DeguNaoto 21:79b94cb922f0 164 /***The function is PID controller initialize.***/
DeguNaoto 21:79b94cb922f0 165 inline void initializeControllers()
DeguNaoto 21:79b94cb922f0 166 {
DeguNaoto 66:14df82661dfa 167 // velocity_controller.setInputLimits(-200000.0, 200000.0); //x1
DeguNaoto 66:14df82661dfa 168 velocity_controller.setInputLimits(-20000.0, 20000.0); //x1
DeguNaoto 21:79b94cb922f0 169 direction_controller.setInputLimits(-10.0, 10.0); //x2
DeguNaoto 21:79b94cb922f0 170
DeguNaoto 21:79b94cb922f0 171 //Pwm output from MOVE_OUTPUT_LIMIT_BOTTOM to MOVE_OUTPUT_LIMIT_TOP
DeguNaoto 21:79b94cb922f0 172 velocity_controller.setOutputLimits(-1.0, 1.0);
DeguNaoto 21:79b94cb922f0 173 direction_controller.setOutputLimits(-1.0, 1.0);
DeguNaoto 31:74e77ef0831c 174
DeguNaoto 21:79b94cb922f0 175 //set bias. 初期値
DeguNaoto 21:79b94cb922f0 176 velocity_controller.setBias(0.0);
DeguNaoto 21:79b94cb922f0 177 direction_controller.setBias(0.0);
DeguNaoto 21:79b94cb922f0 178
DeguNaoto 21:79b94cb922f0 179 //set mode.
DeguNaoto 21:79b94cb922f0 180 velocity_controller.setMode(AUTO_MODE);
DeguNaoto 21:79b94cb922f0 181 direction_controller.setMode(AUTO_MODE);
DeguNaoto 21:79b94cb922f0 182 }
DeguNaoto 21:79b94cb922f0 183
DeguNaoto 21:79b94cb922f0 184 /***The function is following move speed.***/
DeguNaoto 21:79b94cb922f0 185 inline void velocity_following()
DeguNaoto 21:79b94cb922f0 186 {
DeguNaoto 21:79b94cb922f0 187 velocity_controller.setSetPoint((float)targ_velocity);
DeguNaoto 21:79b94cb922f0 188 velocity_controller.setProcessValue((float)velocity);
DeguNaoto 21:79b94cb922f0 189 x1 = (double)velocity_controller.compute();
DeguNaoto 21:79b94cb922f0 190 }
DeguNaoto 21:79b94cb922f0 191
DeguNaoto 21:79b94cb922f0 192 inline void sita_following()
DeguNaoto 21:79b94cb922f0 193 {
DeguNaoto 21:79b94cb922f0 194 direction_controller.setSetPoint((float)targ_sita);
DeguNaoto 21:79b94cb922f0 195 direction_controller.setProcessValue((float)sita);
DeguNaoto 55:db1797ac6cb1 196 //direction_controller.setSetPoint(0.0); /*目標値とのずれをなくす*/
DeguNaoto 21:79b94cb922f0 197 //direction_controller.setProcessValue(-y);
DeguNaoto 21:79b94cb922f0 198 x2 = (double)direction_controller.compute();
DeguNaoto 21:79b94cb922f0 199 }
DeguNaoto 21:79b94cb922f0 200
DeguNaoto 21:79b94cb922f0 201 inline void move_following()
DeguNaoto 21:79b94cb922f0 202 {
DeguNaoto 21:79b94cb922f0 203 velocity_following();
DeguNaoto 21:79b94cb922f0 204 sita_following();
DeguNaoto 64:a98fe602c26d 205
DeguNaoto 64:a98fe602c26d 206 if(flagf==0){
DeguNaoto 64:a98fe602c26d 207 Vr = ( x1 + x2 ) / 2.0;
DeguNaoto 64:a98fe602c26d 208 Vl = ( x1 - x2 ) / 2.0;
DeguNaoto 63:dad3f4e5e99c 209 }
DeguNaoto 64:a98fe602c26d 210 else if(flagf==1){
DeguNaoto 64:a98fe602c26d 211 if(x2>0){
DeguNaoto 64:a98fe602c26d 212 Vr = x1;
DeguNaoto 64:a98fe602c26d 213 Vl = x1 - x2;
DeguNaoto 64:a98fe602c26d 214 }
DeguNaoto 64:a98fe602c26d 215 else{
DeguNaoto 64:a98fe602c26d 216 Vr = x1 + x2;
DeguNaoto 64:a98fe602c26d 217 Vl = x1;
DeguNaoto 64:a98fe602c26d 218 }
DeguNaoto 63:dad3f4e5e99c 219 }
DeguNaoto 63:dad3f4e5e99c 220
DeguNaoto 64:a98fe602c26d 221 if( abs(Vr) < 0.2 ) Vr = 0.0;
DeguNaoto 64:a98fe602c26d 222 if( abs(Vl) < 0.2 ) Vl = 0.0;
DeguNaoto 21:79b94cb922f0 223 Move_r( ( float ) Vr );
DeguNaoto 21:79b94cb922f0 224 Move_l( ( float ) Vl );
DeguNaoto 21:79b94cb922f0 225 }
DeguNaoto 21:79b94cb922f0 226
DeguNaoto 21:79b94cb922f0 227 #endif /*autoMode.h*/