version 3 通信方式,マイコン等に変更あり
Dependencies: AQM0802A PID Servo mbed
Diff: main.cpp
- Revision:
- 3:4a39486ff238
- Parent:
- 2:e84bb87eea71
- Child:
- 4:2857f273a7f4
--- a/main.cpp Tue Mar 10 08:28:15 2015 +0000 +++ b/main.cpp Tue Mar 10 13:22:00 2015 +0000 @@ -39,10 +39,10 @@ #include "mbed.h" #include <math.h> #include <sstream> +#include "Servo.h" #include "MultiSerial.h" #include "PID.h" #include "AQM0802A.h" -#include "Servo.h" #include "main.h" @@ -54,7 +54,7 @@ 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; @@ -71,7 +71,7 @@ pwm[0] = vr - vs; pwm[1] = 0; pwm[2] = 0; - pwm[3] = vl*1.1 + vs; + pwm[3] = vl + vs; for(i = 0; i < 4; i++){ if(pwm[i] > 100){ @@ -253,7 +253,7 @@ void IrFrontAction()//ball 12 or 0 o-clock { - if(IrData[1]>700){ + if(IrData[1]>70){ if(abs(CompassPID)>10){ move(0,0,CompassPID,0); return; @@ -261,7 +261,7 @@ move(30,30,CompassPID,0); return; } - if(IrData[1]>600){ + if(IrData[1]>60){ move(25,25,CompassPID,0); return; } @@ -303,28 +303,28 @@ **/ if(PingData[1]>PingData[3]){ /*右が大きい*/ - if(IrData[1]>700){ + if(IrData[1]>70){ move(-20,-20,CompassPID,45); return; } - if(IrData[1]>600){ + if(IrData[1]>60){ move(-20,-20,CompassPID,60); return; } - move(-20,-20,CompassPID,90); + move(-20,-20,CompassPID,80); return; } /*左が大きい*/ - if(IrData[1]>700){ + if(IrData[1]>70){ move(-20,-20,CompassPID,-45); return; } - if(IrData[1]>600){ + if(IrData[1]>60){ move(-20,-20,CompassPID,-60); return; } - move(-20,-20,CompassPID,-90); + move(-20,-20,CompassPID,-80); return; } @@ -332,11 +332,11 @@ void GoHome()//Ball is not found. { - if(PingData[2] >=60){//後ろからの距離60cm + /*if(PingData[2] >=60){//後ろからの距離60cm move(-20,-20,CompassPID,0); return ; } - + */ move(0,0,CompassPID,0);//stop } @@ -344,12 +344,12 @@ void PidUpdate() { - + Compass = rx_data[7]+rx_data[8]; pid.setSetPoint((int)((REFERENCE + SetC) / 1.0)); InputPID = Compass; pid.setProcessValue(InputPID); - CompassPID = -(pid.compute()); + CompassPID = 0;//-(pid.compute()); } @@ -395,6 +395,8 @@ void SetUp(){ /*初期化*/ + + Motor.baud(115200); //ボーレート設定 Motor.printf("1F0002F0003F0004F000\r\n"); //モータ停止 Motor.attach(&tx_motor,Serial::TxIrq); //送信空き割り込み(モータ用) @@ -407,7 +409,6 @@ 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 @@ -494,7 +495,8 @@ /*State */ uint8_t LineIr = 0; uint8_t IrChange[13] ={0x01,0x01,0x03,0x02,0x02,0x06, - 0x04,0x04,0x0B,0x08,0x08,0x09,0x00}; + + 0x04,0x04,0x0B,0x08,0x08,0x09,0x00}; /*行動設定*/ int Power = 0; @@ -517,9 +519,11 @@ SetUp();/*set up routine*/ - Mbed.read_data(rx_data, ADDRESS); - Mbed.start_read(); + + //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); @@ -546,11 +550,13 @@ wait_ms(10); } }else if(LinePing){ + move(0,0,0,0); while(LinePing){ Led[1] = Led[2] = Led[3] = 1; Receive(); LineData = (~Line+0x00) & 0x0F; LinePing = PingChange(LineData); + wait_ms(10); } } @@ -602,9 +608,9 @@ 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]); - + //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]); + pc.printf("%d\t %d\t %d\t %d\n",speed[0],speed[1],speed[2],speed[3]); wait(0.1); }