![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
CatPot for defence on RoboCup in 2015 winter
Dependencies: AQM0802A HMC6352 MultiSerial PID Servo mbed
main.cpp
- Committer:
- lilac0112_1
- Date:
- 2015-03-13
- Revision:
- 4:7c488c059498
- Parent:
- 3:2f74791564c9
- Child:
- 5:c8e4d5cdc353
File content as of revision 4:7c488c059498:
/*********************************** *RoboCupJunior Soccer B Open 2015 *Koshinnestu progrum * * * データからロボットの移動やキッカー等のモータの動作を決定する処理を行う * * MotorDriverにmaxonに命令 * * servoにステアリング指示 * * LCDでデバック * * スイッチ4つとスタートスイッチで処理を実行 * ************************* * Pin Map * * p5~p8 >> BusIn >> LineSensor * * p9,p10 >> I2C >> LPC1114FN28/102 read & Compass * * p13,p14 >> Serial >> Motor * * p21 >> PwmOut >> Servo * * p22~p26 >> DigitalIn >> DebugSw and StartSw * * p27,p28 >> I2C >> DebugLCD * * p29 >> DigitalOut >> Kicker * * *never use pin number p11,p12,p15,p16,p17,p18,p19,p20,p30 * * ******************************/ #include "mbed.h" #include <math.h> #include <sstream> #include "Servo.h" #include "PID.h" #include "AQM0802A.h" #include "main.h" //#include "MultiSerial.h" /* void Receive(){ IrData[0] = rx_data[0]; IrData[1] = rx_data[1]; IrData[2] = rx_data[2]; PingData[0] = rx_data[3]; PingData[1] = rx_data[4]; PingData[2] = rx_data[5]; PingData[3] = rx_data[6]; if((IrData[0] == 255)||(IrData[1] == 255)||(IrData[2] == 255)){ IrNum = 12; return; } IrNum = IrData[0]/12; d } */ void Interval(void){ WaitFlag=0; } void move(int vr,int vl, double vs ,int Degree){ double pwm[4] = {0}; uint8_t i = 0; double CompassVias=15.0; double CompassDif; vr=vr*( 1.0); vl=vl*(-1.0); Degree = Degree*(-1.0); CompassDif=(((Compass / 10) + 540 - CompassDef) % 360) - REFERENCE; if(abs(CompassDif)<CompassVias){ CompassVias=15.0; pwm[0] = LastPwm[0] = vr; pwm[1] = LastPwm[1] =0; pwm[2] = LastPwm[2] =0; pwm[3] = LastPwm[3] =vl*(1.0); LastDeg = Degree; } else{ CompassVias=10.0; if(CompassDif<0){ //vs = vs - (20-CompassVias); vs = -(15+(abs(CompassDif))*0.1); if( abs(CompassDif) >= 100) vs = -(20+(100)*0.1); } else{ //vs = vs + (20-CompassVias); vs = (15+(abs(CompassDif))*0.1); if( abs(CompassDif) >= 100) vs = (20+(100)*0.1); } pwm[0] = vs; pwm[1] = 0; pwm[2] = 0; pwm[3] = vs*(-1)*(-1); Degree = 0.0; } if(Loop==1){ pwm[0] = 0; pwm[1] = 0; pwm[2] = 0; pwm[3] = 0; } for(i = 0; i < 4; i++){ if(pwm[i] > 100){ pwm[i] = 100; }else if(pwm[i] < -100){ pwm[i] = -100; } speed[i] = pwm[i]; } if(Degree > 110){ Degree = 110; }else if(Degree < -110){ Degree = -110; } if(WaitFlag==1) Degree = LastDegree; SetDegree = LastDegree = Degree; S555.position(SetDegree*11.0/9.0); if(WaitFlag==0){ WaitFlag = 1; STimer.attach(Interval, WAIT_TIME); } } void moving(int IrNumber){ move(vrl[IrNumber],vrl[IrNumber],0,IrServo[IrNumber]); } uint8_t PingChange(uint8_t LineData){ static uint8_t Last_Line; static uint8_t Last_Ping; uint8_t LinePing = 0; if(!LineData){ return 0; } if(PingData[0] <20) LinePing = LinePing + 8; if(PingData[1] <20) LinePing = LinePing + 2; if(PingData[2] <20) LinePing = LinePing + 4; if(PingData[3] <20) LinePing = LinePing + 1; if(LinePing&0x01){ if((LineData&0x01) ||(Last_Line&0x01)||(Last_Ping&0x01)){ Last_Ping = LinePing; Last_Line = LineData; return 1; } } if(LinePing&0x02){ if((LineData&0x02) ||(Last_Line&0x02)||(Last_Ping&0x02)){ Last_Ping = LinePing; Last_Line = LineData; return 2; } } if(LinePing&0x04){ if((LineData&0x04) ||(Last_Line&0x04)||(Last_Ping&0x04)){ Last_Ping = LinePing; Last_Line = LineData; return 4; } } if(LinePing&0x08){ if((LineData&0x08) ||(Last_Line&0x08)||(Last_Ping&0x08)){ Last_Ping = LinePing; Last_Line = LineData; return 8; } } Last_Ping = 0; Last_Line = 0; return 0; } void fool (int *Degree, int *Power){ static int Last_Degree = 0; static int Last_Vector = 1; int degree = *Degree; int Temp; if((*Degree <0)||(*Degree >=360)){ *Degree = 0; Last_Degree = 0; Last_Vector = 1; return ; } Temp = Last_Degree % 180; if((Temp==90)){ Temp = *Degree % 180; if((Temp==90)){ Temp = abs(*Degree - Last_Degree); if(Temp>160){ Last_Vector = -1 * Last_Vector;//正転逆転切り替え if(*Degree/180){ *Degree = Angle[Last_Degree%180] -(Last_Degree - *Degree%180); *Power = *Power * Last_Vector; }else{ *Degree = Angle[Last_Degree%180] -(Last_Degree%180 - *Degree); *Power = *Power * Last_Vector; } Last_Degree = degree; if((*Degree <= -120)||(*Degree >=120)){ *Degree = 0; } return; }else if((Last_Vector+2) == 1){ /*逆転のまま角度拡張*/ if(*Degree/180){ *Degree = -360 + *Degree ; } *Power = *Power * Last_Vector; Last_Degree = degree; if((*Degree <= -120)||(*Degree >=120)){ *Degree = 0; } return; }else if((Last_Vector+2) == 3){ /*正転のまま*/ if(*Degree/180){ *Degree = -360 + *Degree ; } *Power = *Power * Last_Vector; Last_Degree = degree; if((*Degree <= -120)||(*Degree >=120)){ *Degree = 0; } return; } } } /*通常動作*/ if(*Degree == 0){ Last_Vector = DegreeToVector[0]; *Degree = Angle[*Degree%180]; *Power = *Power * Last_Vector; Last_Degree = degree; return ; } if(*Degree == 270){ Last_Vector = DegreeToVector[0]; *Degree = -90; *Power = *Power * Last_Vector; Last_Degree = degree; return ; } Last_Vector = DegreeToVector[(*Degree)/90]; *Degree = Angle[*Degree%180]; *Power = *Power * Last_Vector; Last_Degree = degree; if((*Degree <= -120)||(*Degree >=120)){ *Degree = 0; } } int IrDegree(){ /*irの1位の値,2位の場所からirの360へ持っていく*/ uint8_t IrF ,IrS; unsigned int irdegree = 0; if(IrNum >=12){ return 0; } IrF = IrData[0]/12; IrS = IrData[0]%12; if(IrF == 0 ){ if(IrS == 11){ irdegree = 15; }else if(IrS == 1){ irdegree = 345; } return irdegree; } irdegree = 360 - IrF*30; if(IrS&&(abs(IrF-IrS) == 1)){ irdegree = irdegree - (IrF - IrS)*15; } if(irdegree>=360){ return 0; } return irdegree; } void IrFrontAction()//ball 12 or 0 o-clock { if(IrData[1]>70){ if(abs(CompassPID)>10){ move(0,0,CompassPID,0); return; } move(30,30,CompassPID,0); return; } if(IrData[1]>60){ move(25,25,CompassPID,0); return; } /*IrData[1]>500*/ if(PingData[0]<50){ if((PingData[1]<60)&&(PingData[1]>40)){ /*右側に居る*/ move(25,40,0,10); wait_ms(100); move(30,30,0,0); return; } if((PingData[1]<60)&&(PingData[1]>40)){ /*左側に居る*/ move(40,25,0,-10); wait_ms(100); move(30,30,0,0); return; } /*それ以外*/ move(10,10,CompassPID,0); //Receive(); if(!(IrNum == 0)) return; move(40,40,CompassPID,0); return ; } move(20,20,CompassPID,0); } void IrBackAction()//ball found six o-clock { /*** * 6時にボールがある場合の処理.右と左のスペースを確認して広い方から回り込む * **/ if(PingData[1]>PingData[3]){ /*右が大きい*/ if(IrData[1]>70){ move(-20,-20,CompassPID,45); return; } if(IrData[1]>60){ move(-20,-20,CompassPID,60); return; } move(-20,-20,CompassPID,80); return; } /*左が大きい*/ if(IrData[1]>70){ move(-20,-20,CompassPID,-45); return; } if(IrData[1]>60){ move(-20,-20,CompassPID,-60); return; } move(-20,-20,CompassPID,-80); return; } void GoHome()//Ball is not found. { static bool State[20]={0}; /* State 0 1 2 3 4 5 6 */ if( ( ( ( (Compass / 10) + 540 - CompassDef ) % 360 ) - REFERENCE ) >10.0 ){ move(0,0,0,0); return; } for(int i=0; i<20; i++) State[i]=0; if(PingData[2] >=60) State[0]=1; if(PingData[2] <=40) State[1]=1; if(PingData[2] >=90-10) State[5]=1; if(PingData[2] <=40+10) State[6]=1; if(PingData[2] >=10) State[7]=1; if(PingData[2] <=5) State[8]=1; if(PingData[2] >=25-5) State[9]=1; if(PingData[2] <=5+5) State[10]=1; if(PingData[0]>PingData[1]) State[2]=1; if( (PingData[0]+PingData[1]) >= 120 ) State[3]=1; if( (abs( int( int(PingData[0]) - int(PingData[1]) ) ) ) <= 10 ) State[4]=1; /*if(State[0]){ move(vrl[6],vrl[6],CompassPID,IrServo[6]); return ; }*/ if(State[4]){ if(State[3]){ if((State[7])==(State[8])){ move(0,0,CompassPID,LastDegree); return ; } if(State[7]){ move(vrl[6],vrl[6],CompassPID,IrServo[6]); return ; } if(State[8]){ move(vrl[0],vrl[0],CompassPID,IrServo[0]); return ; } } if((State[0])==(State[1])){ move(0,0,CompassPID,LastDegree); return ; } if(State[0]){ move(vrl[6],vrl[6],CompassPID,IrServo[6]); return ; } if(State[1]){ move(vrl[0],vrl[0],CompassPID,IrServo[0]); return ; } }else{ if(State[2]){ move(vrl[9],vrl[9],CompassPID,IrServo[9]); return ; } else{ move(vrl[3],vrl[3],CompassPID,IrServo[3]); return ; } } } void PidUpdate() { pid.setSetPoint((int)((REFERENCE + SetC) / 1.0)); InputPID = ((Compass/10) + 540 - CompassDef) % 360; pid.setProcessValue(InputPID); CompassPID = (pid.compute()); } uint8_t SwRead(){ /****** *retrun : sw_state *StartS = 0x01; *Debug2 = 0x02; *Debug1 = 0x04; *Debug3 = 0x06; *Kicker = 0x08; *Calibration = 0x10; * *****/ uint8_t i,temp,temp2; temp = ~Sw - 224; if(!(temp == Kicker ||temp == Debug1 ||temp == Debug2 ||temp == Debug3 ||temp == Debug12 ||temp == Debug23 ||temp == StartS)) return 0;/*スイッチが押されていない*/ if(!(temp == 0x00)){ for(i = 0; i < 50; i++); temp2 = ~Sw - 224; if(temp == temp2){ return temp; } } return 0; } //通信(モータ用) void tx_motor(){ /* speed[0]=0; speed[1]=0; speed[2]=0; speed[3]=0; */ array(speed[0],speed[1],speed[3],speed[2]); Motor.printf("%s",StringFIN.c_str()); } void SetUp(){ /*初期化*/ Motor.baud(115200); //ボーレート設定 Motor.printf("1F0002F0003F0004F000\r\n"); //モータ停止 Motor.attach(&tx_motor,Serial::TxIrq); //送信空き割り込み(モータ用) Mbed.attach(&micon_rx,Serial::RxIrq); //送信空き割り込み(センサ用) S555.calibrate(0.0006, 120.0); //S555.position(0.0); //初期位置にセット move(0,0,0,0);//停止 Kick = 0; Sw.mode(PullUp); pid.setInputLimits(MINIMUM,MAXIMUM); //pid sed def pid.setOutputLimits(-OUT_LIMIT, OUT_LIMIT); //pid sed def pid.setBias(PID_BIAS); //pid sed def pid.setMode(AUTO_MODE); //pid sed def pid.setSetPoint(REFERENCE); //pid sed def for(int i=0; i<15; i++){ CompassDef = (Compass / 10); wait_ms(10); } //pidupdate.attach(&PidUpdate, PID_CYCLE); } void StartLoop(){ uint8_t State = 0; uint8_t LineData = 0; while(1){ Led[0] = Led[1] = Led[2] = Led[3] = 1; //Lcd.cls(); State = SwRead(); if(State == 0) continue; if(State == StartS){ /*loop end & start*/ return; } if(State == Debug1){ while((State == Debug1)){ LineData = (~Line+0x00) & 0x0F; Lcd.printf("%d\n",LineData); wait_ms(100); State = SwRead(); } Lcd.cls(); continue; } if(State == Debug2){ while((State == Debug2)){ //Receive(); Lcd.printf("%d\n",IrNum); //pc.printf("%d\t %d\t %d\t %d\t %d\t %d\t\n",IrData[0],IrData[1],IrData[2],PingData[0],PingData[1],PingData[2]); wait_ms(100); State = SwRead(); } Lcd.cls(); continue; } if(State == Debug3){ while((State == Debug3)){ Lcd.printf("%d\n",((Compass / 10) + 540 - CompassDef) % 360); wait_ms(100); State = SwRead(); } Lcd.cls(); continue; } if(State == Debug12){ while((State == Debug12)){ Lcd.printf("%d\n", PingData[0]);//Left Lcd.printf("%d\n", PingData[1]);//Right wait_ms(100); State = SwRead(); } Lcd.cls(); continue; } if(State == Debug23){ while((State == Debug23)){ Lcd.printf("%d\n", PingData[2]);//Back Lcd.printf("%d\n", PingData[3]);//Front wait_ms(100); State = SwRead(); } Lcd.cls(); continue; } if(State == Kicker){ while((State == Kicker)){ Lcd.printf("%f\n",CompassPID); wait_ms(100); State = SwRead(); } Lcd.cls(); continue; /*Led[0] = Led[1] = Led[2] = 0; Kick = 1; wait_ms(500); Kick = 0; while((State == Kicker)){ wait_ms(100); State = SwRead(); } continue;*/ } } } uint8_t LinkBit(bool eight, bool seven, bool six, bool five, bool fore, bool three, bool two, bool one){ return 0x80*eight + 0x40*seven + 0x20*six + 0x10*five + 0x08*fore + 0x04*three + 0x02*two + 0x01*one; } int main() { //uint8_t IrNumOld = 0;//過去値 /*Line*/ uint8_t LineData = 0; //uint8_t LinePing = 0; /*State */ /*uint8_t LineIr = 0; uint8_t IrChange[13] ={0x01,0x01,0x03,0x02,0x02,0x06, 0x04,0x04,0x0B,0x08,0x08,0x09,0x00}; */ /*行動設定*/ //int Power = 0; //int Degree = 0; /*楽しい変数達*/ //int nDegree =0;//基礎角 //int addDegree = 0;//追加角 /**/ //int Gap=2; bool LineState[10]={0}; /*関数ポインタ*/ //void (*AnotherAction[3])(uint8_t [],double); /*void (*AnotherAction[3])(); AnotherAction[0] = IrFrontAction; AnotherAction[1] = IrBackAction; AnotherAction[2] = GoHome;*/ SetUp();/*set up routine*/ StartLoop(); /*loop strat, switch push end*/ Led[0] = Led[1] = Led[2] = Led[3] = 0; wait_ms(100); Loop = 0; while(1){ if( ( ( ( (Compass / 10) + 540 - CompassDef ) % 360 ) - REFERENCE ) >15.0 ){ move(0,0,0,0); continue; } LineData = (~Line+0x00) & 0x0F; for(int i=0; i<10; i++) LineState[i]=0; if(LineData!=0){ if(PingData[0]>PingData[1]) LineState[0]=1; if(PingData[2]<=20) LineState[1]=1; switch(LinkBit(0,0,0,0,0,0,LineState[1],LineState[0])&0x03){ case 0: moving(3); break; case 1: moving(9); break; case 2: moving(1); break; case 3: moving(11); break; default : break; } /*if(LastPwm[0]==0){ } move(LastPwm[0]*(-1),LastPwm[3]*(-1)*(-1),0,LastDeg);*/ wait(0.5); continue; }else{ if(PingData[0]<=5) LineState[2]=1; if(PingData[1]<=5) LineState[3]=1; if(PingData[2]<=5) LineState[4]=1; switch(LinkBit(0,0,0,0,0,LineState[4],LineState[3],LineState[2])&0x07){ case 0: break; case 1: moving(3); break; case 2: moving(9); break; case 3: moving(6); break; case 4: moving(0); break; case 5: moving(1); break; case 6: moving(11); break; case 7: moving(0); break; default : break; } if((LinkBit(0,0,0,0,0,LineState[4],LineState[3],LineState[2])&0x07)!=0){ wait(0.2); continue; } } if(PingData[2]>=90){ moving(6); wait(0.2); continue; } else{ moving(12); continue; } } /* Gap=2; switch(IrNum){ case 0: case 1: case 2: case 3: moving(IrNum); break; case 4: case 5: moving(IrNum+Gap); break; case 6: if(PingData[0]>PingData[1]) moving(IrNum+Gap); else moving(IrNum-Gap); break; case 7: case 8: moving(IrNum-Gap); break; case 9: case 10: case 11: moving(IrNum); break; case 12: GoHome(); break; } */ }