2015_robocon_bteam / Mbed 2 deprecated 2015robot_main

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

Fork of 2015robot_main by Naoto Deguchi

main_ps3.cpp@102:e483c7453f52, 2015-10-17 (annotated)

Committer:
DeguNaoto
Date:
Sat Oct 17 09:53:52 2015 +0000
Revision:
102:e483c7453f52
Parent:
101:b67d33e56b66 
20151017 ??

Who changed what in which revision?

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