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@41:cd916d59832d, 2015-11-17 (annotated)

Committer:
DeguNaoto
Date:
Tue Nov 17 08:17:19 2015 +0000
Revision:
41:cd916d59832d
Parent:
40:aed70f4dcd06
Child:
42:c420857a3a6f
20151117 ?????????????

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 40:aed70f4dcd06 72 #ifdef BLUE
DeguNaoto 40:aed70f4dcd06 73 swingspeed = 16.0;
DeguNaoto 40:aed70f4dcd06 74 #else
DeguNaoto 40:aed70f4dcd06 75 swingspeed = 16.0;
DeguNaoto 40:aed70f4dcd06 76 #endif
DeguNaoto 33:a4323c20494b 77 }
DeguNaoto 33:a4323c20494b 78 }
DeguNaoto 38:f4e9893641ac 79 #ifdef BLUE
DeguNaoto 38:f4e9893641ac 80 else if((b==6)&&(!flaga)){ //Swing Start(over)
DeguNaoto 38:f4e9893641ac 81 if(edge6){
DeguNaoto 38:f4e9893641ac 82 edge6=0;
DeguNaoto 38:f4e9893641ac 83 #else
DeguNaoto 38:f4e9893641ac 84 else if((b==7)&&(!flaga)){ //Swing Start(over)
DeguNaoto 38:f4e9893641ac 85 if(edge7){
DeguNaoto 38:f4e9893641ac 86 edge7=0;
DeguNaoto 38:f4e9893641ac 87 #endif
DeguNaoto 36:b8954b13a6d5 88 resetState(0);
DeguNaoto 36:b8954b13a6d5 89 flagf=0;
DeguNaoto 33:a4323c20494b 90 spcount=0.0;
DeguNaoto 33:a4323c20494b 91 dpcount=0.0;
DeguNaoto 33:a4323c20494b 92 direction_controller.setBias(0.0);
DeguNaoto 33:a4323c20494b 93 sendData(5,60); //right
DeguNaoto 33:a4323c20494b 94 wait(0.05);
DeguNaoto 33:a4323c20494b 95 sendData(4,60); //left
DeguNaoto 33:a4323c20494b 96 wait(0.05);
DeguNaoto 36:b8954b13a6d5 97 sendData(6,30); //middle
DeguNaoto 33:a4323c20494b 98 stateR = 90;
DeguNaoto 33:a4323c20494b 99 stateL = 90;
DeguNaoto 36:b8954b13a6d5 100 mstep = 10;
DeguNaoto 36:b8954b13a6d5 101 mCStep = 114;
DeguNaoto 30:cd344beb415d 102 flaga = 1;
DeguNaoto 38:f4e9893641ac 103 #ifdef BLUE
DeguNaoto 37:75fcd28f48c7 104 swingspeed = 18.015; //入った
DeguNaoto 38:f4e9893641ac 105 #else
DeguNaoto 39:5ba406327aa3 106 swingspeed = 18.015;
DeguNaoto 38:f4e9893641ac 107 #endif
DeguNaoto 36:b8954b13a6d5 108 }
DeguNaoto 36:b8954b13a6d5 109 }
DeguNaoto 38:f4e9893641ac 110 #ifdef BLUE
DeguNaoto 38:f4e9893641ac 111 else if((b==7)&&(!flaga)){ //Swing Start(front)
DeguNaoto 36:b8954b13a6d5 112 if(edge7){
DeguNaoto 36:b8954b13a6d5 113 edge7=0;
DeguNaoto 38:f4e9893641ac 114 #else
DeguNaoto 38:f4e9893641ac 115 else if((b==6)&&(!flaga)){ //Swing Start(front)
DeguNaoto 38:f4e9893641ac 116 if(edge6){
DeguNaoto 38:f4e9893641ac 117 edge6=0;
DeguNaoto 38:f4e9893641ac 118 #endif
DeguNaoto 38:f4e9893641ac 119 resetState(0);
DeguNaoto 38:f4e9893641ac 120 flagf=0;
DeguNaoto 38:f4e9893641ac 121 spcount=0.0;
DeguNaoto 38:f4e9893641ac 122 dpcount=0.0;
DeguNaoto 38:f4e9893641ac 123 direction_controller.setBias(0.0);
DeguNaoto 38:f4e9893641ac 124 sendData(5,60); //right
DeguNaoto 38:f4e9893641ac 125 wait(0.05);
DeguNaoto 38:f4e9893641ac 126 sendData(4,60); //left
DeguNaoto 38:f4e9893641ac 127 wait(0.05);
DeguNaoto 38:f4e9893641ac 128 sendData(6,30); //middle
DeguNaoto 38:f4e9893641ac 129 stateR = 90;
DeguNaoto 38:f4e9893641ac 130 stateL = 90;
DeguNaoto 38:f4e9893641ac 131 mstep = 20;
DeguNaoto 38:f4e9893641ac 132 mCStep = 114;
DeguNaoto 38:f4e9893641ac 133 flaga = 1;
DeguNaoto 40:aed70f4dcd06 134 #ifdef BLUE
DeguNaoto 41:cd916d59832d 135 swingspeed = 14.5; //入った
DeguNaoto 40:aed70f4dcd06 136 #else
DeguNaoto 41:cd916d59832d 137 swingspeed = 14.5;
DeguNaoto 40:aed70f4dcd06 138 #endif
DeguNaoto 38:f4e9893641ac 139 }
DeguNaoto 38:f4e9893641ac 140 }
DeguNaoto 38:f4e9893641ac 141 else if((b==8)&&(swingmoved!=0)){
DeguNaoto 38:f4e9893641ac 142 if(edge8){
DeguNaoto 38:f4e9893641ac 143 edge8=0;
DeguNaoto 36:b8954b13a6d5 144 if(swingmoved==1){
DeguNaoto 36:b8954b13a6d5 145 mstep=2;
DeguNaoto 36:b8954b13a6d5 146 swingmoved=0;
DeguNaoto 36:b8954b13a6d5 147 }
DeguNaoto 36:b8954b13a6d5 148 else if(swingmoved==2){
DeguNaoto 36:b8954b13a6d5 149 mstep=12;
DeguNaoto 36:b8954b13a6d5 150 swingmoved=0;
DeguNaoto 36:b8954b13a6d5 151 }
DeguNaoto 36:b8954b13a6d5 152 else if(swingmoved==3){
DeguNaoto 36:b8954b13a6d5 153 mstep=22;
DeguNaoto 36:b8954b13a6d5 154 swingmoved=0;
DeguNaoto 36:b8954b13a6d5 155 }
DeguNaoto 26:760f1bce8214 156 }
DeguNaoto 26:760f1bce8214 157 }
DeguNaoto 34:aa2a5c888a27 158 //Move
DeguNaoto 34:aa2a5c888a27 159 if(b==2){
DeguNaoto 39:5ba406327aa3 160 targ_velocity=speed;
DeguNaoto 38:f4e9893641ac 161 #ifdef BLUE
DeguNaoto 39:5ba406327aa3 162 flagf=1;
DeguNaoto 38:f4e9893641ac 163 #else
DeguNaoto 39:5ba406327aa3 164 flagf=0;
DeguNaoto 38:f4e9893641ac 165 #endif
DeguNaoto 34:aa2a5c888a27 166 move=1;
DeguNaoto 34:aa2a5c888a27 167 }
DeguNaoto 34:aa2a5c888a27 168 else if(b==3){
DeguNaoto 39:5ba406327aa3 169 targ_velocity=speed;
DeguNaoto 38:f4e9893641ac 170 #ifdef BLUE
DeguNaoto 39:5ba406327aa3 171 flagf=0;
DeguNaoto 38:f4e9893641ac 172 #else
DeguNaoto 39:5ba406327aa3 173 flagf=1;
DeguNaoto 38:f4e9893641ac 174 #endif
DeguNaoto 34:aa2a5c888a27 175 move=1;
DeguNaoto 34:aa2a5c888a27 176 }
DeguNaoto 34:aa2a5c888a27 177 else if(move){
DeguNaoto 34:aa2a5c888a27 178 move=0;
DeguNaoto 34:aa2a5c888a27 179 targ_velocity=0.0;
DeguNaoto 34:aa2a5c888a27 180 }
DeguNaoto 34:aa2a5c888a27 181 if(b==1){
DeguNaoto 34:aa2a5c888a27 182 if(edge1){
DeguNaoto 34:aa2a5c888a27 183 edge1=0;
DeguNaoto 37:75fcd28f48c7 184 if(swingmoved!=0){
DeguNaoto 34:aa2a5c888a27 185 #ifdef BLUE
DeguNaoto 37:75fcd28f48c7 186 targ_sita-=PI/60.0;
DeguNaoto 34:aa2a5c888a27 187 #else
DeguNaoto 37:75fcd28f48c7 188 targ_sita+=PI/60.0;
DeguNaoto 37:75fcd28f48c7 189 #endif
DeguNaoto 37:75fcd28f48c7 190 }
DeguNaoto 37:75fcd28f48c7 191 else{
DeguNaoto 37:75fcd28f48c7 192 #ifdef BLUE
DeguNaoto 39:5ba406327aa3 193 // targ_sita-=PI/4.0;
DeguNaoto 39:5ba406327aa3 194 targ_sita-=PI/16.0;
DeguNaoto 37:75fcd28f48c7 195 #else
DeguNaoto 39:5ba406327aa3 196 // targ_sita+=PI/4.0;
DeguNaoto 39:5ba406327aa3 197 targ_sita+=PI/16.0;
DeguNaoto 34:aa2a5c888a27 198 #endif
DeguNaoto 37:75fcd28f48c7 199 }
DeguNaoto 34:aa2a5c888a27 200 }
DeguNaoto 34:aa2a5c888a27 201 }
DeguNaoto 34:aa2a5c888a27 202 else if(b==4){
DeguNaoto 34:aa2a5c888a27 203 if(edge4){
DeguNaoto 34:aa2a5c888a27 204 edge4=0;
DeguNaoto 37:75fcd28f48c7 205 if(swingmoved!=0){
DeguNaoto 34:aa2a5c888a27 206 #ifdef BLUE
DeguNaoto 37:75fcd28f48c7 207 targ_sita+=PI/60.0;
DeguNaoto 37:75fcd28f48c7 208 #else
DeguNaoto 37:75fcd28f48c7 209 targ_sita-=PI/60.0;
DeguNaoto 37:75fcd28f48c7 210 #endif
DeguNaoto 37:75fcd28f48c7 211 }
DeguNaoto 37:75fcd28f48c7 212 else{
DeguNaoto 37:75fcd28f48c7 213 #ifdef BLUE
DeguNaoto 39:5ba406327aa3 214 // targ_sita+=PI/4.0;
DeguNaoto 39:5ba406327aa3 215 targ_sita+=PI/16.0;
DeguNaoto 37:75fcd28f48c7 216 #else
DeguNaoto 39:5ba406327aa3 217 // targ_sita-=PI/4.0;
DeguNaoto 39:5ba406327aa3 218 targ_sita-=PI/16.0;
DeguNaoto 34:aa2a5c888a27 219 #endif
DeguNaoto 37:75fcd28f48c7 220 }
DeguNaoto 34:aa2a5c888a27 221 }
DeguNaoto 34:aa2a5c888a27 222 }
DeguNaoto 0:b613dc16f27d 223 if(b!=1) edge1=1;
DeguNaoto 0:b613dc16f27d 224 if(b!=2) edge2=1;
DeguNaoto 0:b613dc16f27d 225 if(b!=3) edge3=1;
DeguNaoto 0:b613dc16f27d 226 if(b!=4) edge4=1;
DeguNaoto 0:b613dc16f27d 227 if(b!=5) edge5=1;
DeguNaoto 21:bdf8ac5c200c 228 if(b!=6) edge6=1;
DeguNaoto 30:cd344beb415d 229 if(b!=7) edge7=1;
DeguNaoto 0:b613dc16f27d 230 if(b!=8) edge8=1;
DeguNaoto 0:b613dc16f27d 231 if(b!=9) edge9=1;
DeguNaoto 30:cd344beb415d 232 if(b!=10) edge10=1;
DeguNaoto 30:cd344beb415d 233 if(b!=11) edge11=1;
DeguNaoto 0:b613dc16f27d 234 }
DeguNaoto 13:57d8e360e9aa 235
DeguNaoto 30:cd344beb415d 236 #endif /*manualMode.h*/