2015_robocon_bteam / Mbed 2 deprecated 2015robot_main_zenkoku

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

autoMode.h@38:f4e9893641ac, 2015-11-16 (annotated)

Committer:
DeguNaoto
Date:
Mon Nov 16 09:37:31 2015 +0000
Revision:
38:f4e9893641ac
Parent:
37:75fcd28f48c7
Child:
39:5ba406327aa3
20151116 ???V?????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
DeguNaoto 0:b613dc16f27d 1 #ifndef AUTOMODE_H
DeguNaoto 0:b613dc16f27d 2 #define AUTOMODE_H
DeguNaoto 0:b613dc16f27d 3
DeguNaoto 0:b613dc16f27d 4 /***PID Controller***/
DeguNaoto 26:760f1bce8214 5 PID velocity_controller(9.0,5274.0,0.0,RATE);
DeguNaoto 26:760f1bce8214 6 PID direction_controller(36.0,3.5,0.0,RATE);
DeguNaoto 0:b613dc16f27d 7 //PID direction_controller(36.0,3.0,0.0,RATE);
DeguNaoto 0:b613dc16f27d 8
DeguNaoto 0:b613dc16f27d 9 /***IM920 correspondence***/
DeguNaoto 0:b613dc16f27d 10 void autoIM920() {
DeguNaoto 33:a4323c20494b 11 /*if(b==7){ //mode change
DeguNaoto 0:b613dc16f27d 12 if(edge7) {
DeguNaoto 0:b613dc16f27d 13 edge7=0;
DeguNaoto 0:b613dc16f27d 14 autoflag=0;
DeguNaoto 0:b613dc16f27d 15 Indicator4=0;
DeguNaoto 0:b613dc16f27d 16 IndicatorAuto=1;
DeguNaoto 26:760f1bce8214 17 targ_velocity=0.0;
DeguNaoto 30:cd344beb415d 18 resetState(0);
DeguNaoto 0:b613dc16f27d 19 flaga=0;
DeguNaoto 30:cd344beb415d 20 step=114;
DeguNaoto 30:cd344beb415d 21 CStep=114;
DeguNaoto 30:cd344beb415d 22 mstep=114;
DeguNaoto 30:cd344beb415d 23 mCStep=114;
DeguNaoto 26:760f1bce8214 24 spcount=speed;
DeguNaoto 26:760f1bce8214 25 dpcount=0.0;
DeguNaoto 0:b613dc16f27d 26 }
DeguNaoto 33:a4323c20494b 27 }*/
DeguNaoto 33:a4323c20494b 28 if(!modeflag){ //mode change
DeguNaoto 33:a4323c20494b 29 autoflag=0;
DeguNaoto 33:a4323c20494b 30 Indicator4=0;
DeguNaoto 33:a4323c20494b 31 IndicatorAuto=1;
DeguNaoto 33:a4323c20494b 32 targ_velocity=0.0;
DeguNaoto 33:a4323c20494b 33 resetState(1);
DeguNaoto 33:a4323c20494b 34 flaga=0;
DeguNaoto 33:a4323c20494b 35 step=114;
DeguNaoto 33:a4323c20494b 36 CStep=114;
DeguNaoto 33:a4323c20494b 37 mstep=114;
DeguNaoto 33:a4323c20494b 38 mCStep=114;
DeguNaoto 33:a4323c20494b 39 spcount=speed;
DeguNaoto 33:a4323c20494b 40 dpcount=0.0;
DeguNaoto 36:b8954b13a6d5 41 swingmoved=0;
DeguNaoto 0:b613dc16f27d 42 }
DeguNaoto 33:a4323c20494b 43 if((b==6)&&(!flaga)){ //Own & Middle Start
DeguNaoto 0:b613dc16f27d 44 if(edge6){
DeguNaoto 0:b613dc16f27d 45 edge6=0;
DeguNaoto 30:cd344beb415d 46 resetState(1);
DeguNaoto 0:b613dc16f27d 47 flagf=1;
DeguNaoto 0:b613dc16f27d 48 spcount=0.0;
DeguNaoto 26:760f1bce8214 49 dpcount=0.0;
DeguNaoto 2:738b28f6a04b 50 direction_controller.setBias(0.0);
DeguNaoto 0:b613dc16f27d 51 #ifdef BLUE
DeguNaoto 37:75fcd28f48c7 52 sendData(5,70); //over
DeguNaoto 35:7b6786193aa2 53 // sendData(5,68); //over
unicore32 29:460e49e37048 54 wait(0.05);
DeguNaoto 37:75fcd28f48c7 55 sendData(4,70); //front
DeguNaoto 35:7b6786193aa2 56 // sendData(4,68); //front
unicore32 29:460e49e37048 57 wait(0.05);
DeguNaoto 37:75fcd28f48c7 58 sendData(6,14); //middle
DeguNaoto 34:aa2a5c888a27 59 // sendData(6,7); //middle
unicore32 29:460e49e37048 60 stateR = 69;
unicore32 29:460e49e37048 61 stateL = 69;
DeguNaoto 0:b613dc16f27d 62 #else
DeguNaoto 38:f4e9893641ac 63 sendData(4,70); //over
DeguNaoto 38:f4e9893641ac 64 // sendData(5,68); //over
DeguNaoto 0:b613dc16f27d 65 wait(0.05);
DeguNaoto 38:f4e9893641ac 66 sendData(5,70); //front
DeguNaoto 38:f4e9893641ac 67 // sendData(4,68); //front
DeguNaoto 17:726b6f53a457 68 wait(0.05);
DeguNaoto 38:f4e9893641ac 69 sendData(6,14); //middle
DeguNaoto 38:f4e9893641ac 70 // sendData(6,7); //middle
DeguNaoto 0:b613dc16f27d 71 stateR = 69;
DeguNaoto 0:b613dc16f27d 72 stateL = 69;
DeguNaoto 0:b613dc16f27d 73 #endif
DeguNaoto 0:b613dc16f27d 74 step = 0;
DeguNaoto 30:cd344beb415d 75 CStep = 0;
DeguNaoto 30:cd344beb415d 76 flaga = 1;
DeguNaoto 30:cd344beb415d 77 }
DeguNaoto 30:cd344beb415d 78 }
DeguNaoto 33:a4323c20494b 79 else if((b==5)&&(!flaga)){ //Own & Opponent Start
DeguNaoto 33:a4323c20494b 80 if(edge5){
DeguNaoto 33:a4323c20494b 81 edge5=0;
DeguNaoto 30:cd344beb415d 82 resetState(1);
DeguNaoto 30:cd344beb415d 83 flagf=1;
DeguNaoto 30:cd344beb415d 84 spcount=0.0;
DeguNaoto 30:cd344beb415d 85 dpcount=0.0;
DeguNaoto 30:cd344beb415d 86 direction_controller.setBias(0.0);
DeguNaoto 30:cd344beb415d 87 #ifdef BLUE
DeguNaoto 37:75fcd28f48c7 88 sendData(5,67);
DeguNaoto 37:75fcd28f48c7 89 // sendData(5,77); //over cylinder4
DeguNaoto 30:cd344beb415d 90 wait(0.05);
DeguNaoto 37:75fcd28f48c7 91 sendData(4,75);
DeguNaoto 37:75fcd28f48c7 92 // sendData(4,75); //front
DeguNaoto 30:cd344beb415d 93 wait(0.05);
DeguNaoto 37:75fcd28f48c7 94 sendData(6,1); //middle
DeguNaoto 30:cd344beb415d 95 stateR = 69;
DeguNaoto 33:a4323c20494b 96 stateL = 69;
DeguNaoto 30:cd344beb415d 97 #else
DeguNaoto 38:f4e9893641ac 98 // sendData(4,65);
DeguNaoto 38:f4e9893641ac 99 sendData(4,66); //over cylinder6
DeguNaoto 30:cd344beb415d 100 wait(0.05);
DeguNaoto 38:f4e9893641ac 101 // sendData(5,75);
DeguNaoto 38:f4e9893641ac 102 sendData(5,72); //front cylinder4
DeguNaoto 30:cd344beb415d 103 wait(0.05);
DeguNaoto 38:f4e9893641ac 104 // sendData(6,1); //middle
DeguNaoto 38:f4e9893641ac 105 sendData(6,3); //middle
DeguNaoto 30:cd344beb415d 106 stateR = 69;
DeguNaoto 30:cd344beb415d 107 stateL = 69;
DeguNaoto 30:cd344beb415d 108 #endif
DeguNaoto 30:cd344beb415d 109 step = 10;
DeguNaoto 30:cd344beb415d 110 CStep = 10;
DeguNaoto 0:b613dc16f27d 111 flaga = 1;
DeguNaoto 0:b613dc16f27d 112 }
DeguNaoto 0:b613dc16f27d 113 }
DeguNaoto 37:75fcd28f48c7 114 if((b==8)&&(!flaga)){ //Middle Start
DeguNaoto 37:75fcd28f48c7 115 if(edge8){
DeguNaoto 37:75fcd28f48c7 116 edge6=0;
DeguNaoto 37:75fcd28f48c7 117 resetState(1);
DeguNaoto 37:75fcd28f48c7 118 flagf=1;
DeguNaoto 37:75fcd28f48c7 119 spcount=0.0;
DeguNaoto 37:75fcd28f48c7 120 dpcount=0.0;
DeguNaoto 37:75fcd28f48c7 121 direction_controller.setBias(0.0);
DeguNaoto 37:75fcd28f48c7 122 #ifdef BLUE
DeguNaoto 37:75fcd28f48c7 123 sendData(5,69); //over
DeguNaoto 37:75fcd28f48c7 124 // sendData(5,69); //over
DeguNaoto 37:75fcd28f48c7 125 wait(0.05);
DeguNaoto 37:75fcd28f48c7 126 sendData(4,70); //front
DeguNaoto 37:75fcd28f48c7 127 // sendData(4,71); //front
DeguNaoto 37:75fcd28f48c7 128 wait(0.05);
DeguNaoto 37:75fcd28f48c7 129 sendData(6,11); //middle
DeguNaoto 37:75fcd28f48c7 130 // sendData(6,7); //middle
DeguNaoto 37:75fcd28f48c7 131 stateR = 69;
DeguNaoto 37:75fcd28f48c7 132 stateL = 69;
DeguNaoto 37:75fcd28f48c7 133 #else
DeguNaoto 38:f4e9893641ac 134 sendData(4,69); //over
DeguNaoto 38:f4e9893641ac 135 // sendData(5,69); //over
DeguNaoto 37:75fcd28f48c7 136 wait(0.05);
DeguNaoto 38:f4e9893641ac 137 sendData(5,71); //front
DeguNaoto 38:f4e9893641ac 138 // sendData(4,71); //front
DeguNaoto 37:75fcd28f48c7 139 wait(0.05);
DeguNaoto 38:f4e9893641ac 140 sendData(6,11); //middle
DeguNaoto 38:f4e9893641ac 141 // sendData(6,7); //middle
DeguNaoto 37:75fcd28f48c7 142 stateR = 69;
DeguNaoto 37:75fcd28f48c7 143 stateL = 69;
DeguNaoto 37:75fcd28f48c7 144 #endif
DeguNaoto 37:75fcd28f48c7 145 step = 0;
DeguNaoto 37:75fcd28f48c7 146 CStep = 4;
DeguNaoto 37:75fcd28f48c7 147 flaga = 1;
DeguNaoto 37:75fcd28f48c7 148 }
DeguNaoto 37:75fcd28f48c7 149 }
DeguNaoto 37:75fcd28f48c7 150 /*else if(b==1){
DeguNaoto 36:b8954b13a6d5 151 if(edge1){
DeguNaoto 36:b8954b13a6d5 152 edge1=0;
DeguNaoto 36:b8954b13a6d5 153 sendData(1,5);
DeguNaoto 36:b8954b13a6d5 154 }
DeguNaoto 36:b8954b13a6d5 155 }
DeguNaoto 36:b8954b13a6d5 156 else if(b==2){
DeguNaoto 36:b8954b13a6d5 157 if(edge2){
DeguNaoto 36:b8954b13a6d5 158 edge2=0;
DeguNaoto 36:b8954b13a6d5 159 sendData(1,4);
DeguNaoto 36:b8954b13a6d5 160 }
DeguNaoto 36:b8954b13a6d5 161 }
DeguNaoto 36:b8954b13a6d5 162 else if(b==3){
DeguNaoto 36:b8954b13a6d5 163 if(edge3){
DeguNaoto 36:b8954b13a6d5 164 edge3=0;
DeguNaoto 36:b8954b13a6d5 165 sendData(1,6);
DeguNaoto 36:b8954b13a6d5 166 }
DeguNaoto 36:b8954b13a6d5 167 }
DeguNaoto 36:b8954b13a6d5 168 else if(b==4){
DeguNaoto 36:b8954b13a6d5 169 if(edge4){
DeguNaoto 36:b8954b13a6d5 170 edge4=0;
DeguNaoto 36:b8954b13a6d5 171 sendData(1,8);
DeguNaoto 36:b8954b13a6d5 172 }
DeguNaoto 37:75fcd28f48c7 173 }*/
DeguNaoto 21:bdf8ac5c200c 174 /*if(a2){
DeguNaoto 0:b613dc16f27d 175 skip = 1;
DeguNaoto 0:b613dc16f27d 176 }
DeguNaoto 0:b613dc16f27d 177 else if(!a2){
DeguNaoto 0:b613dc16f27d 178 skip = 0;
DeguNaoto 21:bdf8ac5c200c 179 }*/
DeguNaoto 21:bdf8ac5c200c 180 skip=0;
DeguNaoto 0:b613dc16f27d 181 if(b!=1) edge1=1;
DeguNaoto 0:b613dc16f27d 182 if(b!=2) edge2=1;
DeguNaoto 0:b613dc16f27d 183 if(b!=3) edge3=1;
DeguNaoto 0:b613dc16f27d 184 if(b!=4) edge4=1;
DeguNaoto 0:b613dc16f27d 185 if(b!=5) edge5=1;
DeguNaoto 0:b613dc16f27d 186 if(b!=6) edge6=1;
DeguNaoto 0:b613dc16f27d 187 if(b!=7) edge7=1;
DeguNaoto 0:b613dc16f27d 188 if(b!=8) edge8=1;
DeguNaoto 0:b613dc16f27d 189 if(b!=9) edge9=1;
DeguNaoto 30:cd344beb415d 190 if(b!=10) edge10=1;
DeguNaoto 30:cd344beb415d 191 if(b!=11) edge11=1;
DeguNaoto 0:b613dc16f27d 192 }
DeguNaoto 0:b613dc16f27d 193
DeguNaoto 0:b613dc16f27d 194
DeguNaoto 30:cd344beb415d 195 #endif /*autoMode.h*/