version 3 通信方式,マイコン等に変更あり
Dependencies: AQM0802A PID Servo mbed
Diff: main.cpp
- Revision:
- 0:65b9e62cc2b6
- Child:
- 1:f91d53098d57
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Mar 06 01:16:05 2015 +0000 @@ -0,0 +1,463 @@ +/*********************************** + *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(){ + 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){ + 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; + + 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() +{ + + //Compass = ((hmc6352.sample() / 10) + 540 - CompassDef) % 360; + wait_ms(10); + + //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()); + S555.position(SetRad); +} + +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 + + Mbed.read_data(rx_data, ADDRESS); + Mbed.start_read(); + + +} +void StartLoop(){ + + uint8_t State = 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)){ + LineData = (~Line+0x00) & 0x0F; + Lcd.printf("%d\n",LineData); + + wait_ms(100); + State = SwRead(); + } + Lcd.cls(); + continue; + + } + if(State == Debug2){ + while((State == Debug2)){ + //Irnum = IrReceiveFast(); + 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() { + + /*Ir*/ + uint8_t IrNum = 12;//場所によるirの数を表したもの0~11まではボールがある状態12はボールがない状態 + //uint8_t IrNumOld = 0;//過去値 + /*Line*/ + uint8_t LineData = 0; + + + /*PID補正move加算値 Revise */ + double ReV = 0.0; + 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; + */ + SetUp();/*set up routine*/ + StartLoop(); /*loop strat, switch push end*/ + + while(1){ + 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(0){ + //デモプログラム + S555.position(0.0); + + wait(1.5); + S555.position(90.0); + + } + + */ + + +}