廣智 角谷
/
LineTracer
ライントレーサー制御用のプログラムです.
Diff: linetracer.cpp
- Revision:
- 0:e9af471e2a2b
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/linetracer.cpp Thu Nov 16 06:15:58 2017 +0000 @@ -0,0 +1,76 @@ +#include "linetracer.h" +#define K 0.000001 //ハンドルのききの良さを決める定数 +DigitalOut test1(PTB8); +DigitalOut test2(PTB9); +DigitalOut test3(PTB10); +DigitalOut test4(PTB11); + +void linetracerInit(){ + servoInit(); + sensorInit(); + motorInit(); + start(); +} + +void linetracerLoop(){ + float pulse; + pulse = 0.00155; + int reaction = 0; + int flag = 0; + while(1){ + reaction = getReaction(); + /* + if((reaction & 0b000011) == 0b000000){ + flag++; + if(flag >= 100){ + wait(0.3); + stop(); + while(1){ + test1 = 1; + } + } + } + else{ + flag = 0; + test1 = 0; + } + */ + + + if((reaction & 0b100000) == 0b100000){ + rotate(handle,pulse - K,&pulse); + test1 = 1; + } + + + if((reaction & 0b000100) == 0b000100){ + rotate(handle,pulse + K,&pulse); + test2 = 1; + } + + if((reaction & 0b010000) == 0b010000){ + rotate(handle,pulse - K / 3.0,&pulse); + test3 = 1; + } + + if((reaction & 0b001000) == 0b001000){ + rotate(handle,pulse + K / 3.0,&pulse); + test4 = 1; + } + + test1 = 0; + test2 = 0; + test3 = 0; + test4 = 0; + + /* + if((reaction & 0b010100) == 0b000000){ + rotate(handle,pulse + K * 0.5,&pulse); + } + + if((reaction & 0b1010000) == 0b000000){ + rotate(handle,pulse - K * 0.5,&pulse); + } + */ + } +}