Gruppe 3
/
PES
Roboter
Diff: Roboter.cpp
- Revision:
- 1:4e84271a70c6
- Parent:
- 0:306a2438de17
- Child:
- 2:953263712480
diff -r 306a2438de17 -r 4e84271a70c6 Roboter.cpp --- a/Roboter.cpp Tue Mar 21 12:42:01 2017 +0000 +++ b/Roboter.cpp Tue Apr 04 11:55:54 2017 +0000 @@ -1,50 +1,60 @@ - #include "Roboter.h" +Roboter::Roboter(AnalogIn* distance, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, PwmOut* pwmL, PwmOut* pwmR) +{ -//timer object for ledshow and distance sensor -Ticker t1; + sensors[0].init( distance, bit0, bit1, bit2, 0); + sensors[1].init( distance, bit0, bit1, bit2, 1); + sensors[2].init( distance, bit0, bit1, bit2, 2); + sensors[3].init( distance, bit0, bit1, bit2, 3); + sensors[4].init( distance, bit0, bit1, bit2, 4); + sensors[5].init( distance, bit0, bit1, bit2, 5); -//lights up the led according to the sensor signal -void ledDistance() + this->pwmR = pwmR; + this->pwmL = pwmL; + + pwmL->period(0.00005f); // Setzt die Periode auf 50 μs + pwmR->period(0.00005f); + +} + +float Roboter::readSensor1() { - for( int ii = 0; ii<6; ++ii) - sensors[ii]< 0.1f ? leds[ii] = 1 : leds[ii] = 0; + return sensors[0].read(); + } -//blinks at startup and starts ledDistance task -void ledShow() + +void Roboter::bandeAusweichen() { - static int timer = 0; - for( int ii = 0; ii<6; ++ii) - leds[ii] = !leds[ii]; + float offsetDir; + float offsetLin; + + float x=0.13f; // Distanz ab welcher sensoren reagieren sollen + + + offsetDir = 0; + offsetLin = 0.1; + - //quit ticker and start led distance show - if( ++timer > 10) { - t1.detach(); - t1.attach( &ledDistance, 0.01f ); + if(sensors[0].read() < x && sensors[5] > x) { // sensor vorne, roboter dreht nach links + offsetDir = -0.05; + offsetLin = 0; + } + if(sensors[1] < x) { // sensor rechts, roboter dreht nach links + offsetDir = -0.05; + offsetLin = 0; } + if(sensors[5] < x && sensors[1]>(x+0.02f)) { // sensor links, roboter dreht nach rechts + offsetDir = 0.05; + offsetLin = 0; + } + + *pwmL = 0.5f+offsetDir+offsetLin; // Setzt die Duty-Cycle auf 50% + *pwmR = 0.5f+offsetDir-offsetLin; + + wait(0.1); + } -int IRSensor() -{ - pwmL.period(0.00005f); // Setzt die Periode auf 50 μs - pwmR.period(0.00005f); - - pwmL = 0.5f; // Setzt die Duty-Cycle auf 50% - pwmR = 0.5f; - enableMotorDriver = 0; - - t1.attach( &ledShow, 0.05f ); - - //init distance sensors - for( int ii = 0; ii<6; ++ii) - sensors[ii].init(&distance, &bit0, &bit1, &bit2, ii); - - enable = 1; - - while(1) { - wait( 0.2f ); - } -}