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@43:a7423cfee3c8, 2015-11-19 (annotated)

Committer:
DeguNaoto
Date:
Thu Nov 19 07:25:27 2015 +0000
Revision:
43:a7423cfee3c8
Parent:
42:c420857a3a6f 
20151119 ????????

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