Takeshi Nishimura
/
MiniTracer2020_V1
sample
https://os.mbed.com/media/uploads/MCR_Xavier/x-board.pdf
Revision 1:f382b4244a89, committed 2020-11-08
- Comitter:
- MCR_Xavier
- Date:
- Sun Nov 08 03:59:27 2020 +0000
- Parent:
- 0:21afd0549d07
- Commit message:
- sample
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Sat Apr 28 06:26:41 2018 +0000 +++ b/main.cpp Sun Nov 08 03:59:27 2020 +0000 @@ -40,16 +40,17 @@ int ErrFlg=0; //エラー判定フラグ int SensValBuf=0; //センサ値のバッファ int SensorR,SensorL; //ラインセンサ +float VolumeVal; //----------ボリューム値取得----------------- int GetVol(void){ int Val; - Val = Volume.read_u16()>>12; + Val = Volume.read_u16()>>7; return Val; } //----------センサ値更新----------------- void SensUp(void){ - SensorR = LineR.read_u16()>>8; - SensorL = LineL.read_u16()>>8; + SensorR = LineR.read_u16()>>4; + SensorL = LineL.read_u16()>>4; } //------------モータ管理-------------------- void MotorCtrl(void){ @@ -84,11 +85,23 @@ } //------------ライントレース-------------------- void LineTrace(void){ - int SensVal,CommSpeed=100,PGain=1; - + long SensVal,CommSpeed=0,sensdelta=0,SensValBuf=0,SensI; + //float PGain=6.3,DGain=4; + //float PGain=0.18,DGain=1.07; + float PGain=0.75,DGain=0,IGain=0.1; + //DGain=0; + DGain=VolumeVal; + //PGain=VolumeVal; + //IGain=VolumeVal; + //CommSpeed=100*VolumeVal; SensVal = SensorR - SensorL; - MotorR = int(CommSpeed - (SensVal * PGain)); - MotorL = int(CommSpeed + (SensVal * PGain)); + sensdelta = SensVal - SensValBuf; + SensI = SensI + SensVal; + if(sensdelta < 0 ) sensdelta = sensdelta * (-1); + MotorR = int(CommSpeed - (SensVal * PGain) - (sensdelta * DGain) - (SensI * IGain)); + //MotorL = int(1.19 * (CommSpeed + (SensVal * PGain) - (sensdelta * DGain))); + MotorL = int(CommSpeed + (SensVal * PGain) - (sensdelta * DGain) + (SensI * IGain)); + SensValBuf = SensVal; } //-------------LED出力------------------ void led_out(void){ @@ -108,7 +121,7 @@ MotorCtrl(); led_out(); SensUp(); - LineTrace(); + //LineTrace(); } //----------マイコン初期設定--------------- @@ -132,14 +145,23 @@ //---------------メイン-------------------- int main() { init(); - int sensV; + float tmpval; wait(1); while(SW_IN); MotorDA = 0; while(1) { - if(!SW_IN) pc.printf("%5d %5d %5d %5d \r\n",SensorL,SensorR,MotorL,MotorR); - ledval = sensV; - wait(0.02); + //tmpval=GetVol(); + //VolumeVal=tmpval/100; + if(!SW_IN) pc.printf("%5d %5d %5d %5d %f \r\n",SensorL,SensorR,MotorL,MotorR,VolumeVal); + //ledval = sensV; + MotorR = 300; + MotorL = 300; + ledval ++; + wait(0.5); + MotorR = -300; + MotorL = -300; + ledval ++; + wait(0.5); } } \ No newline at end of file