2
Dependencies: AQM0802A HMC6352 PID Servo mbed
main.cpp
- Committer:
- ryuna
- Date:
- 2015-03-03
- Revision:
- 4:f7946508daa8
- Parent:
- 3:0c994b3a882e
- Child:
- 5:dace4f3b6e4a
File content as of revision 4:f7946508daa8:
/*********************************** *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 "HMC6352.h" #include "Servo.h" #include "main.h" void Receive(uint8_t Address , char kind,char output[],int num ){ char order[1] = {kind}; bool val; val = Sensor.write(Address&0xFE, order , 1); Led[0] = 0; Led[1] = !val; //wait_ms(.25); val = Sensor.read(Address|1, output, num);// IRデータを受信 Led[0] = !val; Led[1] = 0; } void ReceiveFast(uint8_t Address, char kind, char output[],int num){ /*初めに相手に何が欲しいのか言わない *要は全部よこせということ *上位3つだとirのみ *上位5つだとirとping * *[0]:ir num num *[1]:ir max_data *[2]:ir second_max_data *[3]:ping *[4]:ping **/ bool val; val = Sensor.read(Address|1,output,num); Led[0] = !val; Led[1] = 0; } uint8_t IrReceive(uint8_t IrData[]){ /* *Irdata[0] : 1位,2位の場所(1位*13+2位) *Irdata[1] : 1位の値 *return : 1位の場所 */ char data_r[3] = {0},data_l[3] = {0}; ReceiveFast(ADDRESS_R,BALL,data_r,3); wait_ms(0.5); ReceiveFast(ADDRESS_L,BALL,data_l,3); if((data_r[0] == 125)||(data_l[0] == 125)){/*ボールを検知しているかチェック*/ if((data_r[0] == 125)&&(data_l[0] == 125)){ IrData[0] = 12*13 + 12; return 12; } if(data_r[0] == 125){ IrData[0] = (data_l[0]/6+6)*13 + (data_l[0]%6+6); IrData[1] = data_l[1]; return (data_l[0]/6)+6; } IrData[0] = (data_r[0]/6)*13 + (data_r[0]%6); IrData[1] = data_r[1]; return data_r[0]/6; } if(data_r[2]<data_l[2]){ if(data_r[2]<data_l[1]){ IrData[0] = (data_r[0]/6)*13 + (data_r[0]%6); IrData[1] = data_r[1]; return data_r[0]/6; } IrData[0] = (data_r[0]/6)*13 + (data_l[0]/6+6); IrData[1] = data_r[1]; return data_r[0]/6; }else{ if(data_l[2]<data_r[1]){ IrData[0] = (data_l[0]/6+6)*13 + (data_l[0]%6+6); IrData[1] = data_l[1]; return (data_l[0]/6+6); } IrData[0] = (data_l[0]/6+6)*13 + (data_r[0]/6); IrData[1] = data_l[1]; return (data_l[0]/6+6); } } uint8_t IrReceiveFast(){ /* *return : 1位の場所 */ char data_r[3] = {0},data_l[3] = {0}; ReceiveFast(ADDRESS_R,BALL,data_r,3); wait_ms(0.5); ReceiveFast(ADDRESS_L,BALL,data_l,3); if((data_r[0] == 125)||(data_l[0] == 125)){/*ボールを検知しているかチェック*/ if((data_r[0] == 125)&&(data_l[0] == 125)){ return 12; } if(data_r[0] == 125){ return (data_l[0]/6)+6; } return data_r[0]/6; } if(data_r[1]<data_l[1]){ return data_r[0]/6; }else{ return (data_l[0]/6+6); } } void PingReceiveRL(char ping[]){ /* * ping[0] : Right ping data * ping[1] : Left ping data */ Receive(ADDRESS_R,SONIC,ping,2); } void PingReceiveFB(char ping[]){ /* * ping[0] : FRONT ping data * ping[1] : BACK ping data */ Receive(ADDRESS_L,SONIC,ping,2); } void move(int vr,int vl, double vs ){ double pwm[4] = {0}; uint8_t i = 0; pwm[0] = -vr + vs; pwm[1] = 0; pwm[2] = 0; pwm[3] = vl + 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]; } } char LazyBum(unsigned int Rad){//怠け者 /* * Radは角度(0~360), * Lastには過去の角度(0~360) * * 最終的には+-90度で表すことになる. */ /* static unsigned int Last;//static付けて宣言すると勝手に0で初期化されるハズ.... static char Rotation = CW;//CW or CCW char trans[4] = {1,-1,1,-1}; unsigned int temp,temp0 ; temp = Rad%180; if((temp > 70)&&(temp < 110)){ temp0 = Last%180; if((temp0 > 70)&&(temp0 < 110)){ if(abs(Rad - Last)<40){{ if(Rad<110){ S555.position(Rad*1.0); Last = Rad; Rotation = trans[Rad/110]; return; } S555.position((-360+Rad)*1.0); Last = Rad; Rotation = 1; return Rotation; } if(Last<110){ S555.postiton((-360+Rad)*-1.0); Last = Rad; Rotation = trans[(90+Rad)/90]; return Rotation; } S555.postiton(Rad*-1.0); Last = Rad; Rotation = trans[(90+Rad)/90]; return Rotation; } } if(temp < 90){ S555.position((Rad%90)*1.0); Rotation = [Rad/90]; Last = Rad; return Rotation; } S555.position((Rad%90- 180)*1.0); Last = Rad; Rotation = [Rad/90]; return Rotation; */ return 0; } void IrFrontAction(int *CompassDef , uint8_t IrMemo[],double ReV)//ball 12 or 0 o-clock { } void IrBackAction(int *CompassDef , uint8_t IrMemo[],double ReV)//ball found six o-clock { /*** * 6時にボールがある場合の処理.右と左のスペースを確認して広い方から回り込む * **/ char Ping[2]; PingReceiveRL(Ping); if(IrMemo[0] == 6){//check ir number return ; } if(Ping[0]<Ping[1]){ /*右が大きい*/ return; } /*左が大きい*/ } void GoHome(int *CompassDef , uint8_t IrMemo[],double ReV)//Ball is not found. { //止まっとく S555.position(0.0); move(0,0,ReV); /*line検知をしないバージョン*/ /* char Ping[2] = {0}; PingReceiveFB(Ping); if(Ping[1] >=60){//後ろからの距離60cm move(-20,-20,ReV); PingReceiveFB(Ping); return ; } move(0,0,ReV);//stop */ } double PidUpdate(int CompassDef , double SetAngle) { int Compass; double inputPID = 0.0; Compass = ((hmc6352.sample() / 10) + 540 - CompassDef) % 360; pid.setSetPoint((int)((REFERENCE + SetAngle) / 1.0)); inputPID = Compass; pid.setProcessValue(inputPID); return -(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 == Calibration ||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(int *CompassDef){ /*初期化*/ double ReV; int i; 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 Motor.baud(115200); //ボーレート設定 Motor.printf("1F0002F0003F0004F000\r\n"); //モータ停止 Motor.attach(&tx_motor,Serial::TxIrq); //送信空き割り込み(モータ用) //move(0,0,0);//停止 S555.calibrate(0.0005, 120.0); S555.position(0.0); //初期位置にセット hmc6352.setOpMode(HMC6352_CONTINUOUS, 1, 20); *CompassDef = (hmc6352.sample() / 10); for(i = 0;i<10;i++){ ReV = PidUpdate(*CompassDef,0); } Lcd.printf("%f\n",ReV); wait_ms(100); } void StartLoop(int *CompassDef){ /* *スイッチが押されるまでロボットはスタートしない. * *switch割り当て *1.コンパスのキャリブレーション実行スイッチ *2.キッカーのキック(check用) *3,4.自由 *5.StartSw */ uint8_t State = 0; int Compass = 0; uint8_t IrData[3] = {0}; uint8_t Irnum = 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)){ /*Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360; Lcd.printf("%d\n",Compass); */ LineData = (~Line+0x00) & 0x0F; Lcd.printf("%d\n",LineData); wait_ms(100); State = SwRead(); } Lcd.cls(); continue; /* debug command free * * decide movement of the beginning. * **/ } if(State == Debug2){ while((State == Debug2)){ Irnum = IrReceive(IrData); Lcd.printf("%d\n",Irnum); wait_ms(100); State = SwRead(); } Lcd.cls(); continue; /* debug command free * *display out to selected 3 menus. compass, ir, ping, line,etc * **/ } /* if(State == Debug3){ while((State == Debug3)){ Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360; Lcd.printf("%d\n",Compass); State = SwRead(); wait_ms(100); } Lcd.cls(); */ /* debug command free * *display out to selected 3 menus. compass, ir, ping, line,etc * **/ //} if(State == Kicker){ while((State == Kicker)){ Compass = ((hmc6352.sample() / 10) + 540 - *CompassDef) % 360; Lcd.printf("%d\n",Compass); wait_ms(100); State = SwRead(); } Lcd.cls(); /*kicker check */ /* Led[4] = 1; Kick = 1; wait(0.5); Kick = 0; Led[4] = 0; wait(1); */ continue; } if(State == Calibration){ Led[0] = Led[1] = Led[2] = 0; hmc6352.setCalibrationMode(ENTER); while((State == Calibration)){ State = SwRead(); } hmc6352.setCalibrationMode(EXIT); wait(0.3);//必要 Led[3] = 0; /*calibration command enter*/ continue; } } } int main() { /*Ir*/ uint8_t IrNum = 12;//場所によるirの数を表したもの0~11まではボールがある状態12はボールがない状態 //uint8_t IrNumOld = 0;//過去値 uint8_t IrData[2];//0:1位*13+2位,1:1位の値 /*Line*/ uint8_t LineData = 0; /*Compass*/ int CompassDef = 0; //int Compass = 0; double SetAngle = 0; /*PID補正move加算値 Revise */ double ReV = 0.0; /*State */ //Direction LineIr = NA;//方位設定奴...エラーでてめんどくさいので後でまたやることにする。 uint8_t LineIr = 0; uint8_t IrChange[13] ={0x01,0x01,0x02,0x02,0x02,0x04, 0x04,0x04,0x08,0x08,0x08,0x01,0x00}; /*関数ポインタ*/ void (*AnotherAction[3])(int *, uint8_t [],double); AnotherAction[0] = IrFrontAction; AnotherAction[1] = IrBackAction; AnotherAction[2] = GoHome; /*check 変数*/ double s_deg[13] = { 0.0,-30.0,-60.0,-90.0, 60.0, 30.0, 0.0,-30.0,-60.0, 90.0, 60.0, 30.0, 0.0}; int vr[13] = { 20,20,20,20,-20,-20,-20,-20,-20,20,20,20,0}; int vl[13] = { 20,20,20,20,-20,-20,-20,-20,-20,20,20,20,0}; SetUp(&CompassDef);/*set up routine*/ StartLoop(&CompassDef); /*loop strat, switch push end*/ while(1){ Led[0] = 1; /*白線を読んでいないか確認する*/ /* LineData = (~Line+0x00) & 0x0F; if(LineData){ IrNum = IrReceiveFast(); LineIr = LineData & IrChange[IrNum]; //一箇所でも一致すればlineを検知している. while(LineIr){ move(0,0,0);// Led[1] = Led[2] = Led[3] = 1; LineData = (~Line+0x00) & 0x0F; IrNum = IrReceiveFast(); LineIr = LineData & IrChange[IrNum];//一箇所でも一致すればlineを検知している. } Led[1] = Led[2] = Led[3] = 0; continue; } */ IrNum = IrReceive(IrData); ReV = 0;//PidUpdate(CompassDef ,SetAngle); move(vr[IrNum],vl[IrNum],ReV); wait_ms(10); S555.position(s_deg[IrNum]); Led[0] = 0; wait_ms(200); } /* while(0){ //デモプログラム S555.position(0.0); wait(1.5); S555.position(90.0); } */ }