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@87:6b6b6611d33f, 2015-10-14 (annotated)

Committer:
DeguNaoto
Date:
Wed Oct 14 09:05:46 2015 +0000
Revision:
87:6b6b6611d33f
Parent:
86:5f0b065cb4d3
Child:
88:cbed08732872
20151014 ?????

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 79:7f86e18f40ef 18 #define BLUE
DeguNaoto 79:7f86e18f40ef 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 77:7dd154b5ace3 42 Timeout MStop;
DeguNaoto 77:7dd154b5ace3 43
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 76:f9bcb7ac438f 82 /********************************Nomal Mode*********************************/
DeguNaoto 83:e1638c58e1f1 83 if((step==0)&&((8650.0>x)&&(x>850.0))) {
DeguNaoto 85:dd18a2d79956 84 targ_sita=0.06;
DeguNaoto 83:e1638c58e1f1 85 step=1;
DeguNaoto 83:e1638c58e1f1 86 }
DeguNaoto 83:e1638c58e1f1 87 if((step==1)&&(x>8600.0)) {
DeguNaoto 83:e1638c58e1f1 88 targ_velocity=0.0;
DeguNaoto 79:7f86e18f40ef 89 step=2;
DeguNaoto 83:e1638c58e1f1 90 }
DeguNaoto 83:e1638c58e1f1 91 if((step==2)&&(abs(velocity)<1000.0)) {
DeguNaoto 83:e1638c58e1f1 92 step=3;
DeguNaoto 83:e1638c58e1f1 93 targ_velocity=speed;
DeguNaoto 79:7f86e18f40ef 94 flagf=0;
DeguNaoto 85:dd18a2d79956 95 targ_sita=0.065;
DeguNaoto 79:7f86e18f40ef 96 }
DeguNaoto 86:5f0b065cb4d3 97 if((step==3)&&(x<2000.0)) {
DeguNaoto 66:14df82661dfa 98 targ_sita=PI/4;
DeguNaoto 83:e1638c58e1f1 99 step=4;
unicore32 68:2b2b88ecdcce 100 sendData(7,255);
DeguNaoto 66:14df82661dfa 101 }
DeguNaoto 85:dd18a2d79956 102 if((step==4)&&(x<0.0)) {
DeguNaoto 73:86b7b82ba997 103 targ_velocity=0.0;
DeguNaoto 79:7f86e18f40ef 104 step=114;
DeguNaoto 70:5b8ed76a31c7 105 }
DeguNaoto 76:f9bcb7ac438f 106 /***Cylinder***/
DeguNaoto 86:5f0b065cb4d3 107 if((x>3100.0)&&(CStep==1)) { /*nomal*/
DeguNaoto 72:021a14ee970f 108 if(!skip) sendData(1,1);
DeguNaoto 77:7dd154b5ace3 109 CStep=2;
DeguNaoto 72:021a14ee970f 110 }
DeguNaoto 86:5f0b065cb4d3 111 if((x>5940.0)&&(CStep==2)) {
DeguNaoto 85:dd18a2d79956 112 if(!skip) sendData(1,3);
DeguNaoto 72:021a14ee970f 113 CStep=3;
DeguNaoto 72:021a14ee970f 114 }
DeguNaoto 86:5f0b065cb4d3 115 if((x>7650.0)&&(CStep==3)) {
DeguNaoto 85:dd18a2d79956 116 if(!skip) sendData(1,2);
DeguNaoto 72:021a14ee970f 117 CStep=4;
DeguNaoto 72:021a14ee970f 118 }
DeguNaoto 86:5f0b065cb4d3 119 if((x<6700.0)&&(CStep==4)) {
DeguNaoto 72:021a14ee970f 120 if(!skip) sendData(1,5);
DeguNaoto 72:021a14ee970f 121 CStep=5;
DeguNaoto 72:021a14ee970f 122 }
DeguNaoto 86:5f0b065cb4d3 123 if((x<6160.0)&&(CStep==5)) {
DeguNaoto 72:021a14ee970f 124 if(!skip) sendData(1,4);
DeguNaoto 83:e1638c58e1f1 125 CStep=114;
DeguNaoto 72:021a14ee970f 126 }
DeguNaoto 76:f9bcb7ac438f 127 /////////////////////////////////////////////////////////////////////////
DeguNaoto 76:f9bcb7ac438f 128 /******************************Middle Mode******************************/
DeguNaoto 83:e1638c58e1f1 129 if((step==5)&&((5700.0>x)&&(x>800.0))) {
DeguNaoto 85:dd18a2d79956 130 targ_sita=0.06;
DeguNaoto 76:f9bcb7ac438f 131 step=6;
DeguNaoto 83:e1638c58e1f1 132 }
DeguNaoto 83:e1638c58e1f1 133 if((step==6)&&(x>7000.0)){
DeguNaoto 83:e1638c58e1f1 134 targ_velocity=0.0;
DeguNaoto 83:e1638c58e1f1 135 step=7;
DeguNaoto 76:f9bcb7ac438f 136 }
DeguNaoto 83:e1638c58e1f1 137 if((step==7)&&(abs(velocity)<1000.0)) {
DeguNaoto 83:e1638c58e1f1 138 flagf=0;
DeguNaoto 83:e1638c58e1f1 139 targ_velocity=speed;
DeguNaoto 85:dd18a2d79956 140 targ_sita=0.065;
DeguNaoto 83:e1638c58e1f1 141 step=8;
DeguNaoto 83:e1638c58e1f1 142 }
DeguNaoto 83:e1638c58e1f1 143 if((step==8)&&(x<1900.0)) {
DeguNaoto 76:f9bcb7ac438f 144 targ_sita=PI/4;
DeguNaoto 83:e1638c58e1f1 145 step=9;
unicore32 68:2b2b88ecdcce 146 sendData(7,255);
DeguNaoto 76:f9bcb7ac438f 147 }
DeguNaoto 85:dd18a2d79956 148 if((step==9)&&(x<-10.0)) {
DeguNaoto 76:f9bcb7ac438f 149 targ_velocity=0.0;
DeguNaoto 76:f9bcb7ac438f 150 step=114;
DeguNaoto 76:f9bcb7ac438f 151 }
DeguNaoto 83:e1638c58e1f1 152
DeguNaoto 86:5f0b065cb4d3 153 if((x>4650.0)&&(CStep==6)){
DeguNaoto 83:e1638c58e1f1 154 sendData(1,5);
DeguNaoto 83:e1638c58e1f1 155 CStep=7;
DeguNaoto 83:e1638c58e1f1 156 }
DeguNaoto 86:5f0b065cb4d3 157 if((x>5720.0)&&(CStep==7)){
DeguNaoto 83:e1638c58e1f1 158 sendData(1,4);
DeguNaoto 83:e1638c58e1f1 159 CStep=114;
DeguNaoto 83:e1638c58e1f1 160 }
DeguNaoto 76:f9bcb7ac438f 161 /////////////////////////////////////////////////////////////////////////
DeguNaoto 77:7dd154b5ace3 162 /*****************************Opponents Mode****************************/
DeguNaoto 86:5f0b065cb4d3 163 if((step==15)&&((5700.0>x)&&(x>10.0))) {
DeguNaoto 83:e1638c58e1f1 164 targ_sita=0.03;
DeguNaoto 83:e1638c58e1f1 165 step=16;
DeguNaoto 83:e1638c58e1f1 166 }
DeguNaoto 83:e1638c58e1f1 167 if((step==16)&&(x>2800.0)){
DeguNaoto 79:7f86e18f40ef 168 targ_velocity=0.0;
DeguNaoto 84:919a335ac81e 169 step=17;
DeguNaoto 84:919a335ac81e 170 }
DeguNaoto 84:919a335ac81e 171 if((step==17)&&(abs(velocity)<1000.0)){
DeguNaoto 84:919a335ac81e 172 flagf=0;
DeguNaoto 84:919a335ac81e 173 targ_velocity=speed;
DeguNaoto 85:dd18a2d79956 174 targ_sita=0.03;
DeguNaoto 84:919a335ac81e 175 step=18;
DeguNaoto 84:919a335ac81e 176 }
DeguNaoto 84:919a335ac81e 177 if((step==18)&&(x<-100.0)){
DeguNaoto 84:919a335ac81e 178 targ_velocity=0.0;
DeguNaoto 79:7f86e18f40ef 179 step=114;
DeguNaoto 84:919a335ac81e 180 autoflag=0;
DeguNaoto 84:919a335ac81e 181 }
DeguNaoto 84:919a335ac81e 182
DeguNaoto 85:dd18a2d79956 183 if((x>1550.0)&&(CStep==15)){
DeguNaoto 84:919a335ac81e 184 sendData(1,5);
DeguNaoto 85:dd18a2d79956 185 CStep=16;
DeguNaoto 85:dd18a2d79956 186 }
DeguNaoto 85:dd18a2d79956 187 if((x>2700.0)&&(CStep==16)){
DeguNaoto 85:dd18a2d79956 188 sendData(1,4);
DeguNaoto 84:919a335ac81e 189 CStep=114;
DeguNaoto 77:7dd154b5ace3 190 }
DeguNaoto 77:7dd154b5ace3 191 /////////////////////////////////////////////////////////////////////////
DeguNaoto 71:4ebc1c0fcb4e 192 #else
DeguNaoto 43:f9a75ecbe44e 193 //Red
DeguNaoto 76:f9bcb7ac438f 194 /********************************Nomal Mode*********************************/
DeguNaoto 83:e1638c58e1f1 195 if((step==0)&&((8650.0>x)&&(x>800.0))) {
DeguNaoto 85:dd18a2d79956 196 targ_sita=-0.05;
DeguNaoto 83:e1638c58e1f1 197 step=1;
DeguNaoto 83:e1638c58e1f1 198 }
DeguNaoto 83:e1638c58e1f1 199 if((step==1)&&(x>8600.0)) {
DeguNaoto 83:e1638c58e1f1 200 targ_velocity=0.0;
DeguNaoto 79:7f86e18f40ef 201 step=2;
DeguNaoto 79:7f86e18f40ef 202 }
DeguNaoto 83:e1638c58e1f1 203 if((step==2)&&(abs(velocity)<1000.0)) {
DeguNaoto 71:4ebc1c0fcb4e 204 step=3;
DeguNaoto 83:e1638c58e1f1 205 targ_velocity=speed;
DeguNaoto 83:e1638c58e1f1 206 flagf=0;
DeguNaoto 83:e1638c58e1f1 207 targ_sita=-0.07;
DeguNaoto 71:4ebc1c0fcb4e 208 }
DeguNaoto 83:e1638c58e1f1 209 if((step==3)&&(x<1950.0)) {
DeguNaoto 83:e1638c58e1f1 210 targ_sita=-PI/4;
DeguNaoto 83:e1638c58e1f1 211 step=4;
DeguNaoto 83:e1638c58e1f1 212 sendData(7,255);
DeguNaoto 83:e1638c58e1f1 213 }
DeguNaoto 83:e1638c58e1f1 214 if((step==4)&&(x<-100.0)) {
DeguNaoto 73:86b7b82ba997 215 targ_velocity=0.0;
DeguNaoto 83:e1638c58e1f1 216 step=114;
DeguNaoto 73:86b7b82ba997 217 }
DeguNaoto 83:e1638c58e1f1 218
DeguNaoto 83:e1638c58e1f1 219 if((x>2950.0)&&(CStep==1)) {
DeguNaoto 75:4cdcd50ee666 220 if(!skip) sendData(1,1);
DeguNaoto 75:4cdcd50ee666 221 CStep=2;
DeguNaoto 75:4cdcd50ee666 222 }
DeguNaoto 75:4cdcd50ee666 223 if((x>5850.0)&&(CStep==2)) {
DeguNaoto 85:dd18a2d79956 224 if(!skip) sendData(1,2);
DeguNaoto 75:4cdcd50ee666 225 CStep=3;
DeguNaoto 75:4cdcd50ee666 226 }
DeguNaoto 75:4cdcd50ee666 227 if((x>7600.0)&&(CStep==3)) {
DeguNaoto 85:dd18a2d79956 228 if(!skip) sendData(1,3);
DeguNaoto 75:4cdcd50ee666 229 CStep=6;
DeguNaoto 75:4cdcd50ee666 230 }
DeguNaoto 83:e1638c58e1f1 231 if((x<6650.0)&&(CStep==4)) {
DeguNaoto 75:4cdcd50ee666 232 if(!skip) sendData(1,4);
DeguNaoto 75:4cdcd50ee666 233 CStep=5;
DeguNaoto 75:4cdcd50ee666 234 }
DeguNaoto 83:e1638c58e1f1 235 if((x<6130.0)&&(CStep==5)) {
DeguNaoto 75:4cdcd50ee666 236 if(!skip) sendData(1,5);
DeguNaoto 83:e1638c58e1f1 237 CStep=114;
DeguNaoto 75:4cdcd50ee666 238 }
DeguNaoto 76:f9bcb7ac438f 239 /////////////////////////////////////////////////////////////////////////
DeguNaoto 76:f9bcb7ac438f 240 /******************************Middle Mode******************************/
DeguNaoto 83:e1638c58e1f1 241 if((step==5)&&((5700.0>x)&&(x>800.0))) {
DeguNaoto 83:e1638c58e1f1 242 targ_sita=-0.08;
DeguNaoto 83:e1638c58e1f1 243 step=6;
DeguNaoto 83:e1638c58e1f1 244 }
DeguNaoto 83:e1638c58e1f1 245 if((step==6)&&(x>7000.0)){
DeguNaoto 83:e1638c58e1f1 246 targ_velocity=0.0;
DeguNaoto 83:e1638c58e1f1 247 step=7;
DeguNaoto 77:7dd154b5ace3 248 }
DeguNaoto 83:e1638c58e1f1 249 if((step==7)&&(abs(velocity)<1000.0)) {
DeguNaoto 76:f9bcb7ac438f 250 flagf=0;
DeguNaoto 83:e1638c58e1f1 251 targ_velocity=speed;
DeguNaoto 83:e1638c58e1f1 252 targ_sita=-0.07;
DeguNaoto 83:e1638c58e1f1 253 step=8;
DeguNaoto 76:f9bcb7ac438f 254 }
DeguNaoto 83:e1638c58e1f1 255 if((step==8)&&(x<1900.0)) {
DeguNaoto 76:f9bcb7ac438f 256 targ_sita=-PI/4;
DeguNaoto 83:e1638c58e1f1 257 step=9;
unicore32 78:abb760e0a935 258 sendData(7,255);
DeguNaoto 76:f9bcb7ac438f 259 }
DeguNaoto 84:919a335ac81e 260 if((step==9)&&(x<-100.0)) {
DeguNaoto 76:f9bcb7ac438f 261 targ_velocity=0.0;
DeguNaoto 84:919a335ac81e 262 step=114;
DeguNaoto 76:f9bcb7ac438f 263 autoflag=0;
DeguNaoto 76:f9bcb7ac438f 264 }
DeguNaoto 83:e1638c58e1f1 265
DeguNaoto 83:e1638c58e1f1 266 if((x>4850.0)&&(CStep==6)){
DeguNaoto 83:e1638c58e1f1 267 sendData(1,4);
DeguNaoto 83:e1638c58e1f1 268 CStep=7;
DeguNaoto 83:e1638c58e1f1 269 }
DeguNaoto 84:919a335ac81e 270 /////////////////////////////////////////////////////////////////////////
DeguNaoto 84:919a335ac81e 271 /*****************************Opponents Mode****************************/
DeguNaoto 84:919a335ac81e 272 if((step==15)&&((5700.0>x)&&(x>50.0))) {
DeguNaoto 84:919a335ac81e 273 targ_sita=-0.08;
DeguNaoto 84:919a335ac81e 274 step=16;
DeguNaoto 84:919a335ac81e 275 }
DeguNaoto 84:919a335ac81e 276 if((step==16)&&(x>2800.0)){
DeguNaoto 84:919a335ac81e 277 targ_velocity=0.0;
DeguNaoto 84:919a335ac81e 278 step=17;
DeguNaoto 84:919a335ac81e 279 }
DeguNaoto 84:919a335ac81e 280 if((step==17)&&(abs(velocity)<1000.0)){
DeguNaoto 84:919a335ac81e 281 flagf=0;
DeguNaoto 84:919a335ac81e 282 targ_vellocity=speed;
DeguNaoto 84:919a335ac81e 283 targ_sita=-0.07;
DeguNaoto 84:919a335ac81e 284 step=18;
DeguNaoto 84:919a335ac81e 285 }
DeguNaoto 84:919a335ac81e 286 if((step==18)&&(x<-100.0)){
DeguNaoto 83:e1638c58e1f1 287 targ_velocity=0.0;
DeguNaoto 83:e1638c58e1f1 288 step=114;
DeguNaoto 84:919a335ac81e 289 autoflag=0;
DeguNaoto 84:919a335ac81e 290 }
DeguNaoto 84:919a335ac81e 291
DeguNaoto 84:919a335ac81e 292 if((x>2700.0)&&(CStep==15)){
DeguNaoto 84:919a335ac81e 293 sendData(1,4);
DeguNaoto 84:919a335ac81e 294 CStep=114;
DeguNaoto 83:e1638c58e1f1 295 }
DeguNaoto 76:f9bcb7ac438f 296 /////////////////////////////////////////////////////////////////////////
DeguNaoto 66:14df82661dfa 297 #endif
DeguNaoto 73:86b7b82ba997 298 mesure_state(); /*位置測定*/
DeguNaoto 76:f9bcb7ac438f 299 move_following(); /*移動制御*/
DeguNaoto 4:51d87d2b698c 300 }
DeguNaoto 21:79b94cb922f0 301 else if(!autoflag) {
DeguNaoto 57:3fbd487e055e 302 #ifdef IM920
DeguNaoto 52:d9e1629da852 303 manualMoveIM920(); /*analogStick*/
DeguNaoto 52:d9e1629da852 304 manualIM920(); /*IM920 button*/
DeguNaoto 57:3fbd487e055e 305 #else
DeguNaoto 57:3fbd487e055e 306 manualMovePS3(); /*analogStick*/
DeguNaoto 57:3fbd487e055e 307 manualPS3(); /*PS3 button*/
DeguNaoto 57:3fbd487e055e 308 #endif
DeguNaoto 48:64d005c70df2 309 //Swing
DeguNaoto 87:6b6b6611d33f 310 // swingFollowing();
DeguNaoto 83:e1638c58e1f1 311 #ifdef MESURE
DeguNaoto 83:e1638c58e1f1 312 if(down){
DeguNaoto 50:8ea4714316ce 313 IndicatorAuto=0;
DeguNaoto 50:8ea4714316ce 314 fclose(fp_r);
DeguNaoto 50:8ea4714316ce 315 }
DeguNaoto 50:8ea4714316ce 316 fprintf(fp_r, "time:%1.3f, %f[rad/s], pwm:%f\r\n",time,swingRadVelocity,cont);
DeguNaoto 83:e1638c58e1f1 317 time+=0.01;
DeguNaoto 83:e1638c58e1f1 318 #endif
DeguNaoto 4:51d87d2b698c 319 }
DeguNaoto 55:db1797ac6cb1 320 /***update state***/
DeguNaoto 57:3fbd487e055e 321 #ifdef IM920
DeguNaoto 51:cb430192b28b 322 readIM920();
DeguNaoto 57:3fbd487e055e 323 #endif
DeguNaoto 48:64d005c70df2 324 mesureSwing();
DeguNaoto 0:bd4719e15f7e 325 wait(RATE);
DeguNaoto 55:db1797ac6cb1 326
DeguNaoto 56:ac01d6b46291 327 // 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 328 // pc.printf("A2 = %d, X = %d, Y = %d, B = %d, dead = %d\r\n", a2, X, Y, b, deadflag);
DeguNaoto 57:3fbd487e055e 329 // pc.printf("%f %f\r\n",cont,swingRadVelocity);
DeguNaoto 0:bd4719e15f7e 330 }
DeguNaoto 0:bd4719e15f7e 331 }