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@91:83c2e0381384, 2015-10-15 (annotated)

Committer:
unicore32
Date:
Thu Oct 15 12:01:31 2015 +0000
Revision:
91:83c2e0381384
Parent:
90:9fe39ebe721f
Child:
93:9cacc1763463
20151015 ??????

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