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