CatPot for defence on RoboCup in 2015 winter
Dependencies: AQM0802A HMC6352 MultiSerial PID Servo mbed
main.txt
- Committer:
- lilac0112_1
- Date:
- 2015-03-14
- Revision:
- 7:81f57b67dff8
- Parent:
- 3:2f74791564c9
File content as of revision 7:81f57b67dff8:
/*********************************** *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 "PID.h" #include "AQM0802A.h" #include "MultiSerial.h" #include "Servo.h" #include "main.h" void Receive(void){ 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]<<0) | (rx_data[8]<<8); /* pc.printf("%d\t", IrData[0]); pc.printf("%d\t", IrData[1]); pc.printf("%d\t", IrData[2]); pc.printf("%d\t", PingData[0]); pc.printf("%d\t", PingData[1]); pc.printf("%d\t", PingData[2]); pc.printf("%d\t", PingData[3]); pc.printf("%d\t", Compass); //pc.printf("%d\t", Mbed.__readSize); pc.printf("\n"); */ if(IrData[0] == 255){ IrNum = 12; return; } IrNum = IrData[0]/12; } void move(int vr,int vl, double vs ,double Rad){ double pwm[4] = {0}; uint8_t i = 0; pwm[0] = vr - vs; pwm[1] = 0; pwm[2] = 0; pwm[3] = vl*(-1.2) + vs; pc.printf("%d\t %d\t %f\t %f\t\n", vr, vl, vs, Rad); 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]; } SetRad = Rad; wait_ms(10); } void fool (int *Rad, int *Power){ static int Last_Rad = 0; static int Last_Vector = 1; int rad = *Rad; int Temp; Temp = Last_Rad % 180; if((Temp>70) &&(Temp<110)){ Temp = *Rad % 180; if((Temp>70) &&(Temp<110)){ Temp = abs(*Rad - Last_Rad); if(Temp>160){ Last_Vector = -1 * Last_Vector;//正転逆転切り替え if(*Rad/180){ *Rad = Angle[Last_Rad%180] -(Last_Rad - *Rad%180); *Power = *Power * Last_Vector; }else{ *Rad = Angle[Last_Rad%180] -(Last_Rad%180 - *Rad); *Power = *Power * Last_Vector; } Last_Rad = rad; return; }else if((Last_Vector+2) == 1){ /*逆転のまま角度拡張*/ if(*Rad/180){ *Rad = -360 + *Rad ; *Power = *Power * Last_Vector; }else{ *Power = *Power * Last_Vector; } Last_Rad = rad; return; }else if((Last_Vector+2) == 3){ /*正転のまま*/ if(*Rad/180){ *Rad = -360 + *Rad ; *Power = *Power * Last_Vector; }else{ *Power = *Power * Last_Vector; Last_Rad = rad; } Last_Rad = rad; return; } } } /*通常動作*/ Last_Vector = RadToVector[(*Rad-1)/90]; *Rad = Angle[*Rad%180]; *Power = *Power * Last_Vector; Last_Rad = rad; } int IrRad(){ /*irの1位の値,2位の場所からirの360へ持っていく*/ uint8_t IrF =IrData[0]/12,IrS = IrData[0]%12; int irrad; irrad = 359 - IrF*30; if(IrS){ if(IrF == 0 ){ if(IrS == 11){ irrad = 15; }else{ irrad = 345; } } irrad = irrad - (IrF - IrS)*15; } return irrad; } void IrFrontAction( uint8_t IrMemo[],double ReV)//ball 12 or 0 o-clock { } void IrBackAction( uint8_t IrMemo[],double ReV)//ball found six o-clock { /*** * 6時にボールがある場合の処理.右と左のスペースを確認して広い方から回り込む * **/ char Ping[2]={0}; //PingReceiveRL(Ping); if(IrMemo[0] == 6){//check ir number return ; } if(Ping[0]<Ping[1]){ /*右が大きい*/ return; } /*左が大きい*/ } void GoHome( uint8_t IrMemo[],double ReV)//Ball is not found. { //止まっとく S555.position(0.0); move(0,0,ReV,0); /*line検知をしないバージョン*/ /* char Ping[2] = {0}; PingReceiveFB(Ping); if(Ping[1] >=60){//後ろからの距離60cm move(-20,-20,ReV); PingReceiveFB(Ping); return ; } move(0,0,ReV);//stop */ } void PidUpdate() { double inputPID = 0.0; Receive(); pid.setSetPoint((int)((REFERENCE + SetAngle) / 1.0)); inputPID = ((Compass / 10) + 540 - CompassDef) % 360;; pid.setProcessValue(inputPID); ReV = -(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(){ /*if(loop==3){ speed[0]=0; speed[1]=0; speed[2]=0; speed[3]=0; Motor.printf("1F0002F0003F0004F000\r\n"); //モータ停止 }else{ Motor.printf("1F0202F0003F0204F000\r\n"); }*/ array(speed[0],speed[1],speed[3],speed[2]); Motor.printf("%s",StringFIN.c_str()); S555.position(SetRad*11/9.0); } 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 for(int i=0; i<15; i++){ wait_ms(10); Receive(); CompassDef = (Compass / 10); PidUpdate(); } } 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=0; return; /*loop end & start*/ } 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)){ Receive(); Lcd.printf("%d\n",((Compass / 10) + 540 - CompassDef) % 360); 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; } } } void CompassFB(){ for(int i=1; i<=18; i++){ if(abs(REFERENCE - Compass) <= i*10){ ReV = (i-1)*2; if(Compass > REFERENCE){ ReV = ReV*(-1); } break; } } } int main() { /*Ir*/ //→.h /*Line*/ uint8_t LineData = 0; /*PID補正move加算値 Revise */ //double ReV = 0.0;→global //double SetC; /*State */ //uint8_t LineIr = 0; //uint8_t IrChange[13] ={0x01,0x01,0x02,0x02,0x02,0x04, // 0x04,0x04,0x08,0x08,0x08,0x01,0x00}; /*行動設定*/ int Power = 0; int Rad = 0;// /*楽しい変数達*/ int nRad =0;//基礎角 int addRad = 0;//追加角 /*関数ポインタ*/ /* void (*AnotherAction[3])(uint8_t [],double); AnotherAction[0] = IrFrontAction; AnotherAction[1] = IrBackAction; AnotherAction[2] = GoHome; */ loop=1; Mbed.read_data(rx_data, ADDRESS, DATA_NUM); Mbed.start_read(); SetUp();/*set up routine*/ StartLoop(); /*loop strat, switch push end*/ loop=0; //Monitor.attach(CompassFB, 0.1); while(0){ Power = 0; Led[0] = 1; Rad = 0; ReV = 0; //SetC = 0.0; Receive(); /*白線を読んでいないか確認する*/ LineData = (~Line+0x00) & 0x0F; if(LineData){ while(LineData){ move(0,0,0,0);// Led[1] = Led[2] = Led[3] = 1; LineData = (~Line+0x00) & 0x0F; wait_ms(10); } Led[1] = Led[2] = Led[3] = 0; } Led[3] = 1; /*if(IrNum == 12){ move(0,0,ReV,Rad); continue; }*/ //Rad = IrRad(); //nRad = wrapRad[Rad/15]; Power = 20; //Rad = nRad + addRad; //fool(&Rad,&Power); move(Power,Power,ReV,Rad); wait_ms(10); Led[0] =0; } while(1){ //デモプログラム Receive(); wait_ms(50); } }