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@96:a972ba5cf2d7, 2015-10-16 (annotated)

Committer:
DeguNaoto
Date:
Fri Oct 16 07:12:29 2015 +0000
Revision:
96:a972ba5cf2d7
Parent:
93:9cacc1763463
Child:
97:5e6f513b26f2
20151016 ????????????????????????

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 92:3a12444b5192 18 //#define BLUE
DeguNaoto 92:3a12444b5192 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 92:3a12444b5192 87 if((step==0)&&((8650.0>x)&&(x>800.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 92:3a12444b5192 157 if((x>4620.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 92:3a12444b5192 198 if(spcount<speed){
DeguNaoto 92:3a12444b5192 199 spcount+=speed/100.0;
DeguNaoto 92:3a12444b5192 200 targ_velocity=spcount;
DeguNaoto 92:3a12444b5192 201 }
DeguNaoto 76:f9bcb7ac438f 202 /********************************Nomal Mode*********************************/
DeguNaoto 92:3a12444b5192 203 if((step==0)&&((8650.0>x)&&(x>850.0))) {
DeguNaoto 92:3a12444b5192 204 targ_sita=-0.035;
DeguNaoto 83:e1638c58e1f1 205 step=1;
DeguNaoto 83:e1638c58e1f1 206 }
DeguNaoto 83:e1638c58e1f1 207 if((step==1)&&(x>8600.0)) {
DeguNaoto 83:e1638c58e1f1 208 targ_velocity=0.0;
DeguNaoto 79:7f86e18f40ef 209 step=2;
DeguNaoto 79:7f86e18f40ef 210 }
DeguNaoto 92:3a12444b5192 211 if((step==2)&&((velocity<500.0)&&(velocity>-500.0))){
DeguNaoto 71:4ebc1c0fcb4e 212 step=3;
DeguNaoto 92:3a12444b5192 213 spcount=0.0;
DeguNaoto 83:e1638c58e1f1 214 flagf=0;
DeguNaoto 92:3a12444b5192 215 targ_sita=-0.045;
DeguNaoto 71:4ebc1c0fcb4e 216 }
DeguNaoto 92:3a12444b5192 217 if((step==3)&&(x<2000.0)) {
DeguNaoto 83:e1638c58e1f1 218 targ_sita=-PI/4;
DeguNaoto 83:e1638c58e1f1 219 step=4;
DeguNaoto 83:e1638c58e1f1 220 sendData(7,255);
DeguNaoto 83:e1638c58e1f1 221 }
DeguNaoto 92:3a12444b5192 222 if((step==4)&&(x<0.0)) {
DeguNaoto 73:86b7b82ba997 223 targ_velocity=0.0;
DeguNaoto 83:e1638c58e1f1 224 step=114;
DeguNaoto 73:86b7b82ba997 225 }
DeguNaoto 83:e1638c58e1f1 226
DeguNaoto 96:a972ba5cf2d7 227 if((x>3100.0)&&(CStep==1)) {
DeguNaoto 75:4cdcd50ee666 228 if(!skip) sendData(1,1);
DeguNaoto 75:4cdcd50ee666 229 CStep=2;
DeguNaoto 75:4cdcd50ee666 230 }
DeguNaoto 96:a972ba5cf2d7 231 if((x>5940.0)&&(CStep==2)) {
DeguNaoto 85:dd18a2d79956 232 if(!skip) sendData(1,2);
DeguNaoto 75:4cdcd50ee666 233 CStep=3;
DeguNaoto 75:4cdcd50ee666 234 }
DeguNaoto 96:a972ba5cf2d7 235 if((x>7650.0)&&(CStep==3)) {
DeguNaoto 85:dd18a2d79956 236 if(!skip) sendData(1,3);
DeguNaoto 75:4cdcd50ee666 237 CStep=6;
DeguNaoto 75:4cdcd50ee666 238 }
DeguNaoto 96:a972ba5cf2d7 239 if((x<6700.0)&&(CStep==4)) {
DeguNaoto 75:4cdcd50ee666 240 if(!skip) sendData(1,4);
DeguNaoto 75:4cdcd50ee666 241 CStep=5;
DeguNaoto 75:4cdcd50ee666 242 }
DeguNaoto 96:a972ba5cf2d7 243 if((x<6160.0)&&(CStep==5)) {
DeguNaoto 75:4cdcd50ee666 244 if(!skip) sendData(1,5);
DeguNaoto 83:e1638c58e1f1 245 CStep=114;
DeguNaoto 75:4cdcd50ee666 246 }
DeguNaoto 76:f9bcb7ac438f 247 /////////////////////////////////////////////////////////////////////////
DeguNaoto 76:f9bcb7ac438f 248 /******************************Middle Mode******************************/
DeguNaoto 92:3a12444b5192 249 if((step==5)&&((5700.0>x)&&(x>800.0))) {
DeguNaoto 92:3a12444b5192 250 targ_sita=-0.035;
DeguNaoto 92:3a12444b5192 251 step=6;
DeguNaoto 83:e1638c58e1f1 252 }
DeguNaoto 83:e1638c58e1f1 253 if((step==6)&&(x>7000.0)){
DeguNaoto 83:e1638c58e1f1 254 targ_velocity=0.0;
DeguNaoto 83:e1638c58e1f1 255 step=7;
DeguNaoto 77:7dd154b5ace3 256 }
DeguNaoto 92:3a12444b5192 257 if((step==7)&&((velocity<500.0)&&(velocity>-500.0))){
DeguNaoto 76:f9bcb7ac438f 258 flagf=0;
DeguNaoto 92:3a12444b5192 259 spcount=0.0;
DeguNaoto 92:3a12444b5192 260 targ_sita=-0.045;
DeguNaoto 83:e1638c58e1f1 261 step=8;
DeguNaoto 76:f9bcb7ac438f 262 }
DeguNaoto 83:e1638c58e1f1 263 if((step==8)&&(x<1900.0)) {
DeguNaoto 76:f9bcb7ac438f 264 targ_sita=-PI/4;
DeguNaoto 83:e1638c58e1f1 265 step=9;
unicore32 78:abb760e0a935 266 sendData(7,255);
DeguNaoto 76:f9bcb7ac438f 267 }
DeguNaoto 92:3a12444b5192 268 if((step==9)&&(x<-10.0)) {
DeguNaoto 76:f9bcb7ac438f 269 targ_velocity=0.0;
DeguNaoto 84:919a335ac81e 270 step=114;
DeguNaoto 76:f9bcb7ac438f 271 }
DeguNaoto 83:e1638c58e1f1 272
DeguNaoto 92:3a12444b5192 273 if((x>4620.0)&&(CStep==6)){
DeguNaoto 83:e1638c58e1f1 274 sendData(1,4);
DeguNaoto 83:e1638c58e1f1 275 CStep=7;
DeguNaoto 83:e1638c58e1f1 276 }
DeguNaoto 92:3a12444b5192 277 if((x>5720.0)&&(CStep==7)){
DeguNaoto 92:3a12444b5192 278 sendData(1,5);
DeguNaoto 92:3a12444b5192 279 CStep=114;
DeguNaoto 92:3a12444b5192 280 }
DeguNaoto 84:919a335ac81e 281 /////////////////////////////////////////////////////////////////////////
DeguNaoto 84:919a335ac81e 282 /*****************************Opponents Mode****************************/
DeguNaoto 92:3a12444b5192 283 if((step==15)&&((5700.0>x)&&(x>70.0))) {
DeguNaoto 92:3a12444b5192 284 targ_sita=-0.035;
DeguNaoto 84:919a335ac81e 285 step=16;
DeguNaoto 84:919a335ac81e 286 }
DeguNaoto 84:919a335ac81e 287 if((step==16)&&(x>2800.0)){
DeguNaoto 84:919a335ac81e 288 targ_velocity=0.0;
DeguNaoto 84:919a335ac81e 289 step=17;
DeguNaoto 84:919a335ac81e 290 }
DeguNaoto 92:3a12444b5192 291 if((step==17)&&((velocity<500.0)&&(velocity>-500.0))){
DeguNaoto 84:919a335ac81e 292 flagf=0;
DeguNaoto 92:3a12444b5192 293 spcount=0.0;
DeguNaoto 92:3a12444b5192 294 targ_sita=-0.045;
DeguNaoto 84:919a335ac81e 295 step=18;
DeguNaoto 84:919a335ac81e 296 }
DeguNaoto 84:919a335ac81e 297 if((step==18)&&(x<-100.0)){
DeguNaoto 83:e1638c58e1f1 298 targ_velocity=0.0;
DeguNaoto 92:3a12444b5192 299 targ_sita=-PI/4.0;
DeguNaoto 83:e1638c58e1f1 300 step=114;
DeguNaoto 84:919a335ac81e 301 }
DeguNaoto 84:919a335ac81e 302
DeguNaoto 84:919a335ac81e 303 if((x>2700.0)&&(CStep==15)){
DeguNaoto 84:919a335ac81e 304 sendData(1,4);
DeguNaoto 84:919a335ac81e 305 CStep=114;
DeguNaoto 83:e1638c58e1f1 306 }
DeguNaoto 76:f9bcb7ac438f 307 /////////////////////////////////////////////////////////////////////////
DeguNaoto 66:14df82661dfa 308 #endif
DeguNaoto 73:86b7b82ba997 309 mesure_state(); /*位置測定*/
DeguNaoto 76:f9bcb7ac438f 310 move_following(); /*移動制御*/
DeguNaoto 4:51d87d2b698c 311 }
DeguNaoto 21:79b94cb922f0 312 else if(!autoflag) {
DeguNaoto 57:3fbd487e055e 313 #ifdef IM920
DeguNaoto 52:d9e1629da852 314 manualMoveIM920(); /*analogStick*/
DeguNaoto 52:d9e1629da852 315 manualIM920(); /*IM920 button*/
DeguNaoto 57:3fbd487e055e 316 #else
DeguNaoto 57:3fbd487e055e 317 manualMovePS3(); /*analogStick*/
DeguNaoto 57:3fbd487e055e 318 manualPS3(); /*PS3 button*/
DeguNaoto 57:3fbd487e055e 319 #endif
DeguNaoto 48:64d005c70df2 320 //Swing
unicore32 91:83c2e0381384 321 swingFollowing();
DeguNaoto 89:048a72382b0a 322 flaga=0;
DeguNaoto 83:e1638c58e1f1 323 #ifdef MESURE
DeguNaoto 83:e1638c58e1f1 324 if(down){
DeguNaoto 50:8ea4714316ce 325 IndicatorAuto=0;
DeguNaoto 50:8ea4714316ce 326 fclose(fp_r);
DeguNaoto 50:8ea4714316ce 327 }
DeguNaoto 50:8ea4714316ce 328 fprintf(fp_r, "time:%1.3f, %f[rad/s], pwm:%f\r\n",time,swingRadVelocity,cont);
DeguNaoto 83:e1638c58e1f1 329 time+=0.01;
DeguNaoto 83:e1638c58e1f1 330 #endif
DeguNaoto 4:51d87d2b698c 331 }
DeguNaoto 55:db1797ac6cb1 332 /***update state***/
DeguNaoto 57:3fbd487e055e 333 #ifdef IM920
DeguNaoto 51:cb430192b28b 334 readIM920();
DeguNaoto 57:3fbd487e055e 335 #endif
DeguNaoto 48:64d005c70df2 336 mesureSwing();
DeguNaoto 0:bd4719e15f7e 337 wait(RATE);
DeguNaoto 55:db1797ac6cb1 338
DeguNaoto 56:ac01d6b46291 339 // 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 340 // pc.printf("A2 = %d, X = %d, Y = %d, B = %d, dead = %d\r\n", a2, X, Y, b, deadflag);
DeguNaoto 57:3fbd487e055e 341 // pc.printf("%f %f\r\n",cont,swingRadVelocity);
DeguNaoto 0:bd4719e15f7e 342 }
DeguNaoto 0:bd4719e15f7e 343 }