CatPot 2015-2016 / Mbed 2 deprecated CatPot_Main_T

CatPot for defence on RoboCup in 2015 winter

Dependencies:   AQM0802A HMC6352 MultiSerial PID Servo mbed

main.txt@7:81f57b67dff8, 2015-03-14 (annotated)

Committer:
lilac0112_1
Date:
Sat Mar 14 07:16:46 2015 +0000
Revision:
7:81f57b67dff8
Parent:
3:2f74791564c9 
The end...

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lilac0112_1 0:d35efbf4d62e 1 /***********************************
lilac0112_1 2:39135c67083d 2 *RoboCupJunior Soccer B Open 2015
lilac0112_1 2:39135c67083d 3 *Koshinnestu progrum
lilac0112_1 0:d35efbf4d62e 4 *
lilac0112_1 0:d35efbf4d62e 5 *　
lilac0112_1 0:d35efbf4d62e 6 *　データからロボットの移動やキッカー等のモータの動作を決定する処理を行う
lilac0112_1 0:d35efbf4d62e 7 *
lilac0112_1 0:d35efbf4d62e 8 *　MotorDriverにmaxonに命令
lilac0112_1 0:d35efbf4d62e 9 *　
lilac0112_1 2:39135c67083d 10 *　servoにステアリング指示
lilac0112_1 0:d35efbf4d62e 11 *　
lilac0112_1 0:d35efbf4d62e 12 *　LCDでデバック
lilac0112_1 0:d35efbf4d62e 13 *
lilac0112_1 0:d35efbf4d62e 14 *　スイッチ4つとスタートスイッチで処理を実行
lilac0112_1 0:d35efbf4d62e 15 *
lilac0112_1 2:39135c67083d 16
lilac0112_1 0:d35efbf4d62e 17 *************************
lilac0112_1 2:39135c67083d 18 * Pin Map
lilac0112_1 2:39135c67083d 19 *
lilac0112_1 2:39135c67083d 20 * p5~p8 >> BusIn >> LineSensor
lilac0112_1 0:d35efbf4d62e 21 *
lilac0112_1 2:39135c67083d 22 * p9,p10 >> I2C >> LPC1114FN28/102 read & Compass
lilac0112_1 0:d35efbf4d62e 23 *
lilac0112_1 2:39135c67083d 24 * p13,p14 >> Serial >> Motor
lilac0112_1 2:39135c67083d 25 *
lilac0112_1 2:39135c67083d 26 * p21 >> PwmOut >> Servo
lilac0112_1 0:d35efbf4d62e 27 *
lilac0112_1 2:39135c67083d 28 * p22~p26 >> DigitalIn >> DebugSw and StartSw
lilac0112_1 0:d35efbf4d62e 29 *
lilac0112_1 2:39135c67083d 30 * p27,p28 >> I2C >> DebugLCD
lilac0112_1 0:d35efbf4d62e 31 *
lilac0112_1 2:39135c67083d 32 * p29 >> DigitalOut >> Kicker
lilac0112_1 2:39135c67083d 33 *
lilac0112_1 2:39135c67083d 34 * *never use pin number p11,p12,p15,p16,p17,p18,p19,p20,p30
lilac0112_1 2:39135c67083d 35 *
lilac0112_1 2:39135c67083d 36 *
lilac0112_1 0:d35efbf4d62e 37 ******************************/
lilac0112_1 0:d35efbf4d62e 38
lilac0112_1 0:d35efbf4d62e 39 #include "mbed.h"
lilac0112_1 2:39135c67083d 40 #include <math.h>
lilac0112_1 2:39135c67083d 41 #include <sstream>
lilac0112_1 0:d35efbf4d62e 42 #include "PID.h"
lilac0112_1 0:d35efbf4d62e 43 #include "AQM0802A.h"
lilac0112_1 2:39135c67083d 44 #include "MultiSerial.h"
lilac0112_1 0:d35efbf4d62e 45 #include "Servo.h"
lilac0112_1 2:39135c67083d 46 #include "main.h"
lilac0112_1 0:d35efbf4d62e 47
lilac0112_1 0:d35efbf4d62e 48
lilac0112_1 2:39135c67083d 49 void Receive(void){
lilac0112_1 0:d35efbf4d62e 50
lilac0112_1 2:39135c67083d 51 IrData[0] = rx_data[0];
lilac0112_1 2:39135c67083d 52 IrData[1] = rx_data[1];
lilac0112_1 2:39135c67083d 53 IrData[2] = rx_data[2];
lilac0112_1 2:39135c67083d 54 PingData[0] = rx_data[3];
lilac0112_1 2:39135c67083d 55 PingData[1] = rx_data[4];
lilac0112_1 2:39135c67083d 56 PingData[2] = rx_data[5];
lilac0112_1 2:39135c67083d 57 PingData[3] = rx_data[6];
lilac0112_1 2:39135c67083d 58 Compass =(rx_data[7]<<0) |
lilac0112_1 2:39135c67083d 59 (rx_data[8]<<8);
lilac0112_1 0:d35efbf4d62e 60
lilac0112_1 2:39135c67083d 61 /*
lilac0112_1 2:39135c67083d 62 pc.printf("%d\t", IrData[0]);
lilac0112_1 2:39135c67083d 63 pc.printf("%d\t", IrData[1]);
lilac0112_1 2:39135c67083d 64 pc.printf("%d\t", IrData[2]);
lilac0112_1 2:39135c67083d 65 pc.printf("%d\t", PingData[0]);
lilac0112_1 2:39135c67083d 66 pc.printf("%d\t", PingData[1]);
lilac0112_1 2:39135c67083d 67 pc.printf("%d\t", PingData[2]);
lilac0112_1 2:39135c67083d 68 pc.printf("%d\t", PingData[3]);
lilac0112_1 2:39135c67083d 69 pc.printf("%d\t", Compass);
lilac0112_1 2:39135c67083d 70 //pc.printf("%d\t", Mbed.__readSize);
lilac0112_1 2:39135c67083d 71 pc.printf("\n");
lilac0112_1 0:d35efbf4d62e 72 */
lilac0112_1 0:d35efbf4d62e 73
lilac0112_1 2:39135c67083d 74 if(IrData[0] == 255){
lilac0112_1 2:39135c67083d 75 IrNum = 12;
lilac0112_1 2:39135c67083d 76 return;
lilac0112_1 0:d35efbf4d62e 77 }
lilac0112_1 2:39135c67083d 78 IrNum = IrData[0]/12;
lilac0112_1 0:d35efbf4d62e 79
lilac0112_1 0:d35efbf4d62e 80 }
lilac0112_1 0:d35efbf4d62e 81
lilac0112_1 2:39135c67083d 82 void move(int vr,int vl, double vs ,double Rad){
lilac0112_1 0:d35efbf4d62e 83 double pwm[4] = {0};
lilac0112_1 0:d35efbf4d62e 84 uint8_t i = 0;
lilac0112_1 2:39135c67083d 85 pwm[0] = vr - vs;
lilac0112_1 1:e3248f278663 86 pwm[1] = 0;
lilac0112_1 0:d35efbf4d62e 87 pwm[2] = 0;
lilac0112_1 2:39135c67083d 88 pwm[3] = vl*(-1.2) + vs;
lilac0112_1 2:39135c67083d 89 pc.printf("%d\t %d\t %f\t %f\t\n", vr, vl, vs, Rad);
lilac0112_1 0:d35efbf4d62e 90 for(i = 0; i < 4; i++){
lilac0112_1 0:d35efbf4d62e 91 if(pwm[i] > 100){
lilac0112_1 0:d35efbf4d62e 92 pwm[i] = 100;
lilac0112_1 0:d35efbf4d62e 93 }else if(pwm[i] < -100){
lilac0112_1 0:d35efbf4d62e 94 pwm[i] = -100;
lilac0112_1 0:d35efbf4d62e 95 }
lilac0112_1 0:d35efbf4d62e 96 speed[i] = pwm[i];
lilac0112_1 0:d35efbf4d62e 97 }
lilac0112_1 2:39135c67083d 98 SetRad = Rad;
lilac0112_1 2:39135c67083d 99 wait_ms(10);
lilac0112_1 2:39135c67083d 100 }
lilac0112_1 2:39135c67083d 101 void fool (int *Rad, int *Power){
lilac0112_1 2:39135c67083d 102 static int Last_Rad = 0;
lilac0112_1 2:39135c67083d 103 static int Last_Vector = 1;
lilac0112_1 2:39135c67083d 104 int rad = *Rad;
lilac0112_1 2:39135c67083d 105 int Temp;
lilac0112_1 2:39135c67083d 106 Temp = Last_Rad % 180;
lilac0112_1 2:39135c67083d 107
lilac0112_1 2:39135c67083d 108 if((Temp>70) &&(Temp<110)){
lilac0112_1 2:39135c67083d 109 Temp = *Rad % 180;
lilac0112_1 2:39135c67083d 110 if((Temp>70) &&(Temp<110)){
lilac0112_1 2:39135c67083d 111 Temp = abs(*Rad - Last_Rad);
lilac0112_1 2:39135c67083d 112 if(Temp>160){
lilac0112_1 2:39135c67083d 113 Last_Vector = -1 * Last_Vector;//正転逆転切り替え
lilac0112_1 2:39135c67083d 114 if(*Rad/180){
lilac0112_1 2:39135c67083d 115 *Rad = Angle[Last_Rad%180] -(Last_Rad - *Rad%180);
lilac0112_1 2:39135c67083d 116 *Power = *Power * Last_Vector;
lilac0112_1 2:39135c67083d 117 }else{
lilac0112_1 2:39135c67083d 118 *Rad = Angle[Last_Rad%180] -(Last_Rad%180 - *Rad);
lilac0112_1 2:39135c67083d 119 *Power = *Power * Last_Vector;
lilac0112_1 2:39135c67083d 120 }
lilac0112_1 2:39135c67083d 121 Last_Rad = rad;
lilac0112_1 2:39135c67083d 122 return;
lilac0112_1 2:39135c67083d 123 }else if((Last_Vector+2) == 1){
lilac0112_1 2:39135c67083d 124 /*逆転のまま角度拡張*/
lilac0112_1 2:39135c67083d 125 if(*Rad/180){
lilac0112_1 2:39135c67083d 126 *Rad = -360 + *Rad ;
lilac0112_1 2:39135c67083d 127 *Power = *Power * Last_Vector;
lilac0112_1 2:39135c67083d 128 }else{
lilac0112_1 2:39135c67083d 129 *Power = *Power * Last_Vector;
lilac0112_1 2:39135c67083d 130 }
lilac0112_1 2:39135c67083d 131 Last_Rad = rad;
lilac0112_1 2:39135c67083d 132 return;
lilac0112_1 2:39135c67083d 133
lilac0112_1 2:39135c67083d 134 }else if((Last_Vector+2) == 3){
lilac0112_1 2:39135c67083d 135 /*正転のまま*/
lilac0112_1 2:39135c67083d 136 if(*Rad/180){
lilac0112_1 2:39135c67083d 137 *Rad = -360 + *Rad ;
lilac0112_1 2:39135c67083d 138 *Power = *Power * Last_Vector;
lilac0112_1 2:39135c67083d 139 }else{
lilac0112_1 2:39135c67083d 140 *Power = *Power * Last_Vector;
lilac0112_1 2:39135c67083d 141 Last_Rad = rad;
lilac0112_1 2:39135c67083d 142 }
lilac0112_1 2:39135c67083d 143 Last_Rad = rad;
lilac0112_1 2:39135c67083d 144 return;
lilac0112_1 2:39135c67083d 145 }
lilac0112_1 2:39135c67083d 146 }
lilac0112_1 2:39135c67083d 147 }
lilac0112_1 2:39135c67083d 148 /*通常動作*/
lilac0112_1 2:39135c67083d 149 Last_Vector = RadToVector[(*Rad-1)/90];
lilac0112_1 2:39135c67083d 150 *Rad = Angle[*Rad%180];
lilac0112_1 2:39135c67083d 151 *Power = *Power * Last_Vector;
lilac0112_1 2:39135c67083d 152 Last_Rad = rad;
lilac0112_1 2:39135c67083d 153
lilac0112_1 2:39135c67083d 154 }
lilac0112_1 2:39135c67083d 155 int IrRad(){
lilac0112_1 2:39135c67083d 156 /*irの1位の値，2位の場所からirの360へ持っていく*/
lilac0112_1 2:39135c67083d 157
lilac0112_1 2:39135c67083d 158 uint8_t IrF =IrData[0]/12,IrS = IrData[0]%12;
lilac0112_1 2:39135c67083d 159 int irrad;
lilac0112_1 2:39135c67083d 160 irrad = 359 - IrF*30;
lilac0112_1 2:39135c67083d 161
lilac0112_1 2:39135c67083d 162 if(IrS){
lilac0112_1 2:39135c67083d 163 if(IrF == 0 ){
lilac0112_1 2:39135c67083d 164 if(IrS == 11){
lilac0112_1 2:39135c67083d 165 irrad = 15;
lilac0112_1 2:39135c67083d 166
lilac0112_1 2:39135c67083d 167 }else{
lilac0112_1 2:39135c67083d 168 irrad = 345;
lilac0112_1 2:39135c67083d 169 }
lilac0112_1 2:39135c67083d 170
lilac0112_1 2:39135c67083d 171 }
lilac0112_1 2:39135c67083d 172 irrad = irrad - (IrF - IrS)*15;
lilac0112_1 2:39135c67083d 173
lilac0112_1 2:39135c67083d 174 }
lilac0112_1 2:39135c67083d 175 return irrad;
lilac0112_1 0:d35efbf4d62e 176 }
lilac0112_1 0:d35efbf4d62e 177
lilac0112_1 2:39135c67083d 178
lilac0112_1 2:39135c67083d 179 void IrFrontAction( uint8_t IrMemo[],double ReV)//ball 12 or 0 o-clock
lilac0112_1 1:e3248f278663 180 {
lilac0112_1 1:e3248f278663 181
lilac0112_1 1:e3248f278663 182 }
lilac0112_1 2:39135c67083d 183
lilac0112_1 2:39135c67083d 184 void IrBackAction( uint8_t IrMemo[],double ReV)//ball found six o-clock
lilac0112_1 0:d35efbf4d62e 185 {
lilac0112_1 2:39135c67083d 186 /***
lilac0112_1 2:39135c67083d 187 * 6時にボールがある場合の処理．右と左のスペースを確認して広い方から回り込む
lilac0112_1 2:39135c67083d 188 *
lilac0112_1 2:39135c67083d 189 **/
lilac0112_1 2:39135c67083d 190 char Ping[2]={0};
lilac0112_1 2:39135c67083d 191
lilac0112_1 2:39135c67083d 192 //PingReceiveRL(Ping);
lilac0112_1 2:39135c67083d 193
lilac0112_1 0:d35efbf4d62e 194
lilac0112_1 2:39135c67083d 195 if(IrMemo[0] == 6){//check ir number
lilac0112_1 2:39135c67083d 196 return ;
lilac0112_1 0:d35efbf4d62e 197 }
lilac0112_1 2:39135c67083d 198 if(Ping[0]<Ping[1]){
lilac0112_1 2:39135c67083d 199 /*右が大きい*/
lilac0112_1 2:39135c67083d 200
lilac0112_1 2:39135c67083d 201
lilac0112_1 0:d35efbf4d62e 202 return;
lilac0112_1 0:d35efbf4d62e 203 }
lilac0112_1 2:39135c67083d 204 /*左が大きい*/
lilac0112_1 0:d35efbf4d62e 205
lilac0112_1 0:d35efbf4d62e 206
lilac0112_1 0:d35efbf4d62e 207
lilac0112_1 0:d35efbf4d62e 208 }
lilac0112_1 0:d35efbf4d62e 209
lilac0112_1 2:39135c67083d 210 void GoHome( uint8_t IrMemo[],double ReV)//Ball is not found.
lilac0112_1 2:39135c67083d 211 {
lilac0112_1 2:39135c67083d 212 //止まっとく
lilac0112_1 2:39135c67083d 213 S555.position(0.0);
lilac0112_1 2:39135c67083d 214
lilac0112_1 2:39135c67083d 215 move(0,0,ReV,0);
lilac0112_1 2:39135c67083d 216
lilac0112_1 2:39135c67083d 217 /*line検知をしないバージョン*/
lilac0112_1 2:39135c67083d 218 /*
lilac0112_1 2:39135c67083d 219 char Ping[2] = {0};
lilac0112_1 2:39135c67083d 220
lilac0112_1 2:39135c67083d 221
lilac0112_1 2:39135c67083d 222 PingReceiveFB(Ping);
lilac0112_1 2:39135c67083d 223 if(Ping[1] >=60){//後ろからの距離60cm
lilac0112_1 2:39135c67083d 224 move(-20,-20,ReV);
lilac0112_1 2:39135c67083d 225 PingReceiveFB(Ping);
lilac0112_1 2:39135c67083d 226 return ;
lilac0112_1 2:39135c67083d 227 }
lilac0112_1 0:d35efbf4d62e 228
lilac0112_1 2:39135c67083d 229 move(0,0,ReV);//stop
lilac0112_1 2:39135c67083d 230 */
lilac0112_1 2:39135c67083d 231 }
lilac0112_1 0:d35efbf4d62e 232
lilac0112_1 0:d35efbf4d62e 233
lilac0112_1 2:39135c67083d 234 void PidUpdate()
lilac0112_1 2:39135c67083d 235 {
lilac0112_1 2:39135c67083d 236 double inputPID = 0.0;
lilac0112_1 0:d35efbf4d62e 237
lilac0112_1 2:39135c67083d 238 Receive();
lilac0112_1 0:d35efbf4d62e 239
lilac0112_1 2:39135c67083d 240 pid.setSetPoint((int)((REFERENCE + SetAngle) / 1.0));
lilac0112_1 2:39135c67083d 241 inputPID = ((Compass / 10) + 540 - CompassDef) % 360;;
lilac0112_1 0:d35efbf4d62e 242
lilac0112_1 2:39135c67083d 243 pid.setProcessValue(inputPID);
lilac0112_1 2:39135c67083d 244 ReV = -(pid.compute());
lilac0112_1 0:d35efbf4d62e 245
lilac0112_1 0:d35efbf4d62e 246 }
lilac0112_1 2:39135c67083d 247
lilac0112_1 2:39135c67083d 248
lilac0112_1 2:39135c67083d 249
lilac0112_1 2:39135c67083d 250
lilac0112_1 2:39135c67083d 251
lilac0112_1 2:39135c67083d 252
lilac0112_1 0:d35efbf4d62e 253
lilac0112_1 0:d35efbf4d62e 254 uint8_t SwRead(){
lilac0112_1 0:d35efbf4d62e 255 /******
lilac0112_1 2:39135c67083d 256 *retrun : sw_state
lilac0112_1 2:39135c67083d 257 *StartS = 0x01;
lilac0112_1 2:39135c67083d 258 *Debug2 = 0x02;
lilac0112_1 0:d35efbf4d62e 259 *Debug1 = 0x04;
lilac0112_1 2:39135c67083d 260 *Debug3 = 0x06;
lilac0112_1 2:39135c67083d 261 *Kicker = 0x08;
lilac0112_1 2:39135c67083d 262 *Calibration = 0x10;
lilac0112_1 2:39135c67083d 263
lilac0112_1 0:d35efbf4d62e 264 *
lilac0112_1 0:d35efbf4d62e 265 *****/
lilac0112_1 0:d35efbf4d62e 266 uint8_t i,temp,temp2;
lilac0112_1 2:39135c67083d 267 temp = ~Sw - 224;
lilac0112_1 2:39135c67083d 268 if(!(temp == Kicker
lilac0112_1 0:d35efbf4d62e 269 ||temp == Debug1
lilac0112_1 0:d35efbf4d62e 270 ||temp == Debug2
lilac0112_1 2:39135c67083d 271 ||temp == Debug3
lilac0112_1 2:39135c67083d 272 ||temp == StartS)) return 0;/*スイッチが押されていない*/
lilac0112_1 2:39135c67083d 273 if(!(temp == 0x00)){
lilac0112_1 0:d35efbf4d62e 274 for(i = 0; i < 50; i++);
lilac0112_1 2:39135c67083d 275 temp2 = ~Sw - 224;
lilac0112_1 0:d35efbf4d62e 276 if(temp == temp2){
lilac0112_1 0:d35efbf4d62e 277 return temp;
lilac0112_1 0:d35efbf4d62e 278 }
lilac0112_1 0:d35efbf4d62e 279 }
lilac0112_1 0:d35efbf4d62e 280 return 0;
lilac0112_1 0:d35efbf4d62e 281 }
lilac0112_1 0:d35efbf4d62e 282
lilac0112_1 0:d35efbf4d62e 283 //通信（モータ用）
lilac0112_1 0:d35efbf4d62e 284 void tx_motor(){
lilac0112_1 2:39135c67083d 285
lilac0112_1 2:39135c67083d 286 /*if(loop==3){
lilac0112_1 2:39135c67083d 287 speed[0]=0;
lilac0112_1 2:39135c67083d 288 speed[1]=0;
lilac0112_1 2:39135c67083d 289 speed[2]=0;
lilac0112_1 2:39135c67083d 290 speed[3]=0;
lilac0112_1 2:39135c67083d 291 Motor.printf("1F0002F0003F0004F000\r\n"); //モータ停止
lilac0112_1 2:39135c67083d 292 }else{
lilac0112_1 2:39135c67083d 293 Motor.printf("1F0202F0003F0204F000\r\n");
lilac0112_1 2:39135c67083d 294 }*/
lilac0112_1 0:d35efbf4d62e 295 array(speed[0],speed[1],speed[3],speed[2]);
lilac0112_1 0:d35efbf4d62e 296 Motor.printf("%s",StringFIN.c_str());
lilac0112_1 2:39135c67083d 297 S555.position(SetRad*11/9.0);
lilac0112_1 0:d35efbf4d62e 298 }
lilac0112_1 0:d35efbf4d62e 299
lilac0112_1 2:39135c67083d 300 void SetUp(){
lilac0112_1 0:d35efbf4d62e 301 /*初期化*/
lilac0112_1 0:d35efbf4d62e 302 Motor.baud(115200); //ボーレート設定
lilac0112_1 0:d35efbf4d62e 303 Motor.printf("1F0002F0003F0004F000\r\n"); //モータ停止
lilac0112_1 0:d35efbf4d62e 304 Motor.attach(&tx_motor,Serial::TxIrq); //送信空き割り込み（モータ用）
lilac0112_1 2:39135c67083d 305
lilac0112_1 0:d35efbf4d62e 306
lilac0112_1 2:39135c67083d 307 S555.calibrate(0.0005, 120.0);
lilac0112_1 2:39135c67083d 308 S555.position(0.0); //初期位置にセット
lilac0112_1 2:39135c67083d 309 move(0,0,0,0);//停止
lilac0112_1 0:d35efbf4d62e 310
lilac0112_1 2:39135c67083d 311 Kick = 0;
lilac0112_1 2:39135c67083d 312 Sw.mode(PullUp);
lilac0112_1 0:d35efbf4d62e 313
lilac0112_1 2:39135c67083d 314 pid.setInputLimits(MINIMUM,MAXIMUM); //pid sed def
lilac0112_1 2:39135c67083d 315 pid.setOutputLimits(-OUT_LIMIT, OUT_LIMIT); //pid sed def
lilac0112_1 2:39135c67083d 316 pid.setBias(PID_BIAS); //pid sed def
lilac0112_1 2:39135c67083d 317 pid.setMode(AUTO_MODE); //pid sed def
lilac0112_1 2:39135c67083d 318 pid.setSetPoint(REFERENCE); //pid sed def
lilac0112_1 0:d35efbf4d62e 319
lilac0112_1 2:39135c67083d 320 for(int i=0; i<15; i++){
lilac0112_1 2:39135c67083d 321 wait_ms(10);
lilac0112_1 2:39135c67083d 322 Receive();
lilac0112_1 2:39135c67083d 323 CompassDef = (Compass / 10);
lilac0112_1 2:39135c67083d 324 PidUpdate();
lilac0112_1 2:39135c67083d 325 }
lilac0112_1 0:d35efbf4d62e 326
lilac0112_1 0:d35efbf4d62e 327 }
lilac0112_1 0:d35efbf4d62e 328 void StartLoop(){
lilac0112_1 2:39135c67083d 329
lilac0112_1 0:d35efbf4d62e 330 uint8_t State = 0;
lilac0112_1 2:39135c67083d 331 uint8_t LineData = 0;
lilac0112_1 0:d35efbf4d62e 332
lilac0112_1 0:d35efbf4d62e 333 while(1){
lilac0112_1 2:39135c67083d 334 Led[0] = Led[1] = Led[2] = Led[3] = 1;
lilac0112_1 2:39135c67083d 335 //Lcd.cls();
lilac0112_1 0:d35efbf4d62e 336 State = SwRead();
lilac0112_1 0:d35efbf4d62e 337 if(State == 0) continue;
lilac0112_1 0:d35efbf4d62e 338
lilac0112_1 2:39135c67083d 339 if(State == StartS){
lilac0112_1 2:39135c67083d 340 loop=0;
lilac0112_1 2:39135c67083d 341 return;
lilac0112_1 2:39135c67083d 342 /*loop end & start*/
lilac0112_1 0:d35efbf4d62e 343 }
lilac0112_1 2:39135c67083d 344
lilac0112_1 2:39135c67083d 345 if(State == Debug1){
lilac0112_1 2:39135c67083d 346 while((State == Debug1)){
lilac0112_1 2:39135c67083d 347 LineData = (~Line+0x00) & 0x0F;
lilac0112_1 2:39135c67083d 348 Lcd.printf("%d\n",LineData);
lilac0112_1 2:39135c67083d 349
lilac0112_1 2:39135c67083d 350 wait_ms(100);
lilac0112_1 2:39135c67083d 351 State = SwRead();
lilac0112_1 2:39135c67083d 352 }
lilac0112_1 2:39135c67083d 353 Lcd.cls();
lilac0112_1 0:d35efbf4d62e 354 continue;
lilac0112_1 0:d35efbf4d62e 355
lilac0112_1 0:d35efbf4d62e 356 }
lilac0112_1 0:d35efbf4d62e 357 if(State == Debug2){
lilac0112_1 2:39135c67083d 358 while((State == Debug2)){
lilac0112_1 2:39135c67083d 359
lilac0112_1 2:39135c67083d 360 Receive();
lilac0112_1 2:39135c67083d 361
lilac0112_1 2:39135c67083d 362 Lcd.printf("%d\n",IrNum);
lilac0112_1 2:39135c67083d 363 wait_ms(100);
lilac0112_1 2:39135c67083d 364 State = SwRead();
lilac0112_1 2:39135c67083d 365 }
lilac0112_1 2:39135c67083d 366 Lcd.cls();
lilac0112_1 2:39135c67083d 367 continue;
lilac0112_1 0:d35efbf4d62e 368 }
lilac0112_1 2:39135c67083d 369
lilac0112_1 2:39135c67083d 370 if(State == Debug3){
lilac0112_1 2:39135c67083d 371 while((State == Debug3)){
lilac0112_1 2:39135c67083d 372
lilac0112_1 2:39135c67083d 373 Receive();
lilac0112_1 2:39135c67083d 374
lilac0112_1 2:39135c67083d 375 Lcd.printf("%d\n",((Compass / 10) + 540 - CompassDef) % 360);
lilac0112_1 2:39135c67083d 376
lilac0112_1 2:39135c67083d 377 wait_ms(100);
lilac0112_1 2:39135c67083d 378 State = SwRead();
lilac0112_1 2:39135c67083d 379 }
lilac0112_1 2:39135c67083d 380 Lcd.cls();
lilac0112_1 2:39135c67083d 381 continue;
lilac0112_1 0:d35efbf4d62e 382 }
lilac0112_1 0:d35efbf4d62e 383
lilac0112_1 2:39135c67083d 384 if(State == Kicker){
lilac0112_1 2:39135c67083d 385 Led[0] = Led[1] = Led[2] = 0;
lilac0112_1 2:39135c67083d 386 /*move(20,20,0,0);
lilac0112_1 2:39135c67083d 387 while((State == Kicker)){
lilac0112_1 2:39135c67083d 388 wait_ms(100);
lilac0112_1 2:39135c67083d 389 State = SwRead();
lilac0112_1 2:39135c67083d 390
lilac0112_1 2:39135c67083d 391 }
lilac0112_1 2:39135c67083d 392 move(0,0,0,0);
lilac0112_1 2:39135c67083d 393 */
lilac0112_1 2:39135c67083d 394 continue;
lilac0112_1 2:39135c67083d 395 }
lilac0112_1 2:39135c67083d 396 }
lilac0112_1 0:d35efbf4d62e 397
lilac0112_1 0:d35efbf4d62e 398 }
lilac0112_1 1:e3248f278663 399
lilac0112_1 2:39135c67083d 400 void CompassFB(){
lilac0112_1 1:e3248f278663 401
lilac0112_1 2:39135c67083d 402 for(int i=1; i<=18; i++){
lilac0112_1 2:39135c67083d 403 if(abs(REFERENCE - Compass) <= i*10){
lilac0112_1 2:39135c67083d 404 ReV = (i-1)*2;
lilac0112_1 2:39135c67083d 405 if(Compass > REFERENCE){
lilac0112_1 2:39135c67083d 406 ReV = ReV*(-1);
lilac0112_1 2:39135c67083d 407 }
lilac0112_1 2:39135c67083d 408 break;
lilac0112_1 2:39135c67083d 409 }
lilac0112_1 1:e3248f278663 410 }
lilac0112_1 1:e3248f278663 411 }
lilac0112_1 1:e3248f278663 412
lilac0112_1 0:d35efbf4d62e 413 int main() {
lilac0112_1 0:d35efbf4d62e 414
lilac0112_1 0:d35efbf4d62e 415 /*Ir*/
lilac0112_1 2:39135c67083d 416 //→.h
lilac0112_1 0:d35efbf4d62e 417 /*Line*/
lilac0112_1 2:39135c67083d 418 uint8_t LineData = 0;
lilac0112_1 0:d35efbf4d62e 419
lilac0112_1 2:39135c67083d 420
lilac0112_1 2:39135c67083d 421 /*PID補正move加算値 Revise */
lilac0112_1 2:39135c67083d 422 //double ReV = 0.0;→global
lilac0112_1 2:39135c67083d 423 //double SetC;
lilac0112_1 0:d35efbf4d62e 424
lilac0112_1 0:d35efbf4d62e 425 /*State */
lilac0112_1 2:39135c67083d 426 //uint8_t LineIr = 0;
lilac0112_1 2:39135c67083d 427 //uint8_t IrChange[13] ={0x01,0x01,0x02,0x02,0x02,0x04,
lilac0112_1 2:39135c67083d 428 // 0x04,0x04,0x08,0x08,0x08,0x01,0x00};
lilac0112_1 2:39135c67083d 429 /*行動設定*/
lilac0112_1 2:39135c67083d 430 int Power = 0;
lilac0112_1 2:39135c67083d 431 int Rad = 0;//
lilac0112_1 2:39135c67083d 432
lilac0112_1 2:39135c67083d 433
lilac0112_1 2:39135c67083d 434 /*楽しい変数達*/
lilac0112_1 2:39135c67083d 435 int nRad =0;//基礎角
lilac0112_1 2:39135c67083d 436 int addRad = 0;//追加角
lilac0112_1 0:d35efbf4d62e 437
lilac0112_1 0:d35efbf4d62e 438
lilac0112_1 0:d35efbf4d62e 439
lilac0112_1 0:d35efbf4d62e 440 /*関数ポインタ*/
lilac0112_1 2:39135c67083d 441 /*
lilac0112_1 2:39135c67083d 442 void (*AnotherAction[3])(uint8_t [],double);
lilac0112_1 2:39135c67083d 443 AnotherAction[0] = IrFrontAction;
lilac0112_1 2:39135c67083d 444 AnotherAction[1] = IrBackAction;
lilac0112_1 2:39135c67083d 445 AnotherAction[2] = GoHome;
lilac0112_1 2:39135c67083d 446 */
lilac0112_1 2:39135c67083d 447
lilac0112_1 2:39135c67083d 448 loop=1;
lilac0112_1 0:d35efbf4d62e 449
lilac0112_1 2:39135c67083d 450 Mbed.read_data(rx_data, ADDRESS, DATA_NUM);
lilac0112_1 2:39135c67083d 451 Mbed.start_read();
lilac0112_1 0:d35efbf4d62e 452
lilac0112_1 2:39135c67083d 453 SetUp();/*set up routine*/
lilac0112_1 0:d35efbf4d62e 454
lilac0112_1 2:39135c67083d 455 StartLoop(); /*loop strat, switch push end*/
lilac0112_1 2:39135c67083d 456
lilac0112_1 2:39135c67083d 457 loop=0;
lilac0112_1 0:d35efbf4d62e 458
lilac0112_1 2:39135c67083d 459 //Monitor.attach(CompassFB, 0.1);
lilac0112_1 2:39135c67083d 460
lilac0112_1 2:39135c67083d 461 while(0){
lilac0112_1 2:39135c67083d 462 Power = 0;
lilac0112_1 2:39135c67083d 463 Led[0] = 1;
lilac0112_1 2:39135c67083d 464 Rad = 0;
lilac0112_1 2:39135c67083d 465 ReV = 0;
lilac0112_1 2:39135c67083d 466 //SetC = 0.0;
lilac0112_1 2:39135c67083d 467
lilac0112_1 2:39135c67083d 468 Receive();
lilac0112_1 2:39135c67083d 469
lilac0112_1 2:39135c67083d 470 /*白線を読んでいないか確認する*/
lilac0112_1 2:39135c67083d 471 LineData = (~Line+0x00) & 0x0F;
lilac0112_1 2:39135c67083d 472 if(LineData){
lilac0112_1 2:39135c67083d 473 while(LineData){
lilac0112_1 2:39135c67083d 474 move(0,0,0,0);//
lilac0112_1 0:d35efbf4d62e 475 Led[1] = Led[2] = Led[3] = 1;
lilac0112_1 2:39135c67083d 476 LineData = (~Line+0x00) & 0x0F;
lilac0112_1 2:39135c67083d 477 wait_ms(10);
lilac0112_1 0:d35efbf4d62e 478 }
lilac0112_1 0:d35efbf4d62e 479 Led[1] = Led[2] = Led[3] = 0;
lilac0112_1 2:39135c67083d 480
lilac0112_1 0:d35efbf4d62e 481 }
lilac0112_1 0:d35efbf4d62e 482
lilac0112_1 0:d35efbf4d62e 483
lilac0112_1 2:39135c67083d 484 Led[3] = 1;
lilac0112_1 0:d35efbf4d62e 485
lilac0112_1 2:39135c67083d 486 /*if(IrNum == 12){
lilac0112_1 2:39135c67083d 487 move(0,0,ReV,Rad);
lilac0112_1 2:39135c67083d 488 continue;
lilac0112_1 2:39135c67083d 489 }*/
lilac0112_1 2:39135c67083d 490
lilac0112_1 2:39135c67083d 491 //Rad = IrRad();
lilac0112_1 2:39135c67083d 492 //nRad = wrapRad[Rad/15];
lilac0112_1 2:39135c67083d 493 Power = 20;
lilac0112_1 2:39135c67083d 494
lilac0112_1 2:39135c67083d 495
lilac0112_1 2:39135c67083d 496 //Rad = nRad + addRad;
lilac0112_1 0:d35efbf4d62e 497
lilac0112_1 2:39135c67083d 498 //fool(&Rad,&Power);
lilac0112_1 2:39135c67083d 499 move(Power,Power,ReV,Rad);
lilac0112_1 2:39135c67083d 500
lilac0112_1 2:39135c67083d 501 wait_ms(10);
lilac0112_1 2:39135c67083d 502
lilac0112_1 2:39135c67083d 503 Led[0] =0;
lilac0112_1 2:39135c67083d 504
lilac0112_1 2:39135c67083d 505
lilac0112_1 0:d35efbf4d62e 506 }
lilac0112_1 0:d35efbf4d62e 507
lilac0112_1 1:e3248f278663 508
lilac0112_1 0:d35efbf4d62e 509 while(1){
lilac0112_1 0:d35efbf4d62e 510
lilac0112_1 2:39135c67083d 511 //デモプログラム
lilac0112_1 2:39135c67083d 512 Receive();
lilac0112_1 2:39135c67083d 513 wait_ms(50);
lilac0112_1 0:d35efbf4d62e 514 }
lilac0112_1 0:d35efbf4d62e 515
lilac0112_1 2:39135c67083d 516
lilac0112_1 2:39135c67083d 517
lilac0112_1 2:39135c67083d 518
lilac0112_1 0:d35efbf4d62e 519 }