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

main_ps3.cpp@0:b613dc16f27d, 2015-10-28 (annotated)

Committer:
DeguNaoto
Date:
Wed Oct 28 09:03:19 2015 +0000
Revision:
0:b613dc16f27d
Child:
1:3ac2087996f3
20151028 Ticker??????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
DeguNaoto 0:b613dc16f27d 1 /**
DeguNaoto 0:b613dc16f27d 2 * This program is written in main micro computer "mbed" for 2015 NHK Robot Contest (Bteam).
DeguNaoto 0:b613dc16f27d 3 */
DeguNaoto 0:b613dc16f27d 4
DeguNaoto 0:b613dc16f27d 5 //回転機構wait PI/4 追加で待つ
DeguNaoto 0:b613dc16f27d 6 //射出をスキップする機能を追加
DeguNaoto 0:b613dc16f27d 7 //スタートゾーンにマシンを戻した後に暴走しないようにする
DeguNaoto 0:b613dc16f27d 8 //手動時の操作性の改善
DeguNaoto 0:b613dc16f27d 9 //一度手動モードになったらスタートゾーンに戻るまで再び自動モードにならないようにする
DeguNaoto 0:b613dc16f27d 10 //flagfで前進・後退切り替え
DeguNaoto 0:b613dc16f27d 11 //大和田射角
DeguNaoto 0:b613dc16f27d 12 //午後に一定回転制御の実験
DeguNaoto 0:b613dc16f27d 13 //モード切替時に時間を待つ
DeguNaoto 0:b613dc16f27d 14 //赤チーム側を青チーム側のプログラムに合わせて実装
DeguNaoto 0:b613dc16f27d 15 //横幅調整済み
DeguNaoto 0:b613dc16f27d 16 //相手妨害モードいらない new!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
DeguNaoto 0:b613dc16f27d 17 //赤チームモード、相手チーム側の射出位置調整
DeguNaoto 0:b613dc16f27d 18
DeguNaoto 0:b613dc16f27d 19 /***コース選択***/
DeguNaoto 0:b613dc16f27d 20 //#define BLUE
DeguNaoto 0:b613dc16f27d 21 #define RED
DeguNaoto 0:b613dc16f27d 22
DeguNaoto 0:b613dc16f27d 23 /***コントローラ選択***/
DeguNaoto 0:b613dc16f27d 24 #define IM920
DeguNaoto 0:b613dc16f27d 25 //#define PS3
DeguNaoto 0:b613dc16f27d 26
DeguNaoto 0:b613dc16f27d 27 /***回転機構測定***/
DeguNaoto 0:b613dc16f27d 28 //#define MESURE
DeguNaoto 0:b613dc16f27d 29
DeguNaoto 0:b613dc16f27d 30 #if defined(IM920) && defined(PS3)
DeguNaoto 0:b613dc16f27d 31 #error Caution, You should define either IM920 or PS3
DeguNaoto 0:b613dc16f27d 32 #endif
DeguNaoto 0:b613dc16f27d 33
DeguNaoto 0:b613dc16f27d 34 #if defined(BLUE) && defined(RED)
DeguNaoto 0:b613dc16f27d 35 #error Caution, You should define either BLUE or RED
DeguNaoto 0:b613dc16f27d 36 #endif
DeguNaoto 0:b613dc16f27d 37
DeguNaoto 0:b613dc16f27d 38 #include "machine_ps3.h"
DeguNaoto 0:b613dc16f27d 39
DeguNaoto 0:b613dc16f27d 40 #define deff 70.0
DeguNaoto 0:b613dc16f27d 41
DeguNaoto 0:b613dc16f27d 42 Serial pc(USBTX, USBRX);
DeguNaoto 0:b613dc16f27d 43 #ifdef MESURE
DeguNaoto 0:b613dc16f27d 44 LocalFileSystem local("local");
DeguNaoto 0:b613dc16f27d 45 #endif
DeguNaoto 0:b613dc16f27d 46
DeguNaoto 0:b613dc16f27d 47 short cal=0;
DeguNaoto 0:b613dc16f27d 48 Ticker Com;
DeguNaoto 0:b613dc16f27d 49 void Call(){
DeguNaoto 0:b613dc16f27d 50 mesure_state();
DeguNaoto 0:b613dc16f27d 51 if(autoflag) cal=1;
DeguNaoto 0:b613dc16f27d 52 }
DeguNaoto 0:b613dc16f27d 53
DeguNaoto 0:b613dc16f27d 54 int main() {
DeguNaoto 0:b613dc16f27d 55 Com.attach(&Call,0.01);
DeguNaoto 0:b613dc16f27d 56 #ifdef IM920
DeguNaoto 0:b613dc16f27d 57 initializeIM920();
DeguNaoto 0:b613dc16f27d 58 #else
DeguNaoto 0:b613dc16f27d 59 initializeSBDBT();
DeguNaoto 0:b613dc16f27d 60 #endif
DeguNaoto 0:b613dc16f27d 61 initializeMotors();
DeguNaoto 0:b613dc16f27d 62 initializeControllers();
DeguNaoto 0:b613dc16f27d 63 initializeRS485();
DeguNaoto 0:b613dc16f27d 64 #ifdef BLUE
DeguNaoto 0:b613dc16f27d 65 sita=PI/4.0,targ_sita=PI/4.0;
DeguNaoto 0:b613dc16f27d 66 IndicatorBLUE = 1;
DeguNaoto 0:b613dc16f27d 67 #else
DeguNaoto 0:b613dc16f27d 68 sita=-PI/4.0,targ_sita=-PI/4.0;
DeguNaoto 0:b613dc16f27d 69 IndicatorRED = 1;
DeguNaoto 0:b613dc16f27d 70 #endif
DeguNaoto 0:b613dc16f27d 71 Indicator4=1;
DeguNaoto 0:b613dc16f27d 72 Enable=1;
DeguNaoto 0:b613dc16f27d 73 #ifdef MESURE
DeguNaoto 0:b613dc16f27d 74 FILE *fp_r = fopen("/local/velocity.dat", "w");
DeguNaoto 0:b613dc16f27d 75 double time=0.0;
DeguNaoto 0:b613dc16f27d 76 #endif
DeguNaoto 0:b613dc16f27d 77 wait(0.3);
DeguNaoto 0:b613dc16f27d 78 sendData(7,0);
DeguNaoto 0:b613dc16f27d 79 while(1) {
DeguNaoto 0:b613dc16f27d 80 if(autoflag){
DeguNaoto 0:b613dc16f27d 81 #ifdef IM920
DeguNaoto 0:b613dc16f27d 82 autoIM920(); /*IM920 button*/
DeguNaoto 0:b613dc16f27d 83 #else
DeguNaoto 0:b613dc16f27d 84 autoPS3(); /*PS3 button*/
DeguNaoto 0:b613dc16f27d 85 #endif
DeguNaoto 0:b613dc16f27d 86 #ifdef BLUE
DeguNaoto 0:b613dc16f27d 87 //Blue
DeguNaoto 0:b613dc16f27d 88 if(spcount<speed){
DeguNaoto 0:b613dc16f27d 89 spcount+=speed/100.0;
DeguNaoto 0:b613dc16f27d 90 targ_velocity=spcount;
DeguNaoto 0:b613dc16f27d 91 }
DeguNaoto 0:b613dc16f27d 92 /********************************Nomal Mode*********************************/
DeguNaoto 0:b613dc16f27d 93 if((step==0)&&((8650.0>x)&&(x>800.0))) {
DeguNaoto 0:b613dc16f27d 94 targ_sita=0.025;
DeguNaoto 0:b613dc16f27d 95 step=1;
DeguNaoto 0:b613dc16f27d 96 }
DeguNaoto 0:b613dc16f27d 97 if((step==1)&&(x>8600.0+deff)) {
DeguNaoto 0:b613dc16f27d 98 targ_velocity=0.0;
DeguNaoto 0:b613dc16f27d 99 step=2;
DeguNaoto 0:b613dc16f27d 100 }
DeguNaoto 0:b613dc16f27d 101 if((step==2)&&((velocity<500.0)&&(velocity>-500.0))){
DeguNaoto 0:b613dc16f27d 102 step=3;
DeguNaoto 0:b613dc16f27d 103 spcount=0.0;
DeguNaoto 0:b613dc16f27d 104 flagf=0;
DeguNaoto 0:b613dc16f27d 105 targ_sita=0.035;
DeguNaoto 0:b613dc16f27d 106 }
DeguNaoto 0:b613dc16f27d 107 if((step==3)&&(x<2000.0)) {
DeguNaoto 0:b613dc16f27d 108 targ_sita=PI/4;
DeguNaoto 0:b613dc16f27d 109 step=4;
DeguNaoto 0:b613dc16f27d 110 }
DeguNaoto 0:b613dc16f27d 111 if((step==4)&&(x<0.0)) {
DeguNaoto 0:b613dc16f27d 112 targ_velocity=0.0;
DeguNaoto 0:b613dc16f27d 113 step=114;
DeguNaoto 0:b613dc16f27d 114 }
DeguNaoto 0:b613dc16f27d 115 /***Cylinder***/
DeguNaoto 0:b613dc16f27d 116 if((x>3030.0+deff)&&(CStep==1)) {
DeguNaoto 0:b613dc16f27d 117 if(!skip) sendData(1,1);
DeguNaoto 0:b613dc16f27d 118 CStep=2;
DeguNaoto 0:b613dc16f27d 119 }
DeguNaoto 0:b613dc16f27d 120 if((x>5900.0+deff)&&(CStep==2)) {
DeguNaoto 0:b613dc16f27d 121 if(!skip) sendData(1,3);
DeguNaoto 0:b613dc16f27d 122 CStep=3;
DeguNaoto 0:b613dc16f27d 123 }
DeguNaoto 0:b613dc16f27d 124 if((x>7680.0+deff)&&(CStep==3)) {
DeguNaoto 0:b613dc16f27d 125 if(!skip) sendData(1,2);
DeguNaoto 0:b613dc16f27d 126 CStep=4;
DeguNaoto 0:b613dc16f27d 127 }
DeguNaoto 0:b613dc16f27d 128 if((x<6580.0+deff)&&(CStep==4)) {
DeguNaoto 0:b613dc16f27d 129 if(!skip) sendData(1,5);
DeguNaoto 0:b613dc16f27d 130 CStep=5;
DeguNaoto 0:b613dc16f27d 131 }
DeguNaoto 0:b613dc16f27d 132 if((x<6100.0+deff)&&(CStep==5)) {
DeguNaoto 0:b613dc16f27d 133 if(!skip) sendData(1,4);
DeguNaoto 0:b613dc16f27d 134 CStep=6;
DeguNaoto 0:b613dc16f27d 135 }
DeguNaoto 0:b613dc16f27d 136 if((x<4000.0)&&(CStep==6)){
DeguNaoto 0:b613dc16f27d 137 sendData(7,0);
DeguNaoto 0:b613dc16f27d 138 CStep=114;
DeguNaoto 0:b613dc16f27d 139 }
DeguNaoto 0:b613dc16f27d 140 /////////////////////////////////////////////////////////////////////////
DeguNaoto 0:b613dc16f27d 141 /******************************Middle Mode******************************/
DeguNaoto 0:b613dc16f27d 142 if((step==5)&&((5700.0>x)&&(x>800.0))) {
DeguNaoto 0:b613dc16f27d 143 targ_sita=0.025;
DeguNaoto 0:b613dc16f27d 144 step=6;
DeguNaoto 0:b613dc16f27d 145 }
DeguNaoto 0:b613dc16f27d 146 if((step==6)&&(x>7000.0)){
DeguNaoto 0:b613dc16f27d 147 targ_velocity=0.0;
DeguNaoto 0:b613dc16f27d 148 step=7;
DeguNaoto 0:b613dc16f27d 149 }
DeguNaoto 0:b613dc16f27d 150 if((step==7)&&((velocity<500.0)&&(velocity>-500.0))){
DeguNaoto 0:b613dc16f27d 151 flagf=0;
DeguNaoto 0:b613dc16f27d 152 spcount=0.0;
DeguNaoto 0:b613dc16f27d 153 targ_sita=0.035;
DeguNaoto 0:b613dc16f27d 154 step=8;
DeguNaoto 0:b613dc16f27d 155 }
DeguNaoto 0:b613dc16f27d 156 if((step==8)&&(x<1900.0)) {
DeguNaoto 0:b613dc16f27d 157 targ_sita=PI/4;
DeguNaoto 0:b613dc16f27d 158 step=9;
DeguNaoto 0:b613dc16f27d 159 }
DeguNaoto 0:b613dc16f27d 160 if((step==9)&&(x<-10.0)) {
DeguNaoto 0:b613dc16f27d 161 targ_velocity=0.0;
DeguNaoto 0:b613dc16f27d 162 step=114;
DeguNaoto 0:b613dc16f27d 163 }
DeguNaoto 0:b613dc16f27d 164
DeguNaoto 0:b613dc16f27d 165 if((x>4620.0+deff)&&(CStep==7)){
DeguNaoto 0:b613dc16f27d 166 sendData(1,5);
DeguNaoto 0:b613dc16f27d 167 CStep=8;
DeguNaoto 0:b613dc16f27d 168 }
DeguNaoto 0:b613dc16f27d 169 if((x>5720.0+deff)&&(CStep==8)){
DeguNaoto 0:b613dc16f27d 170 sendData(1,4);
DeguNaoto 0:b613dc16f27d 171 CStep=9;
DeguNaoto 0:b613dc16f27d 172 }
DeguNaoto 0:b613dc16f27d 173 if((x<4000.0)&&(CStep==9)){
DeguNaoto 0:b613dc16f27d 174 sendData(7,0);
DeguNaoto 0:b613dc16f27d 175 CStep=114;
DeguNaoto 0:b613dc16f27d 176 }
DeguNaoto 0:b613dc16f27d 177 /////////////////////////////////////////////////////////////////////////
DeguNaoto 0:b613dc16f27d 178 /*****************************Opponents Mode****************************/
DeguNaoto 0:b613dc16f27d 179 if((step==15)&&((5700.0>x)&&(x>50.0))) {
DeguNaoto 0:b613dc16f27d 180 targ_sita=0.025;
DeguNaoto 0:b613dc16f27d 181 step=16;
DeguNaoto 0:b613dc16f27d 182 }
DeguNaoto 0:b613dc16f27d 183 if((step==16)&&(x>2850.0+deff)){
DeguNaoto 0:b613dc16f27d 184 targ_velocity=0.0;
DeguNaoto 0:b613dc16f27d 185 step=17;
DeguNaoto 0:b613dc16f27d 186 }
DeguNaoto 0:b613dc16f27d 187 if((step==17)&&((velocity<5000.0)&&(velocity>-5000.0))){
DeguNaoto 0:b613dc16f27d 188 flagf=0;
DeguNaoto 0:b613dc16f27d 189 spcount=0.0;
DeguNaoto 0:b613dc16f27d 190 targ_sita=0.035;
DeguNaoto 0:b613dc16f27d 191 step=18;
DeguNaoto 0:b613dc16f27d 192 }
DeguNaoto 0:b613dc16f27d 193 if((step==18)&&(x<-100.0)){
DeguNaoto 0:b613dc16f27d 194 targ_velocity=0.0;
DeguNaoto 0:b613dc16f27d 195 targ_sita=PI/4.0;
DeguNaoto 0:b613dc16f27d 196 step=114;
DeguNaoto 0:b613dc16f27d 197 }
DeguNaoto 0:b613dc16f27d 198
DeguNaoto 0:b613dc16f27d 199 // if((x>1200.0+deff)&&(CStep==15)){
DeguNaoto 0:b613dc16f27d 200 // if((x>1000.0+deff)&&(CStep==15)){
DeguNaoto 0:b613dc16f27d 201 if((x>1050.0+deff)&&(CStep==15)){
DeguNaoto 0:b613dc16f27d 202 sendData(1,5);
DeguNaoto 0:b613dc16f27d 203 CStep=16;
DeguNaoto 0:b613dc16f27d 204 }
DeguNaoto 0:b613dc16f27d 205 if((x>2505.0+deff)&&(CStep==16)){
DeguNaoto 0:b613dc16f27d 206 // if((x>2455.0+deff)&&(CStep==16)){
DeguNaoto 0:b613dc16f27d 207 // if((x>2630.0)&&(CStep==16)){
DeguNaoto 0:b613dc16f27d 208 sendData(1,4);
DeguNaoto 0:b613dc16f27d 209 CStep=17;
DeguNaoto 0:b613dc16f27d 210 }
DeguNaoto 0:b613dc16f27d 211 if((x<2400.0+deff)&&(CStep==17)){
DeguNaoto 0:b613dc16f27d 212 sendData(7,0);
DeguNaoto 0:b613dc16f27d 213 CStep=114;
DeguNaoto 0:b613dc16f27d 214 }
DeguNaoto 0:b613dc16f27d 215 #else
DeguNaoto 0:b613dc16f27d 216 //Red
DeguNaoto 0:b613dc16f27d 217 if(spcount<speed){
DeguNaoto 0:b613dc16f27d 218 spcount+=speed/100.0;
DeguNaoto 0:b613dc16f27d 219 targ_velocity=spcount;
DeguNaoto 0:b613dc16f27d 220 }
DeguNaoto 0:b613dc16f27d 221 /********************************Nomal Mode*********************************/
DeguNaoto 0:b613dc16f27d 222 if((step==0)&&((8650.0>x)&&(x>800.0))) {
DeguNaoto 0:b613dc16f27d 223 targ_sita=-0.025;
DeguNaoto 0:b613dc16f27d 224 step=1;
DeguNaoto 0:b613dc16f27d 225 }
DeguNaoto 0:b613dc16f27d 226 if((step==1)&&(x>8600.0+deff)) {
DeguNaoto 0:b613dc16f27d 227 targ_velocity=0.0;
DeguNaoto 0:b613dc16f27d 228 step=2;
DeguNaoto 0:b613dc16f27d 229 }
DeguNaoto 0:b613dc16f27d 230 if((step==2)&&((velocity<500.0)&&(velocity>-500.0))){
DeguNaoto 0:b613dc16f27d 231 step=3;
DeguNaoto 0:b613dc16f27d 232 spcount=0.0;
DeguNaoto 0:b613dc16f27d 233 flagf=0;
DeguNaoto 0:b613dc16f27d 234 targ_sita=-0.035;
DeguNaoto 0:b613dc16f27d 235 }
DeguNaoto 0:b613dc16f27d 236 if((step==3)&&(x<2000.0)) {
DeguNaoto 0:b613dc16f27d 237 targ_sita=-PI/4;
DeguNaoto 0:b613dc16f27d 238 step=4;
DeguNaoto 0:b613dc16f27d 239 }
DeguNaoto 0:b613dc16f27d 240 if((step==4)&&(x<0.0)) {
DeguNaoto 0:b613dc16f27d 241 targ_velocity=0.0;
DeguNaoto 0:b613dc16f27d 242 step=114;
DeguNaoto 0:b613dc16f27d 243 }
DeguNaoto 0:b613dc16f27d 244 /***Cylinder***/
DeguNaoto 0:b613dc16f27d 245 // if((x>3030.0+deff)&&(CStep==1)) {
DeguNaoto 0:b613dc16f27d 246 if((x>3100.0+deff)&&(CStep==1)) {
DeguNaoto 0:b613dc16f27d 247 if(!skip) sendData(1,1);
DeguNaoto 0:b613dc16f27d 248 CStep=2;
DeguNaoto 0:b613dc16f27d 249 }
DeguNaoto 0:b613dc16f27d 250 // if((x>5940.0+deff)&&(CStep==2)) {
DeguNaoto 0:b613dc16f27d 251 if((x>6000.0+deff)&&(CStep==2)) {
DeguNaoto 0:b613dc16f27d 252 if(!skip) sendData(1,2);
DeguNaoto 0:b613dc16f27d 253 CStep=3;
DeguNaoto 0:b613dc16f27d 254 }
DeguNaoto 0:b613dc16f27d 255 if((x>7680.0+deff)&&(CStep==3)) {
DeguNaoto 0:b613dc16f27d 256 if(!skip) sendData(1,3);
DeguNaoto 0:b613dc16f27d 257 CStep=4;
DeguNaoto 0:b613dc16f27d 258 }
DeguNaoto 0:b613dc16f27d 259 if((x<6730.0+deff)&&(CStep==4)) {
DeguNaoto 0:b613dc16f27d 260 if(!skip) sendData(1,4);
DeguNaoto 0:b613dc16f27d 261 CStep=5;
DeguNaoto 0:b613dc16f27d 262 }
DeguNaoto 0:b613dc16f27d 263 if((x<6170.0+deff)&&(CStep==5)) {
DeguNaoto 0:b613dc16f27d 264 if(!skip) sendData(1,5);
DeguNaoto 0:b613dc16f27d 265 CStep=6;
DeguNaoto 0:b613dc16f27d 266 }
DeguNaoto 0:b613dc16f27d 267 if((x<4000.0)&&(CStep==6)){
DeguNaoto 0:b613dc16f27d 268 sendData(7,0);
DeguNaoto 0:b613dc16f27d 269 CStep=114;
DeguNaoto 0:b613dc16f27d 270 }
DeguNaoto 0:b613dc16f27d 271 /////////////////////////////////////////////////////////////////////////
DeguNaoto 0:b613dc16f27d 272 /******************************Middle Mode******************************/
DeguNaoto 0:b613dc16f27d 273 if((step==5)&&((5700.0>x)&&(x>800.0))) {
DeguNaoto 0:b613dc16f27d 274 targ_sita=-0.025;
DeguNaoto 0:b613dc16f27d 275 step=6;
DeguNaoto 0:b613dc16f27d 276 }
DeguNaoto 0:b613dc16f27d 277 if((step==6)&&(x>7000.0)){
DeguNaoto 0:b613dc16f27d 278 targ_velocity=0.0;
DeguNaoto 0:b613dc16f27d 279 step=7;
DeguNaoto 0:b613dc16f27d 280 }
DeguNaoto 0:b613dc16f27d 281 if((step==7)&&((velocity<500.0)&&(velocity>-500.0))){
DeguNaoto 0:b613dc16f27d 282 flagf=0;
DeguNaoto 0:b613dc16f27d 283 spcount=0.0;
DeguNaoto 0:b613dc16f27d 284 targ_sita=-0.035;
DeguNaoto 0:b613dc16f27d 285 step=8;
DeguNaoto 0:b613dc16f27d 286 }
DeguNaoto 0:b613dc16f27d 287 if((step==8)&&(x<1820.0)) {
DeguNaoto 0:b613dc16f27d 288 targ_sita=-PI/4;
DeguNaoto 0:b613dc16f27d 289 step=9;
DeguNaoto 0:b613dc16f27d 290 }
DeguNaoto 0:b613dc16f27d 291 if((step==9)&&(x<-10.0)) {
DeguNaoto 0:b613dc16f27d 292 targ_velocity=0.0;
DeguNaoto 0:b613dc16f27d 293 step=114;
DeguNaoto 0:b613dc16f27d 294 }
DeguNaoto 0:b613dc16f27d 295
DeguNaoto 0:b613dc16f27d 296 if((x>4690.0+deff)&&(CStep==7)){
DeguNaoto 0:b613dc16f27d 297 sendData(1,4);
DeguNaoto 0:b613dc16f27d 298 CStep=8;
DeguNaoto 0:b613dc16f27d 299 }
DeguNaoto 0:b613dc16f27d 300 if((x>5790.0+deff)&&(CStep==8)){
DeguNaoto 0:b613dc16f27d 301 sendData(1,5);
DeguNaoto 0:b613dc16f27d 302 CStep=9;
DeguNaoto 0:b613dc16f27d 303 }
DeguNaoto 0:b613dc16f27d 304 if((x<4000.0)&&(CStep==9)){
DeguNaoto 0:b613dc16f27d 305 sendData(7,0);
DeguNaoto 0:b613dc16f27d 306 CStep=114;
DeguNaoto 0:b613dc16f27d 307 }
DeguNaoto 0:b613dc16f27d 308 /////////////////////////////////////////////////////////////////////////
DeguNaoto 0:b613dc16f27d 309 /*****************************Opponents Mode****************************/
DeguNaoto 0:b613dc16f27d 310 if((step==15)&&((5700.0>x)&&(x>50.0))) {
DeguNaoto 0:b613dc16f27d 311 // targ_sita=-0.035;
DeguNaoto 0:b613dc16f27d 312 targ_sita=-0.025;
DeguNaoto 0:b613dc16f27d 313 step=16;
DeguNaoto 0:b613dc16f27d 314 }
DeguNaoto 0:b613dc16f27d 315 // if((step==16)&&(x>2800.0+deff)){
DeguNaoto 0:b613dc16f27d 316 if((step==16)&&(x>2850.0+deff)){
DeguNaoto 0:b613dc16f27d 317 targ_velocity=0.0;
DeguNaoto 0:b613dc16f27d 318 step=17;
DeguNaoto 0:b613dc16f27d 319 }
DeguNaoto 0:b613dc16f27d 320 if((step==17)&&((velocity<5000.0)&&(velocity>-5000.0))){
DeguNaoto 0:b613dc16f27d 321 flagf=0;
DeguNaoto 0:b613dc16f27d 322 spcount=0.0;
DeguNaoto 0:b613dc16f27d 323 targ_sita=-0.035;
DeguNaoto 0:b613dc16f27d 324 step=18;
DeguNaoto 0:b613dc16f27d 325 }
DeguNaoto 0:b613dc16f27d 326 if((step==18)&&(x<-100.0)){
DeguNaoto 0:b613dc16f27d 327 targ_velocity=0.0;
DeguNaoto 0:b613dc16f27d 328 targ_sita=-PI/4.0;
DeguNaoto 0:b613dc16f27d 329 step=114;
DeguNaoto 0:b613dc16f27d 330 }
DeguNaoto 0:b613dc16f27d 331
DeguNaoto 0:b613dc16f27d 332 // if((x>1350.0+deff)&&(CStep==15)){
DeguNaoto 0:b613dc16f27d 333 if((x>1400.0+deff)&&(CStep==15)){
DeguNaoto 0:b613dc16f27d 334 sendData(1,4);
DeguNaoto 0:b613dc16f27d 335 CStep=16;
DeguNaoto 0:b613dc16f27d 336 }
DeguNaoto 0:b613dc16f27d 337 if((x>2700.0+deff)&&(CStep==16)){
DeguNaoto 0:b613dc16f27d 338 // if((x>2650.0+deff)&&(CStep==16)){
DeguNaoto 0:b613dc16f27d 339 // if((x>2850.0+deff)&&(CStep==16)){
DeguNaoto 0:b613dc16f27d 340 sendData(1,5);
DeguNaoto 0:b613dc16f27d 341 CStep=17;
DeguNaoto 0:b613dc16f27d 342 }
DeguNaoto 0:b613dc16f27d 343 if((x<2400.0+deff)&&(CStep==17)){
DeguNaoto 0:b613dc16f27d 344 sendData(7,0);
DeguNaoto 0:b613dc16f27d 345 CStep=114;
DeguNaoto 0:b613dc16f27d 346
DeguNaoto 0:b613dc16f27d 347 }
DeguNaoto 0:b613dc16f27d 348 #endif
DeguNaoto 0:b613dc16f27d 349 // mesure_state(); /*位置測定*/
DeguNaoto 0:b613dc16f27d 350 // move_following(); /*移動制御*/
DeguNaoto 0:b613dc16f27d 351 if(cal){
DeguNaoto 0:b613dc16f27d 352 cal=0;
DeguNaoto 0:b613dc16f27d 353 move_following();
DeguNaoto 0:b613dc16f27d 354 }
DeguNaoto 0:b613dc16f27d 355 }
DeguNaoto 0:b613dc16f27d 356 else if(!autoflag) {
DeguNaoto 0:b613dc16f27d 357 flaga=0;
DeguNaoto 0:b613dc16f27d 358 #ifdef IM920
DeguNaoto 0:b613dc16f27d 359 manualMoveIM920(); /*analogStick*/
DeguNaoto 0:b613dc16f27d 360 manualIM920(); /*IM920 button*/
DeguNaoto 0:b613dc16f27d 361 #else
DeguNaoto 0:b613dc16f27d 362 manualMovePS3(); /*analogStick*/
DeguNaoto 0:b613dc16f27d 363 manualPS3(); /*PS3 button*/
DeguNaoto 0:b613dc16f27d 364 #endif
DeguNaoto 0:b613dc16f27d 365 #ifdef MESURE
DeguNaoto 0:b613dc16f27d 366 if(down){
DeguNaoto 0:b613dc16f27d 367 IndicatorAuto=0;
DeguNaoto 0:b613dc16f27d 368 fclose(fp_r);
DeguNaoto 0:b613dc16f27d 369 }
DeguNaoto 0:b613dc16f27d 370 time+=0.01;
DeguNaoto 0:b613dc16f27d 371 #endif
DeguNaoto 0:b613dc16f27d 372 }
DeguNaoto 0:b613dc16f27d 373 /***update state***/
DeguNaoto 0:b613dc16f27d 374 #ifdef IM920
DeguNaoto 0:b613dc16f27d 375 readIM920();
DeguNaoto 0:b613dc16f27d 376 #endif
DeguNaoto 0:b613dc16f27d 377 // mesureSwing();
DeguNaoto 0:b613dc16f27d 378 pc.printf("x:%f,y:%f\r\n",x,y);
DeguNaoto 0:b613dc16f27d 379 // wait(RATE);
DeguNaoto 0:b613dc16f27d 380
DeguNaoto 0:b613dc16f27d 381 // pc.printf("%f\r\n",(float)((((2.0 * PI) / swingRadVelocity) / 4.0)));
DeguNaoto 0:b613dc16f27d 382 // pc.printf("sita:%f, x:%f, y:%f ,x1:%f, x2:%f ,velocity:%f\r\n",sita,x,y,x1,x2,velocity);
DeguNaoto 0:b613dc16f27d 383 // pc.printf("A2 = %d, X = %d, Y = %d, B = %d, dead = %d\r\n", a2, X, Y, b, deadflag);
DeguNaoto 0:b613dc16f27d 384 // pc.printf("%f %f %f\r\n",cont,swingRadVelocity, (double)SwingSens.getPulses());
DeguNaoto 0:b613dc16f27d 385 }
DeguNaoto 0:b613dc16f27d 386 }