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

manualMode.h@39:5ba406327aa3, 2015-11-16 (annotated)

Committer:
DeguNaoto
Date:
Mon Nov 16 10:54:45 2015 +0000
Revision:
39:5ba406327aa3
Parent:
38:f4e9893641ac
Child:
40:aed70f4dcd06
20151116 ???????V???????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
DeguNaoto 0:b613dc16f27d 1 #ifndef MANUALMODE_H
DeguNaoto 0:b613dc16f27d 2 #define MANUALMODE_H
DeguNaoto 0:b613dc16f27d 3
DeguNaoto 0:b613dc16f27d 4 void manualIM920()
DeguNaoto 0:b613dc16f27d 5 {
DeguNaoto 33:a4323c20494b 6 if(modeflag){ //mode change
DeguNaoto 33:a4323c20494b 7 autoflag=1;
DeguNaoto 33:a4323c20494b 8 Indicator4=1;
DeguNaoto 33:a4323c20494b 9 IndicatorAuto=0;
DeguNaoto 33:a4323c20494b 10 resetState(1);
DeguNaoto 33:a4323c20494b 11 flaga=0;
DeguNaoto 33:a4323c20494b 12 step=114;
DeguNaoto 33:a4323c20494b 13 CStep=114;
DeguNaoto 33:a4323c20494b 14 mstep=114;
DeguNaoto 33:a4323c20494b 15 mCStep=114;
DeguNaoto 33:a4323c20494b 16 spcount=speed;
DeguNaoto 33:a4323c20494b 17 dpcount=0.0;
DeguNaoto 36:b8954b13a6d5 18 swingmoved=0;
DeguNaoto 0:b613dc16f27d 19 }
DeguNaoto 38:f4e9893641ac 20 if(b==9){ //swing start&stop
DeguNaoto 34:aa2a5c888a27 21 if(edge9){
DeguNaoto 34:aa2a5c888a27 22 edge9=0;
DeguNaoto 34:aa2a5c888a27 23 toggle9=!toggle9;
DeguNaoto 34:aa2a5c888a27 24 if(toggle9){
DeguNaoto 34:aa2a5c888a27 25 // swingspeed=15.0;
DeguNaoto 34:aa2a5c888a27 26 // swingspeed=20.0;
DeguNaoto 36:b8954b13a6d5 27 // swingspeed=5.0;
DeguNaoto 34:aa2a5c888a27 28 resetSwingSpeed = 1;
DeguNaoto 34:aa2a5c888a27 29 interruptSwingSpeed.attach(&countSwingSpeed, 0.05);
DeguNaoto 34:aa2a5c888a27 30 enableSwing=1;
DeguNaoto 34:aa2a5c888a27 31 /*for(float i=0.0;i<1.0;i+=0.01){
DeguNaoto 34:aa2a5c888a27 32 Motor_swing=i;
DeguNaoto 34:aa2a5c888a27 33 wait(0.01);
DeguNaoto 34:aa2a5c888a27 34 }
DeguNaoto 34:aa2a5c888a27 35 Motor_swing = 1.0;*/
DeguNaoto 3:8d8c25c556ae 36 }
DeguNaoto 34:aa2a5c888a27 37 else{
DeguNaoto 34:aa2a5c888a27 38 interruptSwingSpeed.detach();
DeguNaoto 34:aa2a5c888a27 39 targSwingRadVelocity = 0.0;
DeguNaoto 34:aa2a5c888a27 40 enableSwing=0;
DeguNaoto 34:aa2a5c888a27 41 contSwing.reset();
DeguNaoto 34:aa2a5c888a27 42 Motor_swing = 0.0;
DeguNaoto 34:aa2a5c888a27 43 }
DeguNaoto 3:8d8c25c556ae 44 }
DeguNaoto 3:8d8c25c556ae 45 }
DeguNaoto 38:f4e9893641ac 46 else if(b==10){ //shoot
DeguNaoto 34:aa2a5c888a27 47 if(edge10){
DeguNaoto 34:aa2a5c888a27 48 edge10=0;
DeguNaoto 33:a4323c20494b 49 enableShoot=!enableShoot;
DeguNaoto 33:a4323c20494b 50 if(enableShoot) sendData(1,11);
DeguNaoto 33:a4323c20494b 51 else sendData(1,12);
DeguNaoto 0:b613dc16f27d 52 }
DeguNaoto 0:b613dc16f27d 53 }
DeguNaoto 36:b8954b13a6d5 54 else if((b==5)&&(!flaga)){ //Swing Start(middle)
DeguNaoto 33:a4323c20494b 55 if(edge5){
DeguNaoto 33:a4323c20494b 56 edge5=0;
DeguNaoto 30:cd344beb415d 57 resetState(0);
DeguNaoto 32:b8c8ad2eeca7 58 flagf=0;
DeguNaoto 30:cd344beb415d 59 spcount=0.0;
DeguNaoto 30:cd344beb415d 60 dpcount=0.0;
DeguNaoto 30:cd344beb415d 61 direction_controller.setBias(0.0);
DeguNaoto 32:b8c8ad2eeca7 62 sendData(5,60); //right
DeguNaoto 30:cd344beb415d 63 wait(0.05);
DeguNaoto 32:b8c8ad2eeca7 64 sendData(4,60); //left
DeguNaoto 30:cd344beb415d 65 wait(0.05);
DeguNaoto 32:b8c8ad2eeca7 66 sendData(6,30); //middle
DeguNaoto 30:cd344beb415d 67 stateR = 90;
DeguNaoto 30:cd344beb415d 68 stateL = 90;
DeguNaoto 30:cd344beb415d 69 mstep = 0;
DeguNaoto 33:a4323c20494b 70 mCStep = 114;
DeguNaoto 33:a4323c20494b 71 flaga = 1;
DeguNaoto 37:75fcd28f48c7 72 swingspeed = 17.0;
DeguNaoto 33:a4323c20494b 73 }
DeguNaoto 33:a4323c20494b 74 }
DeguNaoto 38:f4e9893641ac 75 #ifdef BLUE
DeguNaoto 38:f4e9893641ac 76 else if((b==6)&&(!flaga)){ //Swing Start(over)
DeguNaoto 38:f4e9893641ac 77 if(edge6){
DeguNaoto 38:f4e9893641ac 78 edge6=0;
DeguNaoto 38:f4e9893641ac 79 #else
DeguNaoto 38:f4e9893641ac 80 else if((b==7)&&(!flaga)){ //Swing Start(over)
DeguNaoto 38:f4e9893641ac 81 if(edge7){
DeguNaoto 38:f4e9893641ac 82 edge7=0;
DeguNaoto 38:f4e9893641ac 83 #endif
DeguNaoto 36:b8954b13a6d5 84 resetState(0);
DeguNaoto 36:b8954b13a6d5 85 flagf=0;
DeguNaoto 33:a4323c20494b 86 spcount=0.0;
DeguNaoto 33:a4323c20494b 87 dpcount=0.0;
DeguNaoto 33:a4323c20494b 88 direction_controller.setBias(0.0);
DeguNaoto 33:a4323c20494b 89 sendData(5,60); //right
DeguNaoto 33:a4323c20494b 90 wait(0.05);
DeguNaoto 33:a4323c20494b 91 sendData(4,60); //left
DeguNaoto 33:a4323c20494b 92 wait(0.05);
DeguNaoto 36:b8954b13a6d5 93 sendData(6,30); //middle
DeguNaoto 33:a4323c20494b 94 stateR = 90;
DeguNaoto 33:a4323c20494b 95 stateL = 90;
DeguNaoto 36:b8954b13a6d5 96 mstep = 10;
DeguNaoto 36:b8954b13a6d5 97 mCStep = 114;
DeguNaoto 30:cd344beb415d 98 flaga = 1;
DeguNaoto 38:f4e9893641ac 99 #ifdef BLUE
DeguNaoto 37:75fcd28f48c7 100 swingspeed = 18.015; //入った
DeguNaoto 38:f4e9893641ac 101 #else
DeguNaoto 39:5ba406327aa3 102 swingspeed = 18.015;
DeguNaoto 38:f4e9893641ac 103 #endif
DeguNaoto 36:b8954b13a6d5 104 }
DeguNaoto 36:b8954b13a6d5 105 }
DeguNaoto 38:f4e9893641ac 106 #ifdef BLUE
DeguNaoto 38:f4e9893641ac 107 else if((b==7)&&(!flaga)){ //Swing Start(front)
DeguNaoto 36:b8954b13a6d5 108 if(edge7){
DeguNaoto 36:b8954b13a6d5 109 edge7=0;
DeguNaoto 38:f4e9893641ac 110 #else
DeguNaoto 38:f4e9893641ac 111 else if((b==6)&&(!flaga)){ //Swing Start(front)
DeguNaoto 38:f4e9893641ac 112 if(edge6){
DeguNaoto 38:f4e9893641ac 113 edge6=0;
DeguNaoto 38:f4e9893641ac 114 #endif
DeguNaoto 38:f4e9893641ac 115 resetState(0);
DeguNaoto 38:f4e9893641ac 116 flagf=0;
DeguNaoto 38:f4e9893641ac 117 spcount=0.0;
DeguNaoto 38:f4e9893641ac 118 dpcount=0.0;
DeguNaoto 38:f4e9893641ac 119 direction_controller.setBias(0.0);
DeguNaoto 38:f4e9893641ac 120 sendData(5,60); //right
DeguNaoto 38:f4e9893641ac 121 wait(0.05);
DeguNaoto 38:f4e9893641ac 122 sendData(4,60); //left
DeguNaoto 38:f4e9893641ac 123 wait(0.05);
DeguNaoto 38:f4e9893641ac 124 sendData(6,30); //middle
DeguNaoto 38:f4e9893641ac 125 stateR = 90;
DeguNaoto 38:f4e9893641ac 126 stateL = 90;
DeguNaoto 38:f4e9893641ac 127 mstep = 20;
DeguNaoto 38:f4e9893641ac 128 mCStep = 114;
DeguNaoto 38:f4e9893641ac 129 flaga = 1;
DeguNaoto 38:f4e9893641ac 130 swingspeed = 13.0;
DeguNaoto 38:f4e9893641ac 131 }
DeguNaoto 38:f4e9893641ac 132 }
DeguNaoto 38:f4e9893641ac 133 else if((b==8)&&(swingmoved!=0)){
DeguNaoto 38:f4e9893641ac 134 if(edge8){
DeguNaoto 38:f4e9893641ac 135 edge8=0;
DeguNaoto 36:b8954b13a6d5 136 if(swingmoved==1){
DeguNaoto 36:b8954b13a6d5 137 mstep=2;
DeguNaoto 36:b8954b13a6d5 138 swingmoved=0;
DeguNaoto 36:b8954b13a6d5 139 }
DeguNaoto 36:b8954b13a6d5 140 else if(swingmoved==2){
DeguNaoto 36:b8954b13a6d5 141 mstep=12;
DeguNaoto 36:b8954b13a6d5 142 swingmoved=0;
DeguNaoto 36:b8954b13a6d5 143 }
DeguNaoto 36:b8954b13a6d5 144 else if(swingmoved==3){
DeguNaoto 36:b8954b13a6d5 145 mstep=22;
DeguNaoto 36:b8954b13a6d5 146 swingmoved=0;
DeguNaoto 36:b8954b13a6d5 147 }
DeguNaoto 26:760f1bce8214 148 }
DeguNaoto 26:760f1bce8214 149 }
DeguNaoto 34:aa2a5c888a27 150 //Move
DeguNaoto 34:aa2a5c888a27 151 if(b==2){
DeguNaoto 39:5ba406327aa3 152 targ_velocity=speed;
DeguNaoto 38:f4e9893641ac 153 #ifdef BLUE
DeguNaoto 39:5ba406327aa3 154 flagf=1;
DeguNaoto 38:f4e9893641ac 155 #else
DeguNaoto 39:5ba406327aa3 156 flagf=0;
DeguNaoto 38:f4e9893641ac 157 #endif
DeguNaoto 34:aa2a5c888a27 158 move=1;
DeguNaoto 34:aa2a5c888a27 159 }
DeguNaoto 34:aa2a5c888a27 160 else if(b==3){
DeguNaoto 39:5ba406327aa3 161 targ_velocity=speed;
DeguNaoto 38:f4e9893641ac 162 #ifdef BLUE
DeguNaoto 39:5ba406327aa3 163 flagf=0;
DeguNaoto 38:f4e9893641ac 164 #else
DeguNaoto 39:5ba406327aa3 165 flagf=1;
DeguNaoto 38:f4e9893641ac 166 #endif
DeguNaoto 34:aa2a5c888a27 167 move=1;
DeguNaoto 34:aa2a5c888a27 168 }
DeguNaoto 34:aa2a5c888a27 169 else if(move){
DeguNaoto 34:aa2a5c888a27 170 move=0;
DeguNaoto 34:aa2a5c888a27 171 targ_velocity=0.0;
DeguNaoto 34:aa2a5c888a27 172 }
DeguNaoto 34:aa2a5c888a27 173 if(b==1){
DeguNaoto 34:aa2a5c888a27 174 if(edge1){
DeguNaoto 34:aa2a5c888a27 175 edge1=0;
DeguNaoto 37:75fcd28f48c7 176 if(swingmoved!=0){
DeguNaoto 34:aa2a5c888a27 177 #ifdef BLUE
DeguNaoto 37:75fcd28f48c7 178 targ_sita-=PI/60.0;
DeguNaoto 34:aa2a5c888a27 179 #else
DeguNaoto 37:75fcd28f48c7 180 targ_sita+=PI/60.0;
DeguNaoto 37:75fcd28f48c7 181 #endif
DeguNaoto 37:75fcd28f48c7 182 }
DeguNaoto 37:75fcd28f48c7 183 else{
DeguNaoto 37:75fcd28f48c7 184 #ifdef BLUE
DeguNaoto 39:5ba406327aa3 185 // targ_sita-=PI/4.0;
DeguNaoto 39:5ba406327aa3 186 targ_sita-=PI/16.0;
DeguNaoto 37:75fcd28f48c7 187 #else
DeguNaoto 39:5ba406327aa3 188 // targ_sita+=PI/4.0;
DeguNaoto 39:5ba406327aa3 189 targ_sita+=PI/16.0;
DeguNaoto 34:aa2a5c888a27 190 #endif
DeguNaoto 37:75fcd28f48c7 191 }
DeguNaoto 34:aa2a5c888a27 192 }
DeguNaoto 34:aa2a5c888a27 193 }
DeguNaoto 34:aa2a5c888a27 194 else if(b==4){
DeguNaoto 34:aa2a5c888a27 195 if(edge4){
DeguNaoto 34:aa2a5c888a27 196 edge4=0;
DeguNaoto 37:75fcd28f48c7 197 if(swingmoved!=0){
DeguNaoto 34:aa2a5c888a27 198 #ifdef BLUE
DeguNaoto 37:75fcd28f48c7 199 targ_sita+=PI/60.0;
DeguNaoto 37:75fcd28f48c7 200 #else
DeguNaoto 37:75fcd28f48c7 201 targ_sita-=PI/60.0;
DeguNaoto 37:75fcd28f48c7 202 #endif
DeguNaoto 37:75fcd28f48c7 203 }
DeguNaoto 37:75fcd28f48c7 204 else{
DeguNaoto 37:75fcd28f48c7 205 #ifdef BLUE
DeguNaoto 39:5ba406327aa3 206 // targ_sita+=PI/4.0;
DeguNaoto 39:5ba406327aa3 207 targ_sita+=PI/16.0;
DeguNaoto 37:75fcd28f48c7 208 #else
DeguNaoto 39:5ba406327aa3 209 // targ_sita-=PI/4.0;
DeguNaoto 39:5ba406327aa3 210 targ_sita-=PI/16.0;
DeguNaoto 34:aa2a5c888a27 211 #endif
DeguNaoto 37:75fcd28f48c7 212 }
DeguNaoto 34:aa2a5c888a27 213 }
DeguNaoto 34:aa2a5c888a27 214 }
DeguNaoto 0:b613dc16f27d 215 if(b!=1) edge1=1;
DeguNaoto 0:b613dc16f27d 216 if(b!=2) edge2=1;
DeguNaoto 0:b613dc16f27d 217 if(b!=3) edge3=1;
DeguNaoto 0:b613dc16f27d 218 if(b!=4) edge4=1;
DeguNaoto 0:b613dc16f27d 219 if(b!=5) edge5=1;
DeguNaoto 21:bdf8ac5c200c 220 if(b!=6) edge6=1;
DeguNaoto 30:cd344beb415d 221 if(b!=7) edge7=1;
DeguNaoto 0:b613dc16f27d 222 if(b!=8) edge8=1;
DeguNaoto 0:b613dc16f27d 223 if(b!=9) edge9=1;
DeguNaoto 30:cd344beb415d 224 if(b!=10) edge10=1;
DeguNaoto 30:cd344beb415d 225 if(b!=11) edge11=1;
DeguNaoto 0:b613dc16f27d 226 }
DeguNaoto 13:57d8e360e9aa 227
DeguNaoto 30:cd344beb415d 228 #endif /*manualMode.h*/