廣智 角谷
/
LineTracer
ライントレーサー制御用のプログラムです.
Revision 0:e9af471e2a2b, committed 2017-11-16
- Comitter:
- Hirotomo777
- Date:
- Thu Nov 16 06:15:58 2017 +0000
- Commit message:
- ?7????????????????
Changed in this revision
diff -r 000000000000 -r e9af471e2a2b linetracer.cpp --- /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); + } + */ + } +}
diff -r 000000000000 -r e9af471e2a2b linetracer.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/linetracer.h Thu Nov 16 06:15:58 2017 +0000 @@ -0,0 +1,4 @@ +#include "motor.h" +#include "sensor.h" +void linetracerInit(); +void linetracerLoop();
diff -r 000000000000 -r e9af471e2a2b main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Thu Nov 16 06:15:58 2017 +0000 @@ -0,0 +1,10 @@ +#include "mbed.h" +#include "linetracer.h" + +int main() { + linetracerInit(); + linetracerLoop(); + return 0; +} + +
diff -r 000000000000 -r e9af471e2a2b mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Thu Nov 16 06:15:58 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/b484a57bc302 \ No newline at end of file
diff -r 000000000000 -r e9af471e2a2b motor.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/motor.cpp Thu Nov 16 06:15:58 2017 +0000 @@ -0,0 +1,77 @@ + +#include "motor.h" +BusOut leftc(PTA1,PTA2); +PwmOut leftp(PTD4); + +BusOut rightc(PTC0,PTC7); +PwmOut rightp(PTA12); + +PwmOut handle(PTC9); + +//start code for servo +void servoInit(){ + handle.period(0.020); + handle.pulsewidth(0.00155); + wait(0.5); +} + + +void rotate(PwmOut servo,float ppulse,float *npulse){ + if(ppulse > *npulse){ + if(ppulse > 0.002){ + ppulse = 0.002; + } + servo.pulsewidth(ppulse); // servo position determined by a pulsewidth between 1-2ms + wait(1000.0 * 0.06 * ((ppulse - (*npulse)) / 0.1)); + } + else { + if(ppulse < 0.0011){ + ppulse = 0.0011; + } + servo.pulsewidth(ppulse); // servo position determined by a pulsewidth between 1-2ms + wait(1000.0 * 0.06 * (((*npulse) - ppulse) / 0.1)); + } + *npulse = ppulse; +} + +/* +float rotate(PwmOut servo,float ppulse){ + static float npulse = 0.00155; + if(ppulse > npulse){ + if(ppulse > 0.002){ + ppulse = 0.002; + } + servo.pulsewidth(ppulse); // servo position determined by a pulsewidth between 1-2ms + wait(1000.0 * 0.06 * ((ppulse - (npulse)) / 0.1)); + } + else { + if(ppulse < 0.0011){ + ppulse = 0.0011; + } + servo.pulsewidth(ppulse); // servo position determined by a pulsewidth between 1-2ms + wait(1000.0 * 0.06 * (((npulse) - ppulse) / 0.1)); + } + npulse = ppulse; + return npulse; +} +*/ +//end + +//start code for DCmotor +void motorInit(){ + leftc = 0; + leftp = 1.0f; + rightc = 0; + rightp = 1.0f; +} + +void start(){ + leftc = 1; + rightc = 1; +} + +void stop(){ + leftc = 0; + rightc = 0; +} +//end \ No newline at end of file
diff -r 000000000000 -r e9af471e2a2b motor.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/motor.h Thu Nov 16 06:15:58 2017 +0000 @@ -0,0 +1,9 @@ +#include "mbed.h" +extern PwmOut handle; +void motorInit(); +void rotate(PwmOut servo,float ppulse,float *npulse); +//float rotate(PwmOut servo,float ppulse); +void servoInit(); +void start(); +void stop(); +void control(PwmOut left,PwmOut right); \ No newline at end of file
diff -r 000000000000 -r e9af471e2a2b sensor.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/sensor.cpp Thu Nov 16 06:15:58 2017 +0000 @@ -0,0 +1,131 @@ +#include "sensor.h" +#define threshold1 0.005 +#define threshold2 0.005 +/* +AnalogIn sensorfl(PTB0); +AnalogIn sensorfr(PTB1); +AnalogIn sensorbl(PTB2); +AnalogIn sensorbr(PTB3); +*/ +AnalogIn sensor1(PTB0); //左から数えて1,2,3,4 +AnalogIn sensor2(PTB1); +AnalogIn sensor3(PTB2); +AnalogIn sensor4(PTB3); + +DigitalIn sensorl(PTD6); +DigitalIn sensorr(PTD7); + +Timer t; + +static float th1 = 0.0; +static float th2 = 0.0; +static float th3 = 0.0; +static float th4 = 0.0; + +void sensorInit(){ + float th1max = sensor1; + float th1min = sensor1; + float th2max = sensor2; + float th2min = sensor2; + float th3max = sensor3; + float th3min = sensor3; + float th4max = sensor4; + float th4min = sensor4; + handle.pulsewidth(0.001); + t.start(); + while(1){ + if(t.read() > 1.0){ + break; + } + if(th1max < sensor1){ + th1max = sensor1; + } + if(th1min > sensor1){ + th1min = sensor1; + } + if(th2max < sensor2){ + th2max = sensor2; + } + if(th2min > sensor2){ + th2min = sensor2; + } + if(th3max < sensor3){ + th3max = sensor3; + } + if(th3min > sensor3){ + th3min = sensor3; + } + if(th4max < sensor4){ + th4max = sensor4; + } + if(th4min > sensor4){ + th4min = sensor4; + } + } + //wait(1.0); + t.stop(); + t.reset(); + handle.pulsewidth(0.0021); + t.start(); + while(1){ + if(t.read() > 1.0){ + break; + } + if(th1max < sensor1){ + th1max = sensor1; + } + if(th1min > sensor1){ + th1min = sensor1; + } + if(th2max < sensor2){ + th2max = sensor2; + } + if(th2min > sensor2){ + th2min = sensor2; + } + if(th3max < sensor3){ + th3max = sensor3; + } + if(th3min > sensor3){ + th3min = sensor3; + } + if(th4max < sensor4){ + th4max = sensor4; + } + if(th4min > sensor4){ + th4min = sensor4; + } + } + th1 = (th1max + th1min) / 2.0; + th2 = (th2max + th2min) / 2.0; + th3 = (th3max + th3min) / 2.0; + th4 = (th4max + th4min) / 2.0; + t.stop(); + //wait(1.0); + handle.pulsewidth(0.00155); + wait(0.5); +} + +int getReaction(){ + int tmp = 0; + if(sensor1 < th1){ + tmp += 0b100000; + } + + if(sensor2 < th2){ + //tmp += 0b010000; + tmp += 0b000000; + } + if(sensor3 < th3){ + tmp += 0b000000; + //死んでる + } + + if(sensor4 < th4){ + tmp += 0b000100; + } + + tmp += 0b000010 * sensorl; + tmp += 0b000001 * sensorr; + return tmp; +} \ No newline at end of file
diff -r 000000000000 -r e9af471e2a2b sensor.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/sensor.h Thu Nov 16 06:15:58 2017 +0000 @@ -0,0 +1,4 @@ +#include "mbed.h" +#include "motor.h" +void sensorInit(); +int getReaction(); \ No newline at end of file