CatPot 2015-2016
/
CatPot_Main_F
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: AQM0802A HMC6352 L6470_lib PID mbed
Fork of
CatPot_Main_ver1
by 
CatPot 2015-2016
main.cpp@7:7a0aee1477d9, 2015-01-10 (annotated)
- Committer:
- ryuna
- Date:
- Sat Jan 10 13:40:02 2015 +0000
- Revision:
- 7:7a0aee1477d9
- Parent:
- 6:c2c31bc971ad
??????????????????????????; wraparound.cpp??????????????
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| ryuna | 0:ae08e2e1d82d | 1 | /*********************************** |
| ryuna | 0:ae08e2e1d82d | 2 | *RoboCupJunior Soccer B Open 2014 |
| ryuna | 0:ae08e2e1d82d | 3 | *Koshinestu progrum |
| ryuna | 0:ae08e2e1d82d | 4 | * |
| ryuna | 0:ae08e2e1d82d | 5 | *このプログラムでは以下の処理をする． |
| ryuna | 0:ae08e2e1d82d | 6 | * LPC1114FN28/102からI2Cを用いて各種センサデータを収集 |
| ryuna | 0:ae08e2e1d82d | 7 | * |
| ryuna | 0:ae08e2e1d82d | 8 | * データからロボットの移動やキッカー等のモータの動作を決定する処理を行う |
| ryuna | 0:ae08e2e1d82d | 9 | * |
| ryuna | 1:7d4921b5d638 | 10 | * MotorDriverにmaxonに命令 |
| ryuna | 0:ae08e2e1d82d | 11 | * |
| ryuna | 0:ae08e2e1d82d | 12 | * SteppingMotorDriverにステアリング命令 |
| ryuna | 0:ae08e2e1d82d | 13 | * |
| ryuna | 0:ae08e2e1d82d | 14 | * LCDでデバック |
| ryuna | 0:ae08e2e1d82d | 15 | * |
| ryuna | 0:ae08e2e1d82d | 16 | * スイッチ4つとスタートスイッチで処理を実行 |
| ryuna | 1:7d4921b5d638 | 17 | * |
| ryuna | 6:c2c31bc971ad | 18 | * キッカー用のDigitalOutがどこかに入る |
| ryuna | 0:ae08e2e1d82d | 19 | * |
| ryuna | 0:ae08e2e1d82d | 20 | ************************* |
| ryuna | 0:ae08e2e1d82d | 21 | * |
| ryuna | 0:ae08e2e1d82d | 22 | *Pin Map |
| ryuna | 0:ae08e2e1d82d | 23 | * |
| ryuna | 1:7d4921b5d638 | 24 | * p5,p6,p7,p29,p30 >> SPI >>Stepping |
| ryuna | 0:ae08e2e1d82d | 25 | * |
| ryuna | 0:ae08e2e1d82d | 26 | * p9,p10 >> I2C orSerial >> LPC1114FN28/102 read |
| ryuna | 0:ae08e2e1d82d | 27 | * |
| ryuna | 0:ae08e2e1d82d | 28 | * p13,p14 >> Serial >> Motor |
| ryuna | 0:ae08e2e1d82d | 29 | * |
| ryuna | 0:ae08e2e1d82d | 30 | * p22~p26 >> DebugSw and StartSw |
| ryuna | 0:ae08e2e1d82d | 31 | * |
| ryuna | 0:ae08e2e1d82d | 32 | * p27,p28 >> I2C >> DebugLCD |
| ryuna | 0:ae08e2e1d82d | 33 | * |
| ryuna | 0:ae08e2e1d82d | 34 | * |
| ryuna | 0:ae08e2e1d82d | 35 | * |
| ryuna | 0:ae08e2e1d82d | 36 | ******************************/ |
| ryuna | 0:ae08e2e1d82d | 37 | |
| ryuna | 2:054444aa1990 | 38 | |
| ryuna | 2:054444aa1990 | 39 | /* |
| ryuna | 2:054444aa1990 | 40 | |
| ryuna | 2:054444aa1990 | 41 | ****以下IRはNighは距離が近いものとする． |
| ryuna | 2:054444aa1990 | 42 | |
| ryuna | 2:054444aa1990 | 43 | */ |
| ryuna | 2:054444aa1990 | 44 | |
| ryuna | 2:054444aa1990 | 45 | |
| ryuna | 2:054444aa1990 | 46 | |
| ryuna | 2:054444aa1990 | 47 | |
| ryuna | 2:054444aa1990 | 48 | |
| ryuna | 2:054444aa1990 | 49 | |
| ryuna | 0:ae08e2e1d82d | 50 | #include "mbed.h" |
| ryuna | 0:ae08e2e1d82d | 51 | #include "L6470.h" |
| ryuna | 0:ae08e2e1d82d | 52 | #include "PID.h" |
| ryuna | 0:ae08e2e1d82d | 53 | #include "AQM0802A.h" |
| ryuna | 3:8d718ddb84c9 | 54 | #include "HMC6352.h" |
| ryuna | 0:ae08e2e1d82d | 55 | #include <math.h> |
| ryuna | 0:ae08e2e1d82d | 56 | #include <sstream> |
| ryuna | 0:ae08e2e1d82d | 57 | |
| ryuna | 4:8444360f08e2 | 58 | /*回り込みの計算用*/ |
| ryuna | 4:8444360f08e2 | 59 | #define PI 3.141593/*割と早めにきってある*/ |
| ryuna | 4:8444360f08e2 | 60 | #define SHORT_LEN 15 /*cm換算 楕円のB辺の長さを定義しておく*/ |
| ryuna | 4:8444360f08e2 | 61 | |
| ryuna | 2:054444aa1990 | 62 | #define ADDRESS_R 0xA0 |
| ryuna | 2:054444aa1990 | 63 | #define ADDRESS_L 0xC0 |
| ryuna | 2:054444aa1990 | 64 | |
| ryuna | 4:8444360f08e2 | 65 | /*BusIn sw 入力値*/ |
| ryuna | 4:8444360f08e2 | 66 | #define Calibration 0x01 |
| ryuna | 4:8444360f08e2 | 67 | #define Kicker 0x02 |
| ryuna | 4:8444360f08e2 | 68 | #define Debug1 0x04 |
| ryuna | 4:8444360f08e2 | 69 | #define Debug2 0x08 |
| ryuna | 4:8444360f08e2 | 70 | #define StartS 0x10 |
| ryuna | 4:8444360f08e2 | 71 | |
| ryuna | 5:3d68334aab20 | 72 | #define READ_IR 0x01 //送る物指定 |
| ryuna | 5:3d68334aab20 | 73 | #define READ_PING 0x02 |
| ryuna | 5:3d68334aab20 | 74 | |
| ryuna | 3:8d718ddb84c9 | 75 | |
| ryuna | 2:054444aa1990 | 76 | |
| ryuna | 4:8444360f08e2 | 77 | |
| ryuna | 4:8444360f08e2 | 78 | BusIn Sw(p22,p23,p24,p25,p26); |
| ryuna | 1:7d4921b5d638 | 79 | DigitalOut Led[4] = {LED1,LED2,LED3,LED4}; |
| ryuna | 0:ae08e2e1d82d | 80 | |
| ryuna | 1:7d4921b5d638 | 81 | I2C Sensor(p9,p10);//sda,scl |
| ryuna | 3:8d718ddb84c9 | 82 | HMC6352 hmc6352(p9,p10); |
| ryuna | 1:7d4921b5d638 | 83 | AQM0802A Lcd(p28,p27);//sda,scl |
| ryuna | 1:7d4921b5d638 | 84 | L6470 Step(p5,p6,p7,p29,p30);//mosi,miso,sck,#cs,busy(PullUp) |
| ryuna | 1:7d4921b5d638 | 85 | Serial Motor(p13,p14);//tx,rx |
| ryuna | 1:7d4921b5d638 | 86 | Serial pc(USBTX,USBRX); |
| ryuna | 0:ae08e2e1d82d | 87 | |
| ryuna | 3:8d718ddb84c9 | 88 | DigitalOut Kick(p21); |
| ryuna | 0:ae08e2e1d82d | 89 | |
| ryuna | 0:ae08e2e1d82d | 90 | extern string StringFIN; |
| ryuna | 0:ae08e2e1d82d | 91 | extern void array(int,int,int,int); |
| ryuna | 0:ae08e2e1d82d | 92 | |
| ryuna | 0:ae08e2e1d82d | 93 | int speed[4] = {0}; |
| ryuna | 0:ae08e2e1d82d | 94 | |
| ryuna | 3:8d718ddb84c9 | 95 | typedef enum {FRONT = 1, RIGHT, BACK, LEFT, NA} Direction; |
| ryuna | 0:ae08e2e1d82d | 96 | |
| ryuna | 3:8d718ddb84c9 | 97 | |
| ryuna | 4:8444360f08e2 | 98 | unsigned int IrReceiveR(){ |
| ryuna | 3:8d718ddb84c9 | 99 | /*順位解析版 |
| ryuna | 2:054444aa1990 | 100 | Slave側はIRを要求した場合IRのみを返してくるとする． |
| ryuna | 2:054444aa1990 | 101 | irは値が大きいほうが近いと仮定する |
| ryuna | 2:054444aa1990 | 102 | |
| ryuna | 2:054444aa1990 | 103 | |
| ryuna | 2:054444aa1990 | 104 | */ |
| ryuna | 3:8d718ddb84c9 | 105 | char data_r[3],data_l[3]; |
| ryuna | 2:054444aa1990 | 106 | bool val; |
| ryuna | 2:054444aa1990 | 107 | Sensor.write(READ_IR);//一斉送信のつもり//コンパスへの影響はどのくらいですか |
| ryuna | 2:054444aa1990 | 108 | |
| ryuna | 3:8d718ddb84c9 | 109 | val = Sensor.read(ADDRESS_R|1, data_r, 3);// IRデータを受信 |
| ryuna | 2:054444aa1990 | 110 | Led[0] = val; |
| ryuna | 2:054444aa1990 | 111 | wait_ms(5); |
| ryuna | 3:8d718ddb84c9 | 112 | val = Sensor.read(ADDRESS_L|1, data_l, 3); |
| ryuna | 2:054444aa1990 | 113 | Led[0] = !val; |
| ryuna | 2:054444aa1990 | 114 | |
| ryuna | 6:c2c31bc971ad | 115 | if((data_r[0] == 0)||(data_l[0] == 0)){/*ボールを検知しているかチェック*/ |
| ryuna | 6:c2c31bc971ad | 116 | if((data_r[0] == 0)&&(data_l[0] == 0)){ |
| ryuna | 6:c2c31bc971ad | 117 | return 12; |
| ryuna | 6:c2c31bc971ad | 118 | } |
| ryuna | 6:c2c31bc971ad | 119 | if(data_r[0] == 0){ |
| ryuna | 6:c2c31bc971ad | 120 | return (data_l[0]/6+6)*12*12 + (data_l[0]%6+6)*12 + data_r[0]/6; |
| ryuna | 6:c2c31bc971ad | 121 | } |
| ryuna | 6:c2c31bc971ad | 122 | return data_r[0]/6*12*12 + (data_r[0]%6)*12 + data_l[0]/6+6; |
| ryuna | 6:c2c31bc971ad | 123 | |
| ryuna | 6:c2c31bc971ad | 124 | } |
| ryuna | 6:c2c31bc971ad | 125 | |
| ryuna | 2:054444aa1990 | 126 | if(data_r[2]>data_l[2]){ |
| ryuna | 2:054444aa1990 | 127 | if(data_r[2]>data_l[1]){ |
| ryuna | 3:8d718ddb84c9 | 128 | return data_r[0]/6*12*12 + (data_r[0]%6)*12 + data_l[0]/6+6; |
| ryuna | 2:054444aa1990 | 129 | } |
| ryuna | 6:c2c31bc971ad | 130 | |
| ryuna | 6:c2c31bc971ad | 131 | return data_r[0]/6*12*12 +(data_l[0]/6+6)*12+ data_r[0]%6; |
| ryuna | 6:c2c31bc971ad | 132 | |
| ryuna | 2:054444aa1990 | 133 | }else{ |
| ryuna | 2:054444aa1990 | 134 | if(data_l[2]>data_r[1]){ |
| ryuna | 3:8d718ddb84c9 | 135 | return (data_l[0]/6+6)*12*12 + (data_l[0]%6+6)*12 + data_r[0]/6; |
| ryuna | 2:054444aa1990 | 136 | } |
| ryuna | 2:054444aa1990 | 137 | |
| ryuna | 6:c2c31bc971ad | 138 | return (data_l[0]/6+6)*12*12 +(data_r[0]/6)*12+ (data_l[0]%6+6); |
| ryuna | 2:054444aa1990 | 139 | } |
| ryuna | 2:054444aa1990 | 140 | |
| ryuna | 2:054444aa1990 | 141 | } |
| ryuna | 2:054444aa1990 | 142 | |
| ryuna | 3:8d718ddb84c9 | 143 | uint8_t IrReceiveS(){ |
| ryuna | 3:8d718ddb84c9 | 144 | /*単純解析版 一番大きい位置だけ返す |
| ryuna | 3:8d718ddb84c9 | 145 | |
| ryuna | 3:8d718ddb84c9 | 146 | Slave側はIRを要求した場合IRのみを返してくるとする． |
| ryuna | 3:8d718ddb84c9 | 147 | irは値が大きいほうが近いと仮定する |
| ryuna | 3:8d718ddb84c9 | 148 | |
| ryuna | 6:c2c31bc971ad | 149 | */ |
| ryuna | 6:c2c31bc971ad | 150 | char data_r[3],data_l[3]; |
| ryuna | 6:c2c31bc971ad | 151 | bool val; |
| ryuna | 6:c2c31bc971ad | 152 | Sensor.write(READ_IR);//一斉送信のつもり//コンパスへの影響はどのくらいですか |
| ryuna | 6:c2c31bc971ad | 153 | |
| ryuna | 6:c2c31bc971ad | 154 | val = Sensor.read(ADDRESS_R|1, data_r, 3);// IRデータを受信 |
| ryuna | 6:c2c31bc971ad | 155 | Led[0] = val; |
| ryuna | 6:c2c31bc971ad | 156 | wait_ms(5); |
| ryuna | 6:c2c31bc971ad | 157 | val = Sensor.read(ADDRESS_L|1, data_l, 3); |
| ryuna | 6:c2c31bc971ad | 158 | Led[0] = !val; |
| ryuna | 6:c2c31bc971ad | 159 | |
| ryuna | 6:c2c31bc971ad | 160 | if((data_r[0] == 0)||(data_l[0] == 0)){/*ボールを検知しているかチェック*/ |
| ryuna | 6:c2c31bc971ad | 161 | if((data_r[0] == 0)&&(data_l[0] == 0)){ |
| ryuna | 6:c2c31bc971ad | 162 | return 12; |
| ryuna | 6:c2c31bc971ad | 163 | } |
| ryuna | 6:c2c31bc971ad | 164 | if(data_r[0] == 0){ |
| ryuna | 6:c2c31bc971ad | 165 | return data_l[0]/6+6; |
| ryuna | 6:c2c31bc971ad | 166 | } |
| ryuna | 6:c2c31bc971ad | 167 | return data_r[0]/6; |
| ryuna | 6:c2c31bc971ad | 168 | } |
| ryuna | 6:c2c31bc971ad | 169 | |
| ryuna | 6:c2c31bc971ad | 170 | if(data_r[2]>data_l[2]){ |
| ryuna | 6:c2c31bc971ad | 171 | return data_r[0]/6; |
| ryuna | 6:c2c31bc971ad | 172 | } |
| ryuna | 6:c2c31bc971ad | 173 | return data_l[0]/6+6; |
| ryuna | 6:c2c31bc971ad | 174 | |
| ryuna | 6:c2c31bc971ad | 175 | } |
| ryuna | 6:c2c31bc971ad | 176 | uint8_t IrReceiveM(){ |
| ryuna | 6:c2c31bc971ad | 177 | /*値解析版 一番近い場所の値を返す（位置はとりあえずない） |
| ryuna | 6:c2c31bc971ad | 178 | Slave側はIRを要求した場合IRのみを返してくるとする． |
| ryuna | 6:c2c31bc971ad | 179 | irは値が大きいほうが近いと仮定する |
| ryuna | 6:c2c31bc971ad | 180 | |
| ryuna | 3:8d718ddb84c9 | 181 | |
| ryuna | 3:8d718ddb84c9 | 182 | */ |
| ryuna | 3:8d718ddb84c9 | 183 | char data_r[3],data_l[3]; |
| ryuna | 3:8d718ddb84c9 | 184 | bool val; |
| ryuna | 3:8d718ddb84c9 | 185 | Sensor.write(READ_IR);//一斉送信のつもり//コンパスへの影響はどのくらいですか |
| ryuna | 3:8d718ddb84c9 | 186 | |
| ryuna | 3:8d718ddb84c9 | 187 | val = Sensor.read(ADDRESS_R|1, data_r, 3);// IRデータを受信 |
| ryuna | 3:8d718ddb84c9 | 188 | Led[0] = val; |
| ryuna | 3:8d718ddb84c9 | 189 | wait_ms(5); |
| ryuna | 3:8d718ddb84c9 | 190 | val = Sensor.read(ADDRESS_L|1, data_l, 3); |
| ryuna | 3:8d718ddb84c9 | 191 | Led[0] = !val; |
| ryuna | 3:8d718ddb84c9 | 192 | |
| ryuna | 6:c2c31bc971ad | 193 | if((data_r[0] == 0)||(data_l[0] == 0)){/*ボールを検知しているかチェック*/ |
| ryuna | 6:c2c31bc971ad | 194 | if((data_r[0] == 0)&&(data_l[0] == 0)){ |
| ryuna | 6:c2c31bc971ad | 195 | return 255; |
| ryuna | 6:c2c31bc971ad | 196 | } |
| ryuna | 6:c2c31bc971ad | 197 | if(data_r[0] == 0){ |
| ryuna | 6:c2c31bc971ad | 198 | return data_l[1]; |
| ryuna | 6:c2c31bc971ad | 199 | } |
| ryuna | 6:c2c31bc971ad | 200 | return data_r[1]; |
| ryuna | 3:8d718ddb84c9 | 201 | } |
| ryuna | 3:8d718ddb84c9 | 202 | |
| ryuna | 3:8d718ddb84c9 | 203 | |
| ryuna | 3:8d718ddb84c9 | 204 | if(data_r[2]>data_l[2]){ |
| ryuna | 3:8d718ddb84c9 | 205 | return data_r[1]; |
| ryuna | 3:8d718ddb84c9 | 206 | |
| ryuna | 3:8d718ddb84c9 | 207 | } |
| ryuna | 3:8d718ddb84c9 | 208 | |
| ryuna | 7:7a0aee1477d9 | 209 | return data_l[1]; |
| ryuna | 7:7a0aee1477d9 | 210 | |
| ryuna | 3:8d718ddb84c9 | 211 | } |
| ryuna | 3:8d718ddb84c9 | 212 | unsigned int IrReceiveSM(){ |
| ryuna | 3:8d718ddb84c9 | 213 | /*値解析版 一番大きい値とその位置を返す |
| ryuna | 3:8d718ddb84c9 | 214 | Slave側はIRを要求した場合IRのみを返してくるとする． |
| ryuna | 3:8d718ddb84c9 | 215 | irは値が大きいほうが近いと仮定する |
| ryuna | 3:8d718ddb84c9 | 216 | |
| ryuna | 3:8d718ddb84c9 | 217 | |
| ryuna | 3:8d718ddb84c9 | 218 | */ |
| ryuna | 3:8d718ddb84c9 | 219 | char data_r[3],data_l[3]; |
| ryuna | 3:8d718ddb84c9 | 220 | bool val; |
| ryuna | 3:8d718ddb84c9 | 221 | Sensor.write(READ_IR);//一斉送信のつもり//コンパスへの影響はどのくらいですか |
| ryuna | 3:8d718ddb84c9 | 222 | |
| ryuna | 3:8d718ddb84c9 | 223 | val = Sensor.read(ADDRESS_R|1, data_r, 3);// IRデータを受信 |
| ryuna | 3:8d718ddb84c9 | 224 | Led[0] = val; |
| ryuna | 3:8d718ddb84c9 | 225 | wait_ms(5); |
| ryuna | 3:8d718ddb84c9 | 226 | val = Sensor.read(ADDRESS_L|1, data_l, 3); |
| ryuna | 3:8d718ddb84c9 | 227 | Led[0] = !val; |
| ryuna | 6:c2c31bc971ad | 228 | |
| ryuna | 6:c2c31bc971ad | 229 | if((data_r[0] == 0)||(data_l[0] == 0)){/*ボールを検知しているかチェック*/ |
| ryuna | 6:c2c31bc971ad | 230 | if((data_r[0] == 0)&&(data_l[0] == 0)){ |
| ryuna | 6:c2c31bc971ad | 231 | return 12<<8+255; |
| ryuna | 6:c2c31bc971ad | 232 | } |
| ryuna | 6:c2c31bc971ad | 233 | if(data_r[0] == 0){ |
| ryuna | 6:c2c31bc971ad | 234 | return (data_l[0]/6+6)<<8 + data_l[1]; |
| ryuna | 6:c2c31bc971ad | 235 | } |
| ryuna | 6:c2c31bc971ad | 236 | return data_r[0]/6<<8 + data_r[1]; |
| ryuna | 6:c2c31bc971ad | 237 | } |
| ryuna | 6:c2c31bc971ad | 238 | |
| ryuna | 6:c2c31bc971ad | 239 | |
| ryuna | 3:8d718ddb84c9 | 240 | if(data_r[2]>data_l[2]){ |
| ryuna | 3:8d718ddb84c9 | 241 | return data_r[0]/6<<8 + data_r[1]; |
| ryuna | 3:8d718ddb84c9 | 242 | |
| ryuna | 3:8d718ddb84c9 | 243 | } |
| ryuna | 6:c2c31bc971ad | 244 | return ((data_l[0]/6)+6) <<8 + data_l[1]; |
| ryuna | 6:c2c31bc971ad | 245 | |
| ryuna | 3:8d718ddb84c9 | 246 | |
| ryuna | 3:8d718ddb84c9 | 247 | } |
| ryuna | 3:8d718ddb84c9 | 248 | uint8_t LineReceive(){ |
| ryuna | 3:8d718ddb84c9 | 249 | //先に要求しない場合はLineの状況を送信すること．//4bit //前ー右ー後ろー左 |
| ryuna | 2:054444aa1990 | 250 | char data_r[2],data_l[2]; |
| ryuna | 2:054444aa1990 | 251 | bool val; |
| ryuna | 3:8d718ddb84c9 | 252 | //example val = slave.read(address,data,length,repeat); |
| ryuna | 2:054444aa1990 | 253 | val = Sensor.read(ADDRESS_R|1, data_r, 1); |
| ryuna | 2:054444aa1990 | 254 | Led[1] = val; |
| ryuna | 2:054444aa1990 | 255 | wait_ms(5); |
| ryuna | 2:054444aa1990 | 256 | val = Sensor.read(ADDRESS_L|1, data_l, 1); |
| ryuna | 2:054444aa1990 | 257 | Led[1] = !val; |
| ryuna | 2:054444aa1990 | 258 | |
| ryuna | 3:8d718ddb84c9 | 259 | |
| ryuna | 3:8d718ddb84c9 | 260 | return data_r[0]<<2 + data_l[0]; |
| ryuna | 2:054444aa1990 | 261 | |
| ryuna | 2:054444aa1990 | 262 | |
| ryuna | 2:054444aa1990 | 263 | } |
| ryuna | 3:8d718ddb84c9 | 264 | |
| ryuna | 3:8d718ddb84c9 | 265 | |
| ryuna | 3:8d718ddb84c9 | 266 | void PingReceiveRL(char ping[]){//ping[0]==Right,ping[1]==Left |
| ryuna | 5:3d68334aab20 | 267 | char ReadSelect[1] = {READ_PING}; |
| ryuna | 3:8d718ddb84c9 | 268 | bool val; |
| ryuna | 5:3d68334aab20 | 269 | val = Sensor.write(ADDRESS_R|0, ReadSelect , 1); |
| ryuna | 3:8d718ddb84c9 | 270 | Led[2] = val; |
| ryuna | 3:8d718ddb84c9 | 271 | val = Sensor.read(ADDRESS_R|1, ping, 2); |
| ryuna | 3:8d718ddb84c9 | 272 | Led[2] = !val; |
| ryuna | 3:8d718ddb84c9 | 273 | |
| ryuna | 3:8d718ddb84c9 | 274 | |
| ryuna | 3:8d718ddb84c9 | 275 | } |
| ryuna | 5:3d68334aab20 | 276 | |
| ryuna | 7:7a0aee1477d9 | 277 | void PingReceiveFB(char ping[]){//ping[0]==FRONT,ping[1]==BACK |
| ryuna | 7:7a0aee1477d9 | 278 | char ReadSelect[1] = {READ_PING}; |
| ryuna | 7:7a0aee1477d9 | 279 | bool val; |
| ryuna | 7:7a0aee1477d9 | 280 | val = Sensor.write(ADDRESS_L|0, ReadSelect , 1); |
| ryuna | 7:7a0aee1477d9 | 281 | Led[2] = val; |
| ryuna | 7:7a0aee1477d9 | 282 | val = Sensor.read(ADDRESS_L|1, ping, 2); |
| ryuna | 7:7a0aee1477d9 | 283 | Led[2] = !val; |
| ryuna | 7:7a0aee1477d9 | 284 | |
| ryuna | 7:7a0aee1477d9 | 285 | |
| ryuna | 7:7a0aee1477d9 | 286 | } |
| ryuna | 5:3d68334aab20 | 287 | |
| ryuna | 5:3d68334aab20 | 288 | |
| ryuna | 2:054444aa1990 | 289 | void rotate(unsigned int times, bool dir){ |
| ryuna | 2:054444aa1990 | 290 | /* |
| ryuna | 2:054444aa1990 | 291 | *回転速度，回転方向，回転角度等設定変更ののち回転． |
| ryuna | 5:3d68334aab20 | 292 | * this is 使わなそうである、 |
| ryuna | 2:054444aa1990 | 293 | */ |
| ryuna | 3:8d718ddb84c9 | 294 | |
| ryuna | 3:8d718ddb84c9 | 295 | |
| ryuna | 3:8d718ddb84c9 | 296 | |
| ryuna | 3:8d718ddb84c9 | 297 | |
| ryuna | 2:054444aa1990 | 298 | |
| ryuna | 2:054444aa1990 | 299 | } |
| ryuna | 2:054444aa1990 | 300 | |
| ryuna | 2:054444aa1990 | 301 | void move(int vr, int vl){ |
| ryuna | 0:ae08e2e1d82d | 302 | double pwm[4] = {0}; |
| ryuna | 0:ae08e2e1d82d | 303 | uint8_t i = 0; |
| ryuna | 2:054444aa1990 | 304 | pwm[0] = vr/10.0; |
| ryuna | 2:054444aa1990 | 305 | pwm[1] = vl/10.0; |
| ryuna | 2:054444aa1990 | 306 | pwm[2] = 0; |
| ryuna | 2:054444aa1990 | 307 | pwm[3] = 0; |
| ryuna | 0:ae08e2e1d82d | 308 | |
| ryuna | 0:ae08e2e1d82d | 309 | for(i = 0; i < 4; i++){ |
| ryuna | 0:ae08e2e1d82d | 310 | if(pwm[i] > 100){ |
| ryuna | 0:ae08e2e1d82d | 311 | pwm[i] = 100; |
| ryuna | 0:ae08e2e1d82d | 312 | }else if(pwm[i] < -100){ |
| ryuna | 0:ae08e2e1d82d | 313 | pwm[i] = -100; |
| ryuna | 0:ae08e2e1d82d | 314 | } |
| ryuna | 0:ae08e2e1d82d | 315 | speed[i] = pwm[i]; |
| ryuna | 0:ae08e2e1d82d | 316 | } |
| ryuna | 0:ae08e2e1d82d | 317 | } |
| ryuna | 0:ae08e2e1d82d | 318 | |
| ryuna | 3:8d718ddb84c9 | 319 | void ControlRobo0(int *CompassDef)//LeftFront |
| ryuna | 3:8d718ddb84c9 | 320 | {} |
| ryuna | 3:8d718ddb84c9 | 321 | void ControlRobo1(int *CompassDef)//Front |
| ryuna | 3:8d718ddb84c9 | 322 | { |
| ryuna | 3:8d718ddb84c9 | 323 | /*前にボールがある場合の動作*/ |
| ryuna | 3:8d718ddb84c9 | 324 | int Compass; |
| ryuna | 3:8d718ddb84c9 | 325 | char Ping[2]; |
| ryuna | 3:8d718ddb84c9 | 326 | uint8_t Line,LineIr; |
| ryuna | 3:8d718ddb84c9 | 327 | unsigned int IrNumMax; |
| ryuna | 3:8d718ddb84c9 | 328 | |
| ryuna | 3:8d718ddb84c9 | 329 | Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360; |
| ryuna | 3:8d718ddb84c9 | 330 | if(!((Compass>=150)&&(Compass<=210))){ |
| ryuna | 3:8d718ddb84c9 | 331 | while(!((Compass>=150)&&(Compass<=210))){ |
| ryuna | 3:8d718ddb84c9 | 332 | move(20,-20);//適当な回転。 |
| ryuna | 3:8d718ddb84c9 | 333 | Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360; |
| ryuna | 3:8d718ddb84c9 | 334 | } |
| ryuna | 3:8d718ddb84c9 | 335 | return; |
| ryuna | 3:8d718ddb84c9 | 336 | } |
| ryuna | 3:8d718ddb84c9 | 337 | if(IrReceiveM()<=150){//適当な150 |
| ryuna | 3:8d718ddb84c9 | 338 | /*ステッピングを正面に設定*/ |
| ryuna | 3:8d718ddb84c9 | 339 | /*busy_wait()*/ |
| ryuna | 3:8d718ddb84c9 | 340 | /*モーターを前進*/ |
| ryuna | 3:8d718ddb84c9 | 341 | return; |
| ryuna | 3:8d718ddb84c9 | 342 | } |
| ryuna | 3:8d718ddb84c9 | 343 | PingReceiveRL(Ping); |
| ryuna | 3:8d718ddb84c9 | 344 | if((Ping[0]>90)&&(Ping[1]>40)){//数値は適当 |
| ryuna | 3:8d718ddb84c9 | 345 | /*ステッピングを少しだけ傾ける*/ |
| ryuna | 3:8d718ddb84c9 | 346 | /*モーターを左右差つけて回す(ロボットは少し傾く)*/ |
| ryuna | 3:8d718ddb84c9 | 347 | /*busy_wait()*/ |
| ryuna | 3:8d718ddb84c9 | 348 | /*モーターを前進*/ |
| ryuna | 3:8d718ddb84c9 | 349 | while(1){ |
| ryuna | 3:8d718ddb84c9 | 350 | Line = LineReceive(); |
| ryuna | 3:8d718ddb84c9 | 351 | |
| ryuna | 3:8d718ddb84c9 | 352 | if(Line){ //ラインを検知していないか |
| ryuna | 3:8d718ddb84c9 | 353 | LineIr = Line & (IrReceiveS()/3 + 1);//計算により方位を合わせる。 |
| ryuna | 3:8d718ddb84c9 | 354 | while(LineIr){ |
| ryuna | 3:8d718ddb84c9 | 355 | move(0,0);// |
| ryuna | 3:8d718ddb84c9 | 356 | Led[1] = Led[2] = Led[3] = 1; |
| ryuna | 3:8d718ddb84c9 | 357 | |
| ryuna | 3:8d718ddb84c9 | 358 | LineIr = LineReceive() & (IrReceiveS()/3 + 1); |
| ryuna | 3:8d718ddb84c9 | 359 | } |
| ryuna | 3:8d718ddb84c9 | 360 | Led[1] = Led[2] = Led[3] = 0; |
| ryuna | 3:8d718ddb84c9 | 361 | |
| ryuna | 3:8d718ddb84c9 | 362 | Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360; |
| ryuna | 3:8d718ddb84c9 | 363 | while(!((Compass>=150)&&(Compass<=210))){ |
| ryuna | 3:8d718ddb84c9 | 364 | move(20,-20);//適当な回転。 |
| ryuna | 3:8d718ddb84c9 | 365 | Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360; |
| ryuna | 3:8d718ddb84c9 | 366 | } |
| ryuna | 3:8d718ddb84c9 | 367 | return; |
| ryuna | 3:8d718ddb84c9 | 368 | } |
| ryuna | 3:8d718ddb84c9 | 369 | Kick = 1; |
| ryuna | 3:8d718ddb84c9 | 370 | wait_ms(200); |
| ryuna | 3:8d718ddb84c9 | 371 | Kick = 0; |
| ryuna | 3:8d718ddb84c9 | 372 | |
| ryuna | 3:8d718ddb84c9 | 373 | IrNumMax = IrReceiveSM();//位置と大きさ |
| ryuna | 3:8d718ddb84c9 | 374 | |
| ryuna | 3:8d718ddb84c9 | 375 | if(!((IrNumMax&0x00)>>8) == 1){//この1はボールの関数の1を表す. |
| ryuna | 3:8d718ddb84c9 | 376 | Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360; |
| ryuna | 3:8d718ddb84c9 | 377 | while(!((Compass>=150)&&(Compass<=210))){ |
| ryuna | 3:8d718ddb84c9 | 378 | move(20,-20);//適当な回転。 |
| ryuna | 3:8d718ddb84c9 | 379 | Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360; |
| ryuna | 3:8d718ddb84c9 | 380 | } |
| ryuna | 3:8d718ddb84c9 | 381 | return; |
| ryuna | 3:8d718ddb84c9 | 382 | } |
| ryuna | 3:8d718ddb84c9 | 383 | |
| ryuna | 3:8d718ddb84c9 | 384 | if((IrNumMax&0x00FF)<150){ |
| ryuna | 3:8d718ddb84c9 | 385 | Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360; |
| ryuna | 3:8d718ddb84c9 | 386 | while(!((Compass>=150)&&(Compass<=210))){ |
| ryuna | 3:8d718ddb84c9 | 387 | move(20,-20);//適当な回転。 |
| ryuna | 3:8d718ddb84c9 | 388 | Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360; |
| ryuna | 3:8d718ddb84c9 | 389 | } |
| ryuna | 3:8d718ddb84c9 | 390 | return; |
| ryuna | 3:8d718ddb84c9 | 391 | } |
| ryuna | 3:8d718ddb84c9 | 392 | |
| ryuna | 3:8d718ddb84c9 | 393 | } |
| ryuna | 3:8d718ddb84c9 | 394 | |
| ryuna | 3:8d718ddb84c9 | 395 | }else if((Ping[0]>40)&&(Ping[1]>90)){ |
| ryuna | 3:8d718ddb84c9 | 396 | |
| ryuna | 3:8d718ddb84c9 | 397 | }else{ |
| ryuna | 3:8d718ddb84c9 | 398 | |
| ryuna | 3:8d718ddb84c9 | 399 | } |
| ryuna | 3:8d718ddb84c9 | 400 | |
| ryuna | 3:8d718ddb84c9 | 401 | } |
| ryuna | 3:8d718ddb84c9 | 402 | void ControlRobo2(int *CompassDef)//RightFront |
| ryuna | 3:8d718ddb84c9 | 403 | { |
| ryuna | 3:8d718ddb84c9 | 404 | |
| ryuna | 7:7a0aee1477d9 | 405 | int Compass; |
| ryuna | 7:7a0aee1477d9 | 406 | char Ir[2] ={0};//1, 1,2位, 1位の大きさ |
| ryuna | 7:7a0aee1477d9 | 407 | char ir_num,line;//わかりやすい名前 |
| ryuna | 7:7a0aee1477d9 | 408 | |
| ryuna | 7:7a0aee1477d9 | 409 | Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360; |
| ryuna | 7:7a0aee1477d9 | 410 | if(!((Compass>=150)&&(Compass<=210))){ |
| ryuna | 7:7a0aee1477d9 | 411 | while(!((Compass>=150)&&(Compass<=210))){ |
| ryuna | 7:7a0aee1477d9 | 412 | move(20,-20);//適当な回転。 |
| ryuna | 7:7a0aee1477d9 | 413 | Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360; |
| ryuna | 7:7a0aee1477d9 | 414 | } |
| ryuna | 7:7a0aee1477d9 | 415 | move(0,0); |
| ryuna | 7:7a0aee1477d9 | 416 | return; |
| ryuna | 7:7a0aee1477d9 | 417 | } |
| ryuna | 7:7a0aee1477d9 | 418 | Step.GoHome(); |
| ryuna | 7:7a0aee1477d9 | 419 | /*irデータ取得 1,2位の場所、一位の距離*/ |
| ryuna | 7:7a0aee1477d9 | 420 | if(!(Ir[0]%13 ==2)){ |
| ryuna | 7:7a0aee1477d9 | 421 | return; |
| ryuna | 7:7a0aee1477d9 | 422 | } |
| ryuna | 7:7a0aee1477d9 | 423 | Step.BusyWait(0); |
| ryuna | 7:7a0aee1477d9 | 424 | |
| ryuna | 7:7a0aee1477d9 | 425 | if(Ir[1]>=100){//近い場合 |
| ryuna | 7:7a0aee1477d9 | 426 | //2位を含んだ計算 |
| ryuna | 7:7a0aee1477d9 | 427 | /*motorset*/ |
| ryuna | 7:7a0aee1477d9 | 428 | /*stepset*/ |
| ryuna | 7:7a0aee1477d9 | 429 | return; |
| ryuna | 7:7a0aee1477d9 | 430 | } |
| ryuna | 7:7a0aee1477d9 | 431 | |
| ryuna | 7:7a0aee1477d9 | 432 | /*2位を含んだ計算*/ |
| ryuna | 7:7a0aee1477d9 | 433 | /*計算された分stepmove*/ |
| ryuna | 7:7a0aee1477d9 | 434 | Step.BusyWait(0); |
| ryuna | 7:7a0aee1477d9 | 435 | move(20,20); |
| ryuna | 7:7a0aee1477d9 | 436 | |
| ryuna | 7:7a0aee1477d9 | 437 | |
| ryuna | 7:7a0aee1477d9 | 438 | /*line,ir get*/ |
| ryuna | 7:7a0aee1477d9 | 439 | do{ |
| ryuna | 7:7a0aee1477d9 | 440 | if(line){ |
| ryuna | 7:7a0aee1477d9 | 441 | move(0,0); |
| ryuna | 7:7a0aee1477d9 | 442 | return; |
| ryuna | 7:7a0aee1477d9 | 443 | } |
| ryuna | 7:7a0aee1477d9 | 444 | |
| ryuna | 7:7a0aee1477d9 | 445 | /*line,ir get*/ |
| ryuna | 7:7a0aee1477d9 | 446 | }while(ir_num == 2); |
| ryuna | 7:7a0aee1477d9 | 447 | |
| ryuna | 7:7a0aee1477d9 | 448 | move(0,0); |
| ryuna | 7:7a0aee1477d9 | 449 | |
| ryuna | 7:7a0aee1477d9 | 450 | |
| ryuna | 3:8d718ddb84c9 | 451 | |
| ryuna | 3:8d718ddb84c9 | 452 | } |
| ryuna | 3:8d718ddb84c9 | 453 | void ControlRobo3(int *CompassDef)//RightFront |
| ryuna | 7:7a0aee1477d9 | 454 | { |
| ryuna | 7:7a0aee1477d9 | 455 | int Compass; |
| ryuna | 7:7a0aee1477d9 | 456 | char Ir[2] ={0};//1, 1,2位, 1位の大きさ |
| ryuna | 7:7a0aee1477d9 | 457 | char ir_num,line;//わかりやすい名前 |
| ryuna | 7:7a0aee1477d9 | 458 | |
| ryuna | 7:7a0aee1477d9 | 459 | Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360; |
| ryuna | 7:7a0aee1477d9 | 460 | if(!((Compass>=150)&&(Compass<=210))){ |
| ryuna | 7:7a0aee1477d9 | 461 | while(!((Compass>=150)&&(Compass<=210))){ |
| ryuna | 7:7a0aee1477d9 | 462 | move(20,-20);//適当な回転。 |
| ryuna | 7:7a0aee1477d9 | 463 | Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360; |
| ryuna | 7:7a0aee1477d9 | 464 | } |
| ryuna | 7:7a0aee1477d9 | 465 | move(0,0); |
| ryuna | 7:7a0aee1477d9 | 466 | return; |
| ryuna | 7:7a0aee1477d9 | 467 | } |
| ryuna | 7:7a0aee1477d9 | 468 | Step.GoHome(); |
| ryuna | 7:7a0aee1477d9 | 469 | /*irデータ取得 1,2位の場所、一位の距離*/ |
| ryuna | 7:7a0aee1477d9 | 470 | if(!(Ir[0]%13 ==3)){ |
| ryuna | 7:7a0aee1477d9 | 471 | return; |
| ryuna | 7:7a0aee1477d9 | 472 | } |
| ryuna | 7:7a0aee1477d9 | 473 | Step.BusyWait(0); |
| ryuna | 7:7a0aee1477d9 | 474 | |
| ryuna | 7:7a0aee1477d9 | 475 | if(Ir[1] >= 100){ |
| ryuna | 7:7a0aee1477d9 | 476 | /*2位を含めた計算*/ |
| ryuna | 7:7a0aee1477d9 | 477 | /*move()*/ |
| ryuna | 7:7a0aee1477d9 | 478 | /*Stepset*/ |
| ryuna | 7:7a0aee1477d9 | 479 | return; |
| ryuna | 7:7a0aee1477d9 | 480 | } |
| ryuna | 7:7a0aee1477d9 | 481 | /*2位を含めた計算*/ |
| ryuna | 7:7a0aee1477d9 | 482 | /*Stepsetting 距離等により角度が変わる*/ |
| ryuna | 7:7a0aee1477d9 | 483 | Step.BusyWait(0); |
| ryuna | 7:7a0aee1477d9 | 484 | move(-20,-20); |
| ryuna | 7:7a0aee1477d9 | 485 | /*ir,line check*/ |
| ryuna | 7:7a0aee1477d9 | 486 | do{ |
| ryuna | 7:7a0aee1477d9 | 487 | if(line){ |
| ryuna | 7:7a0aee1477d9 | 488 | move(0,0); |
| ryuna | 7:7a0aee1477d9 | 489 | return; |
| ryuna | 7:7a0aee1477d9 | 490 | } |
| ryuna | 7:7a0aee1477d9 | 491 | /*line,ir check*/ |
| ryuna | 7:7a0aee1477d9 | 492 | }while(ir_num == 3); |
| ryuna | 7:7a0aee1477d9 | 493 | |
| ryuna | 7:7a0aee1477d9 | 494 | move(0,0); |
| ryuna | 7:7a0aee1477d9 | 495 | |
| ryuna | 7:7a0aee1477d9 | 496 | |
| ryuna | 7:7a0aee1477d9 | 497 | } |
| ryuna | 3:8d718ddb84c9 | 498 | void ControlRobo4(int *CompassDef)//Right |
| ryuna | 3:8d718ddb84c9 | 499 | { |
| ryuna | 4:8444360f08e2 | 500 | int a = 50; |
| ryuna | 4:8444360f08e2 | 501 | unsigned int ir_sm; |
| ryuna | 4:8444360f08e2 | 502 | int kakudo = -90; |
| ryuna | 4:8444360f08e2 | 503 | double value = 0; |
| ryuna | 4:8444360f08e2 | 504 | double v = 50;/* cm/s */ |
| ryuna | 4:8444360f08e2 | 505 | double x = 20;/*モーターを回す割合,今は適当*/ |
| ryuna | 3:8d718ddb84c9 | 506 | |
| ryuna | 4:8444360f08e2 | 507 | ir_sm = IrReceiveSM(); |
| ryuna | 4:8444360f08e2 | 508 | if(ir_sm&0xF00>>8 == 4){ |
| ryuna | 4:8444360f08e2 | 509 | return; |
| ryuna | 4:8444360f08e2 | 510 | } |
| ryuna | 4:8444360f08e2 | 511 | a = 300 - ir_sm&0x00FF ;//適当な計算//300は勘 |
| ryuna | 4:8444360f08e2 | 512 | value = 10 * a * SHORT_LEN + 3*(a * a + SHORT_LEN * SHORT_LEN); |
| ryuna | 4:8444360f08e2 | 513 | value = PI *(3 * ( a + SHORT_LEN) - sqrt(value))/4/v; |
| ryuna | 4:8444360f08e2 | 514 | |
| ryuna | 4:8444360f08e2 | 515 | if(300 - ir_sm&0x00FF >200){ |
| ryuna | 4:8444360f08e2 | 516 | Step.Step(-10);//適当 |
| ryuna | 4:8444360f08e2 | 517 | Step.BusyWait(0); |
| ryuna | 4:8444360f08e2 | 518 | } |
| ryuna | 4:8444360f08e2 | 519 | |
| ryuna | 4:8444360f08e2 | 520 | /*SetPerm() |
| ryuna | 4:8444360f08e2 | 521 | *valueに応じた何かを計算する |
| ryuna | 4:8444360f08e2 | 522 | *ACCまたはMAX_SPEEDをいじればいい気がする |
| ryuna | 4:8444360f08e2 | 523 | * |
| ryuna | 4:8444360f08e2 | 524 | */ |
| ryuna | 4:8444360f08e2 | 525 | Step.Step(kakudo+10); |
| ryuna | 4:8444360f08e2 | 526 | |
| ryuna | 4:8444360f08e2 | 527 | /*x = v*何か*/ |
| ryuna | 4:8444360f08e2 | 528 | move(-x,-x); |
| ryuna | 4:8444360f08e2 | 529 | |
| ryuna | 3:8d718ddb84c9 | 530 | |
| ryuna | 3:8d718ddb84c9 | 531 | } |
| ryuna | 3:8d718ddb84c9 | 532 | void ControlRobo5(int *CompassDef)//RightBack |
| ryuna | 7:7a0aee1477d9 | 533 | { |
| ryuna | 7:7a0aee1477d9 | 534 | int Compass; |
| ryuna | 7:7a0aee1477d9 | 535 | char Ir[2] ={0};//1, 1,2位, 1位の大きさ |
| ryuna | 7:7a0aee1477d9 | 536 | char ir_num,line;//わかりやすい名前 |
| ryuna | 7:7a0aee1477d9 | 537 | |
| ryuna | 7:7a0aee1477d9 | 538 | Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360; |
| ryuna | 7:7a0aee1477d9 | 539 | if(!((Compass>=150)&&(Compass<=210))){ |
| ryuna | 7:7a0aee1477d9 | 540 | while(!((Compass>=150)&&(Compass<=210))){ |
| ryuna | 7:7a0aee1477d9 | 541 | move(20,-20);//適当な回転。 |
| ryuna | 7:7a0aee1477d9 | 542 | Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360; |
| ryuna | 7:7a0aee1477d9 | 543 | } |
| ryuna | 7:7a0aee1477d9 | 544 | move(0,0); |
| ryuna | 7:7a0aee1477d9 | 545 | return; |
| ryuna | 7:7a0aee1477d9 | 546 | } |
| ryuna | 7:7a0aee1477d9 | 547 | Step.GoHome(); |
| ryuna | 7:7a0aee1477d9 | 548 | /*irデータ取得 1,2位の場所、一位の距離*/ |
| ryuna | 7:7a0aee1477d9 | 549 | if(!(Ir[0]%13 == 5)){ |
| ryuna | 7:7a0aee1477d9 | 550 | return; |
| ryuna | 7:7a0aee1477d9 | 551 | } |
| ryuna | 7:7a0aee1477d9 | 552 | Step.BusyWait(0); |
| ryuna | 7:7a0aee1477d9 | 553 | |
| ryuna | 7:7a0aee1477d9 | 554 | if(Ir[1] >=100){//近い |
| ryuna | 7:7a0aee1477d9 | 555 | /*計算*/ |
| ryuna | 7:7a0aee1477d9 | 556 | /*Step.Move()回転開始位置まで移動*/ |
| ryuna | 7:7a0aee1477d9 | 557 | Step.BusyWait(0); |
| ryuna | 7:7a0aee1477d9 | 558 | move(-20,-20); |
| ryuna | 7:7a0aee1477d9 | 559 | /*Step.setpaaa |
| ryuna | 7:7a0aee1477d9 | 560 | Step.move();-180+初期位置 |
| ryuna | 7:7a0aee1477d9 | 561 | */ |
| ryuna | 7:7a0aee1477d9 | 562 | /*ir,linecheck*/ |
| ryuna | 7:7a0aee1477d9 | 563 | do{ |
| ryuna | 7:7a0aee1477d9 | 564 | if(line){ |
| ryuna | 7:7a0aee1477d9 | 565 | move(0,0); |
| ryuna | 7:7a0aee1477d9 | 566 | return; |
| ryuna | 7:7a0aee1477d9 | 567 | } |
| ryuna | 7:7a0aee1477d9 | 568 | if(!(ir_num >1 && ir_num <=5)){ |
| ryuna | 7:7a0aee1477d9 | 569 | move(0,0); |
| ryuna | 7:7a0aee1477d9 | 570 | return; |
| ryuna | 7:7a0aee1477d9 | 571 | } |
| ryuna | 7:7a0aee1477d9 | 572 | /*ir,line check*/ |
| ryuna | 7:7a0aee1477d9 | 573 | }while(1); |
| ryuna | 7:7a0aee1477d9 | 574 | } |
| ryuna | 7:7a0aee1477d9 | 575 | //遠い |
| ryuna | 7:7a0aee1477d9 | 576 | |
| ryuna | 7:7a0aee1477d9 | 577 | /*計算*/ |
| ryuna | 7:7a0aee1477d9 | 578 | move(-20,-20); |
| ryuna | 7:7a0aee1477d9 | 579 | /*Step.setpaaa |
| ryuna | 7:7a0aee1477d9 | 580 | Step.move(); |
| ryuna | 7:7a0aee1477d9 | 581 | */ |
| ryuna | 7:7a0aee1477d9 | 582 | /*ir,linecheck*/ |
| ryuna | 7:7a0aee1477d9 | 583 | do{ |
| ryuna | 7:7a0aee1477d9 | 584 | if(line){ |
| ryuna | 7:7a0aee1477d9 | 585 | move(0,0); |
| ryuna | 7:7a0aee1477d9 | 586 | return; |
| ryuna | 7:7a0aee1477d9 | 587 | } |
| ryuna | 7:7a0aee1477d9 | 588 | if(!(ir_num >1 && ir_num <=5)){ |
| ryuna | 7:7a0aee1477d9 | 589 | move(0,0); |
| ryuna | 7:7a0aee1477d9 | 590 | return; |
| ryuna | 7:7a0aee1477d9 | 591 | } |
| ryuna | 7:7a0aee1477d9 | 592 | /*ir,line check*/ |
| ryuna | 7:7a0aee1477d9 | 593 | }while(1); |
| ryuna | 7:7a0aee1477d9 | 594 | |
| ryuna | 7:7a0aee1477d9 | 595 | |
| ryuna | 7:7a0aee1477d9 | 596 | } |
| ryuna | 3:8d718ddb84c9 | 597 | void ControlRobo6(int *CompassDef)//BackRight |
| ryuna | 7:7a0aee1477d9 | 598 | { |
| ryuna | 7:7a0aee1477d9 | 599 | |
| ryuna | 7:7a0aee1477d9 | 600 | |
| ryuna | 7:7a0aee1477d9 | 601 | } |
| ryuna | 3:8d718ddb84c9 | 602 | void ControlRobo7(int *CompassDef)//Back |
| ryuna | 5:3d68334aab20 | 603 | { |
| ryuna | 5:3d68334aab20 | 604 | /***** |
| ryuna | 5:3d68334aab20 | 605 | * this function is drawing a semicircle. |
| ryuna | 5:3d68334aab20 | 606 | * semicircle manipulated ir_data&ping_data. |
| ryuna | 5:3d68334aab20 | 607 | * |
| ryuna | 5:3d68334aab20 | 608 | * 変数名は後から変えるためにわかりやすい名前にしておく |
| ryuna | 5:3d68334aab20 | 609 | **/ |
| ryuna | 5:3d68334aab20 | 610 | uint8_t ir_num; |
| ryuna | 5:3d68334aab20 | 611 | uint8_t pingl,pingr; |
| ryuna | 5:3d68334aab20 | 612 | |
| ryuna | 5:3d68334aab20 | 613 | /* |
| ryuna | 5:3d68334aab20 | 614 | ir(1位,2位,必要なら距離も)と超音波のデータを取得 |
| ryuna | 5:3d68334aab20 | 615 | */ |
| ryuna | 5:3d68334aab20 | 616 | if(ir_num==7){//一致しているかどうか念のため |
| ryuna | 5:3d68334aab20 | 617 | return ; |
| ryuna | 5:3d68334aab20 | 618 | } |
| ryuna | 5:3d68334aab20 | 619 | if(pingr<pingl){//本当ならば壁と垂直かどうか、確認すべき。コンパスや超音波の合計を見れば可能 |
| ryuna | 5:3d68334aab20 | 620 | /*軌道の計算、楕円の半分見たいな感じの軌道がベスト*/ |
| ryuna | 5:3d68334aab20 | 621 | /*半円なのであらかじめステッピングを回転させる必要がある*/ |
| ryuna | 5:3d68334aab20 | 622 | /*モーター設定*/ |
| ryuna | 5:3d68334aab20 | 623 | /*ステッピング設定*/ |
| ryuna | 5:3d68334aab20 | 624 | return; |
| ryuna | 5:3d68334aab20 | 625 | } |
| ryuna | 5:3d68334aab20 | 626 | /*軌道の計算、楕円の半分見たいな感じの軌道がベスト*/ |
| ryuna | 5:3d68334aab20 | 627 | /*半円なのであらかじめステッピングを回転させる必要がある*/ |
| ryuna | 5:3d68334aab20 | 628 | /*モーター設定*/ |
| ryuna | 5:3d68334aab20 | 629 | /*ステッピング設定*/ |
| ryuna | 5:3d68334aab20 | 630 | |
| ryuna | 5:3d68334aab20 | 631 | |
| ryuna | 5:3d68334aab20 | 632 | } |
| ryuna | 3:8d718ddb84c9 | 633 | void ControlRobo8(int *CompassDef)//BackLeft |
| ryuna | 3:8d718ddb84c9 | 634 | {} |
| ryuna | 3:8d718ddb84c9 | 635 | void ControlRobo9(int *CompassDef)//LeftBack |
| ryuna | 3:8d718ddb84c9 | 636 | {} |
| ryuna | 3:8d718ddb84c9 | 637 | void ControlRobo10(int *CompassDef)//Left |
| ryuna | 3:8d718ddb84c9 | 638 | {} |
| ryuna | 3:8d718ddb84c9 | 639 | void ControlRobo11(int *CompassDef)//LeftFront |
| ryuna | 3:8d718ddb84c9 | 640 | {} |
| ryuna | 3:8d718ddb84c9 | 641 | void GoHome(int *CompassDef)//Ball is not found. |
| ryuna | 5:3d68334aab20 | 642 | { |
| ryuna | 7:7a0aee1477d9 | 643 | /*line検知をしないバージョン*/ |
| ryuna | 7:7a0aee1477d9 | 644 | int Compass; |
| ryuna | 7:7a0aee1477d9 | 645 | char ping[2] = {0}; |
| ryuna | 5:3d68334aab20 | 646 | |
| ryuna | 7:7a0aee1477d9 | 647 | Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360; |
| ryuna | 7:7a0aee1477d9 | 648 | if(!((Compass>=150)&&(Compass<=210))){ |
| ryuna | 7:7a0aee1477d9 | 649 | while(!((Compass>=150)&&(Compass<=210))){ |
| ryuna | 7:7a0aee1477d9 | 650 | move(20,-20);//適当な回転。 |
| ryuna | 7:7a0aee1477d9 | 651 | Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360; |
| ryuna | 7:7a0aee1477d9 | 652 | } |
| ryuna | 7:7a0aee1477d9 | 653 | /*return;してもいいかもしれない*/ |
| ryuna | 7:7a0aee1477d9 | 654 | } |
| ryuna | 7:7a0aee1477d9 | 655 | Step.GoHome(); |
| ryuna | 7:7a0aee1477d9 | 656 | Step.BusyWait(0); |
| ryuna | 7:7a0aee1477d9 | 657 | |
| ryuna | 7:7a0aee1477d9 | 658 | PingReceiveFB(ping); |
| ryuna | 7:7a0aee1477d9 | 659 | if(ping[1] >=60){//後ろからの距離60cm |
| ryuna | 7:7a0aee1477d9 | 660 | while(ping[1] >=60){ |
| ryuna | 7:7a0aee1477d9 | 661 | move(-20,-20); |
| ryuna | 7:7a0aee1477d9 | 662 | PingReceiveFB(ping); |
| ryuna | 7:7a0aee1477d9 | 663 | } |
| ryuna | 7:7a0aee1477d9 | 664 | } |
| ryuna | 7:7a0aee1477d9 | 665 | |
| ryuna | 7:7a0aee1477d9 | 666 | move(0,0);//stop |
| ryuna | 5:3d68334aab20 | 667 | |
| ryuna | 5:3d68334aab20 | 668 | } |
| ryuna | 3:8d718ddb84c9 | 669 | |
| ryuna | 3:8d718ddb84c9 | 670 | |
| ryuna | 4:8444360f08e2 | 671 | uint8_t SwRead(){ |
| ryuna | 4:8444360f08e2 | 672 | /****** |
| ryuna | 4:8444360f08e2 | 673 | *返却値はスイッチの状況 |
| ryuna | 4:8444360f08e2 | 674 | *参照元::aaaaa |
| ryuna | 4:8444360f08e2 | 675 | * |
| ryuna | 4:8444360f08e2 | 676 | *Calibration = 0x01; |
| ryuna | 4:8444360f08e2 | 677 | *Kicker = 0x02; |
| ryuna | 4:8444360f08e2 | 678 | *Debug1 = 0x04; |
| ryuna | 4:8444360f08e2 | 679 | *Debug2 = 0x08; |
| ryuna | 4:8444360f08e2 | 680 | *StartS = 0x10; |
| ryuna | 4:8444360f08e2 | 681 | * |
| ryuna | 4:8444360f08e2 | 682 | *****/ |
| ryuna | 4:8444360f08e2 | 683 | uint8_t i,temp,temp2; |
| ryuna | 4:8444360f08e2 | 684 | temp = Sw; |
| ryuna | 4:8444360f08e2 | 685 | if(!(temp == Calibration |
| ryuna | 4:8444360f08e2 | 686 | ||temp == Kicker |
| ryuna | 4:8444360f08e2 | 687 | ||temp == Debug1 |
| ryuna | 4:8444360f08e2 | 688 | ||temp == Debug2 |
| ryuna | 4:8444360f08e2 | 689 | ||temp == StartS)) return 0;/*スイッチは押されていない状況*/ |
| ryuna | 4:8444360f08e2 | 690 | if(temp !=0x00){ |
| ryuna | 4:8444360f08e2 | 691 | for(i = 0; i < 50; i++); |
| ryuna | 4:8444360f08e2 | 692 | temp2 = Sw; |
| ryuna | 4:8444360f08e2 | 693 | if(temp == temp2){ |
| ryuna | 4:8444360f08e2 | 694 | return temp; |
| ryuna | 4:8444360f08e2 | 695 | } |
| ryuna | 4:8444360f08e2 | 696 | } |
| ryuna | 4:8444360f08e2 | 697 | return 0; |
| ryuna | 4:8444360f08e2 | 698 | } |
| ryuna | 2:054444aa1990 | 699 | |
| ryuna | 1:7d4921b5d638 | 700 | //通信（モータ用） |
| ryuna | 1:7d4921b5d638 | 701 | void tx_motor(){ |
| ryuna | 1:7d4921b5d638 | 702 | array(speed[0],speed[1],speed[3],speed[2]); |
| ryuna | 1:7d4921b5d638 | 703 | Motor.printf("%s",StringFIN.c_str()); |
| ryuna | 1:7d4921b5d638 | 704 | } |
| ryuna | 0:ae08e2e1d82d | 705 | |
| ryuna | 3:8d718ddb84c9 | 706 | void SetUp(int *compass_def){ |
| ryuna | 2:054444aa1990 | 707 | /*初期化*/ |
| ryuna | 3:8d718ddb84c9 | 708 | Kick = 0; |
| ryuna | 4:8444360f08e2 | 709 | Sw.mode(PullUp); |
| ryuna | 1:7d4921b5d638 | 710 | Motor.baud(115200); //ボーレート設定 |
| ryuna | 1:7d4921b5d638 | 711 | Motor.printf("1F0002F0003F0004F000\r\n"); //モータ停止 |
| ryuna | 1:7d4921b5d638 | 712 | Motor.attach(&tx_motor,Serial::TxIrq); //送信空き割り込み（モータ用） |
| ryuna | 2:054444aa1990 | 713 | move(0,0);//停止 |
| ryuna | 2:054444aa1990 | 714 | |
| ryuna | 2:054444aa1990 | 715 | Step.Resets(); |
| ryuna | 7:7a0aee1477d9 | 716 | Step.ReleseSW(0,1); |
| ryuna | 2:054444aa1990 | 717 | |
| ryuna | 3:8d718ddb84c9 | 718 | hmc6352.setOpMode(HMC6352_CONTINUOUS, 1, 20); |
| ryuna | 3:8d718ddb84c9 | 719 | *compass_def = (hmc6352.sample() / 10); |
| ryuna | 6:c2c31bc971ad | 720 | |
| ryuna | 2:054444aa1990 | 721 | Lcd.cls(); |
| ryuna | 2:054444aa1990 | 722 | |
| ryuna | 2:054444aa1990 | 723 | /*初期化終了*/ |
| ryuna | 0:ae08e2e1d82d | 724 | |
| ryuna | 0:ae08e2e1d82d | 725 | |
| ryuna | 2:054444aa1990 | 726 | } |
| ryuna | 2:054444aa1990 | 727 | void StartLoop(){ |
| ryuna | 2:054444aa1990 | 728 | /* |
| ryuna | 2:054444aa1990 | 729 | *スイッチが押されるまでロボットはスタートしない． |
| ryuna | 2:054444aa1990 | 730 | * |
| ryuna | 2:054444aa1990 | 731 | *待っている間にほかのスイッチが押された場合は押されている間その動作をする等. |
| ryuna | 2:054444aa1990 | 732 | * |
| ryuna | 4:8444360f08e2 | 733 | *switch割り当て |
| ryuna | 4:8444360f08e2 | 734 | *1.コンパスのキャリブレーション実行スイッチ |
| ryuna | 7:7a0aee1477d9 | 735 | *2.キッカーのキック(check用) |
| ryuna | 4:8444360f08e2 | 736 | *3,4.自由 |
| ryuna | 4:8444360f08e2 | 737 | *5.StartSw |
| ryuna | 2:054444aa1990 | 738 | */ |
| ryuna | 4:8444360f08e2 | 739 | uint8_t State = 0; |
| ryuna | 4:8444360f08e2 | 740 | |
| ryuna | 4:8444360f08e2 | 741 | while(1){ |
| ryuna | 4:8444360f08e2 | 742 | |
| ryuna | 4:8444360f08e2 | 743 | State = SwRead(); |
| ryuna | 4:8444360f08e2 | 744 | if(State == 0) continue; |
| ryuna | 4:8444360f08e2 | 745 | |
| ryuna | 4:8444360f08e2 | 746 | if(State == Calibration){ |
| ryuna | 4:8444360f08e2 | 747 | /*calibration command enter... |
| ryuna | 4:8444360f08e2 | 748 | * |
| ryuna | 4:8444360f08e2 | 749 | * |
| ryuna | 4:8444360f08e2 | 750 | */ |
| ryuna | 4:8444360f08e2 | 751 | continue; |
| ryuna | 4:8444360f08e2 | 752 | } |
| ryuna | 4:8444360f08e2 | 753 | if(State == Kicker){ |
| ryuna | 4:8444360f08e2 | 754 | /* |
| ryuna | 4:8444360f08e2 | 755 | *kicker check |
| ryuna | 4:8444360f08e2 | 756 | * |
| ryuna | 4:8444360f08e2 | 757 | * |
| ryuna | 4:8444360f08e2 | 758 | */ |
| ryuna | 4:8444360f08e2 | 759 | continue; |
| ryuna | 4:8444360f08e2 | 760 | } |
| ryuna | 4:8444360f08e2 | 761 | if(State == Debug1){ |
| ryuna | 5:3d68334aab20 | 762 | /* debug command free |
| ryuna | 5:3d68334aab20 | 763 | * |
| ryuna | 5:3d68334aab20 | 764 | *display out to selected 3 menus. compass, ir, ping, line,etc... |
| ryuna | 5:3d68334aab20 | 765 | * |
| ryuna | 5:3d68334aab20 | 766 | **/ |
| ryuna | 4:8444360f08e2 | 767 | |
| ryuna | 4:8444360f08e2 | 768 | } |
| ryuna | 4:8444360f08e2 | 769 | if(State == Debug2){ |
| ryuna | 5:3d68334aab20 | 770 | /* debug command free |
| ryuna | 5:3d68334aab20 | 771 | * |
| ryuna | 5:3d68334aab20 | 772 | * decide movement of the beginning. |
| ryuna | 5:3d68334aab20 | 773 | * |
| ryuna | 5:3d68334aab20 | 774 | * |
| ryuna | 5:3d68334aab20 | 775 | **/ |
| ryuna | 4:8444360f08e2 | 776 | |
| ryuna | 4:8444360f08e2 | 777 | } |
| ryuna | 4:8444360f08e2 | 778 | if(State == StartS){ |
| ryuna | 4:8444360f08e2 | 779 | /*game start...*/ |
| ryuna | 4:8444360f08e2 | 780 | return; |
| ryuna | 4:8444360f08e2 | 781 | } |
| ryuna | 4:8444360f08e2 | 782 | |
| ryuna | 4:8444360f08e2 | 783 | } |
| ryuna | 2:054444aa1990 | 784 | |
| ryuna | 2:054444aa1990 | 785 | } |
| ryuna | 2:054444aa1990 | 786 | int main() { |
| ryuna | 3:8d718ddb84c9 | 787 | |
| ryuna | 3:8d718ddb84c9 | 788 | /*Ir*/ |
| ryuna | 3:8d718ddb84c9 | 789 | uint8_t IrNum;//場所によるirの数を表したもの0~11まではボールがある状態12はボールがない状態 |
| ryuna | 2:054444aa1990 | 790 | |
| ryuna | 2:054444aa1990 | 791 | /*Line*/ |
| ryuna | 3:8d718ddb84c9 | 792 | uint8_t Line; |
| ryuna | 3:8d718ddb84c9 | 793 | |
| ryuna | 3:8d718ddb84c9 | 794 | /*Compass*/ |
| ryuna | 3:8d718ddb84c9 | 795 | int CompassDef = 0, Compass = 0; |
| ryuna | 2:054444aa1990 | 796 | |
| ryuna | 3:8d718ddb84c9 | 797 | /*State */ |
| ryuna | 3:8d718ddb84c9 | 798 | //Direction LineIr = NA;//方位設定奴...エラーでてめんどくさいので後でまたやることにする。 |
| ryuna | 3:8d718ddb84c9 | 799 | uint8_t LineIr = NA; |
| ryuna | 3:8d718ddb84c9 | 800 | |
| ryuna | 2:054444aa1990 | 801 | |
| ryuna | 2:054444aa1990 | 802 | |
| ryuna | 3:8d718ddb84c9 | 803 | /*関数ポインタ*/ |
| ryuna | 3:8d718ddb84c9 | 804 | void (*ControlRobo[13])(int *);//irの位置による分岐 |
| ryuna | 3:8d718ddb84c9 | 805 | ControlRobo[0] = ControlRobo0; |
| ryuna | 3:8d718ddb84c9 | 806 | ControlRobo[1] = ControlRobo1; |
| ryuna | 3:8d718ddb84c9 | 807 | ControlRobo[2] = ControlRobo2; |
| ryuna | 3:8d718ddb84c9 | 808 | ControlRobo[3] = ControlRobo3; |
| ryuna | 3:8d718ddb84c9 | 809 | ControlRobo[4] = ControlRobo4; |
| ryuna | 3:8d718ddb84c9 | 810 | ControlRobo[5] = ControlRobo5; |
| ryuna | 3:8d718ddb84c9 | 811 | ControlRobo[6] = ControlRobo6; |
| ryuna | 3:8d718ddb84c9 | 812 | ControlRobo[7] = ControlRobo7; |
| ryuna | 3:8d718ddb84c9 | 813 | ControlRobo[8] = ControlRobo8; |
| ryuna | 3:8d718ddb84c9 | 814 | ControlRobo[9] = ControlRobo9; |
| ryuna | 3:8d718ddb84c9 | 815 | ControlRobo[10] = ControlRobo10; |
| ryuna | 3:8d718ddb84c9 | 816 | ControlRobo[11] = ControlRobo11; |
| ryuna | 3:8d718ddb84c9 | 817 | ControlRobo[12] = GoHome; |
| ryuna | 2:054444aa1990 | 818 | |
| ryuna | 3:8d718ddb84c9 | 819 | |
| ryuna | 4:8444360f08e2 | 820 | SetUp(&CompassDef);/*set up routine*/ |
| ryuna | 2:054444aa1990 | 821 | |
| ryuna | 4:8444360f08e2 | 822 | StartLoop(); /*loop stat, switch push end*/ |
| ryuna | 2:054444aa1990 | 823 | |
| ryuna | 6:c2c31bc971ad | 824 | /* |
| ryuna | 6:c2c31bc971ad | 825 | 前を向くように設定をするべき |
| ryuna | 6:c2c31bc971ad | 826 | アウトオブバウンズからの復帰時に反対を向けてスタートなので必要。 |
| ryuna | 6:c2c31bc971ad | 827 | */ |
| ryuna | 6:c2c31bc971ad | 828 | while(1){//前を向かせるwhile |
| ryuna | 6:c2c31bc971ad | 829 | Compass = ((hmc6352.sample() / 10) + 540 - CompassDef) % 360; |
| ryuna | 6:c2c31bc971ad | 830 | if(Compass == 3) break;//前を向いたら抜ける。 |
| ryuna | 6:c2c31bc971ad | 831 | } |
| ryuna | 7:7a0aee1477d9 | 832 | while(0) { |
| ryuna | 2:054444aa1990 | 833 | |
| ryuna | 2:054444aa1990 | 834 | Line = LineReceive(); |
| ryuna | 2:054444aa1990 | 835 | |
| ryuna | 3:8d718ddb84c9 | 836 | if(Line){ //ラインを検知していないか |
| ryuna | 7:7a0aee1477d9 | 837 | LineIr = Line & (IrReceiveS()/3 + 1);//計(lineとirの方位を合わせる。 |
| ryuna | 3:8d718ddb84c9 | 838 | while(LineIr){ |
| ryuna | 3:8d718ddb84c9 | 839 | move(0,0);// |
| ryuna | 3:8d718ddb84c9 | 840 | Led[1] = Led[2] = Led[3] = 1; |
| ryuna | 2:054444aa1990 | 841 | |
| ryuna | 3:8d718ddb84c9 | 842 | LineIr = LineReceive() & (IrReceiveS()/3 + 1); |
| ryuna | 3:8d718ddb84c9 | 843 | } |
| ryuna | 3:8d718ddb84c9 | 844 | Led[1] = Led[2] = Led[3] = 0; |
| ryuna | 3:8d718ddb84c9 | 845 | continue; |
| ryuna | 3:8d718ddb84c9 | 846 | |
| ryuna | 3:8d718ddb84c9 | 847 | } |
| ryuna | 3:8d718ddb84c9 | 848 | |
| ryuna | 3:8d718ddb84c9 | 849 | if(Step.BusyCheck()){//ステッピングが動いていないか |
| ryuna | 3:8d718ddb84c9 | 850 | continue; |
| ryuna | 3:8d718ddb84c9 | 851 | |
| ryuna | 3:8d718ddb84c9 | 852 | } |
| ryuna | 3:8d718ddb84c9 | 853 | |
| ryuna | 3:8d718ddb84c9 | 854 | IrNum = IrReceiveS(); |
| ryuna | 3:8d718ddb84c9 | 855 | |
| ryuna | 3:8d718ddb84c9 | 856 | (*ControlRobo[IrNum])(&CompassDef);//irの位置によったループ等の処理に移る。 |
| ryuna | 3:8d718ddb84c9 | 857 | |
| ryuna | 3:8d718ddb84c9 | 858 | //wait_ms(0); |
| ryuna | 7:7a0aee1477d9 | 859 | } |
| ryuna | 7:7a0aee1477d9 | 860 | |
| ryuna | 7:7a0aee1477d9 | 861 | while(1){ |
| ryuna | 7:7a0aee1477d9 | 862 | /*デモプログラム |
| ryuna | 7:7a0aee1477d9 | 863 | * |
| ryuna | 7:7a0aee1477d9 | 864 | *ぐるぐる |
| ryuna | 7:7a0aee1477d9 | 865 | * |
| ryuna | 7:7a0aee1477d9 | 866 | * |
| ryuna | 7:7a0aee1477d9 | 867 | */ |
| ryuna | 7:7a0aee1477d9 | 868 | move(30,30); |
| ryuna | 7:7a0aee1477d9 | 869 | |
| ryuna | 7:7a0aee1477d9 | 870 | wait(1); |
| ryuna | 7:7a0aee1477d9 | 871 | |
| ryuna | 7:7a0aee1477d9 | 872 | move(-30,-30); |
| ryuna | 7:7a0aee1477d9 | 873 | move(0,0); |
| ryuna | 7:7a0aee1477d9 | 874 | |
| ryuna | 7:7a0aee1477d9 | 875 | Step.Run(1,1200); |
| ryuna | 7:7a0aee1477d9 | 876 | move(10,10); |
| ryuna | 7:7a0aee1477d9 | 877 | wait(1.5); |
| ryuna | 7:7a0aee1477d9 | 878 | |
| ryuna | 7:7a0aee1477d9 | 879 | move(0,0); |
| ryuna | 7:7a0aee1477d9 | 880 | Step.SoftStop(); |
| ryuna | 7:7a0aee1477d9 | 881 | |
| ryuna | 7:7a0aee1477d9 | 882 | wait(0.5); |
| ryuna | 7:7a0aee1477d9 | 883 | |
| ryuna | 7:7a0aee1477d9 | 884 | Step.GoHome(); |
| ryuna | 7:7a0aee1477d9 | 885 | |
| ryuna | 7:7a0aee1477d9 | 886 | wait(1.5); |
| ryuna | 7:7a0aee1477d9 | 887 | |
| ryuna | 7:7a0aee1477d9 | 888 | |
| ryuna | 3:8d718ddb84c9 | 889 | |
| ryuna | 0:ae08e2e1d82d | 890 | } |
| ryuna | 7:7a0aee1477d9 | 891 | |
| ryuna | 0:ae08e2e1d82d | 892 | } |