version 3 通信方式,マイコン等に変更あり
Dependencies: AQM0802A PID Servo mbed
main.cpp
- Committer:
- ryuna
- Date:
- 2015-03-10
- Revision:
- 2:e84bb87eea71
- Parent:
- 1:f91d53098d57
- Child:
- 3:4a39486ff238
File content as of revision 2:e84bb87eea71:
/*********************************** *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 "MultiSerial.h" #include "PID.h" #include "AQM0802A.h" #include "Servo.h" #include "main.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]; Compass = rx_data[7]+rx_data[8]; if((IrData[0] == 255)||(IrData[1] == 255)||(IrData[2] == 255)){ IrNum = 12; return; } IrNum = IrData[0]/12; } void move(int vr,int vl, double vs ,int Degree){ double pwm[4] = {0}; uint8_t i = 0; pwm[0] = vr - vs; pwm[1] = 0; pwm[2] = 0; pwm[3] = vl*1.1 + vs; 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; } SetDegree = Degree; S555.position(SetDegree); wait_ms(10); } 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] <40) LinePing = LinePing + 1; if(PingData[1] <40) LinePing = LinePing + 2; if(PingData[2] <40) LinePing = LinePing + 4; if(PingData[3] <40) LinePing = LinePing + 8; 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 ; } Last_Vector = DegreeToVector[(*Degree-1)/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]>700){ if(abs(CompassPID)>10){ move(0,0,CompassPID,0); return; } move(30,30,CompassPID,0); return; } if(IrData[1]>600){ 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]>700){ move(-20,-20,CompassPID,45); return; } if(IrData[1]>600){ move(-20,-20,CompassPID,60); return; } move(-20,-20,CompassPID,90); return; } /*左が大きい*/ if(IrData[1]>700){ move(-20,-20,CompassPID,-45); return; } if(IrData[1]>600){ move(-20,-20,CompassPID,-60); return; } move(-20,-20,CompassPID,-90); return; } void GoHome()//Ball is not found. { if(PingData[2] >=60){//後ろからの距離60cm move(-20,-20,CompassPID,0); return ; } move(0,0,CompassPID,0);//stop } void PidUpdate() { pid.setSetPoint((int)((REFERENCE + SetC) / 1.0)); InputPID = Compass; 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 == 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(){ 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); //送信空き割り込み(モータ用) S555.calibrate(0.0005, 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 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); wait_ms(100); State = SwRead(); } Lcd.cls(); continue; } if(State == Debug3){ while((State == Debug3)){ Lcd.printf("%d\n",Compass); wait_ms(100); State = SwRead(); } Lcd.cls(); continue; } if(State == Kicker){ Led[0] = Led[1] = Led[2] = 0; move(20,20,0,0); while((State == Kicker)){ wait_ms(100); State = SwRead(); } move(0,0,0,0); continue; } } } 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;//追加角 /*関数ポインタ*/ //void (*AnotherAction[3])(uint8_t [],double); void (*AnotherAction[3])(); AnotherAction[0] = IrFrontAction; AnotherAction[1] = IrBackAction; AnotherAction[2] = GoHome; SetUp();/*set up routine*/ Mbed.read_data(rx_data, ADDRESS); Mbed.start_read(); //Mbed.check_rx_wait(); StartLoop(); /*loop strat, switch push end*/ Led[0] = Led[1] = Led[2] = Led[3] = 0; wait_ms(100); while(0){ Receive(); //Lcd.printf("%d\n",IrNum); /*白線を読んでいないか確認する*/ LineData = (~Line+0x00) & 0x0F; if(LineData){ LineIr = LineData & IrChange[IrNum]; LinePing = PingChange(LineData); if(LineIr){ move(0,0,0,0); while(LineIr){ Led[1] = Led[2] = Led[3] = 1; Receive(); LineData = (~Line+0x00) & 0x0F; LineIr = LineData & IrChange[IrNum]; wait_ms(10); } }else if(LinePing){ while(LinePing){ Led[1] = Led[2] = Led[3] = 1; Receive(); LineData = (~Line+0x00) & 0x0F; LinePing = PingChange(LineData); wait_ms(10); } } Led[1] = Led[2] = Led[3] = 0; } Power = 0; Led[0] = 1; Degree = 0; SetC = 0.0; Led[3] = 1; Receive(); Degree = IrDegree(); if((Degree == 0)||(Degree == 180)||(IrNum == 12)){ (AnotherAction[IrNum/6])(); continue; } /* if(IrNum == 12){ move(0,0,0,0); wait_ms(10); continue; }*/ nDegree = wrapDegree[Degree/15]; Power = 20; Degree = nDegree + addDegree; if((Degree <0)||(Degree>=360)){ Degree = 0; } fool(&Degree,&Power); move(Power,Power,0,Degree); //wait_ms(500); Led[0] =0; wait_ms(10); } int i; while(1){ //デモプログラム Receive(); pc.printf("%d\t %d\t %d\t %d\t %d\t %d\t\n",rx_data[3],rx_data[4],rx_data[5],rx_data[6],rx_data[7],rx_data[8]); wait(0.1); } }