![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
für holdor
Fork of PES by
Roboter.cpp
- Committer:
- itslinear
- Date:
- 2017-04-11
- Revision:
- 4:fdcf3b5009c6
- Parent:
- 3:24e098715b78
- Child:
- 6:4af735d26b7a
File content as of revision 4:fdcf3b5009c6:
#include "Roboter.h" Roboter::Roboter(AnalogIn* distance, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, PwmOut* pwmL, PwmOut* pwmR, Servo* s1, Servo* s2,DigitalIn* switch1, DigitalIn* switch2) { 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); this->pwmR = pwmR; this->pwmL = pwmL; this->s1 = s1; this->s2 = s2; this->switch1 = switch1; this->switch2 = switch2; pwmL->period(0.00005f); // Setzt die Periode auf 50 μs pwmR->period(0.00005f); } float Roboter::readSensor1() { return sensors[0].read(); } void Roboter::bandeAusweichen() { float offsetDir; float offsetLin; float x=0.13f; // Distanz ab welcher sensoren reagieren sollen offsetDir = 0; offsetLin = 0.1; 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; } void Roboter::pickup () { } void Roboter::findBlock() //im Kreis drehen { static int count =0; count++; if(count <= 10); //1s lang (10*0.1s) im Kreis drehen *pwmL = 0.55f; *pwmR = 0.55f; if(count > 10 && count <= 20); //zwischen Sekunde 1 und 2 geradeaus fahren *pwmL = 0.6f; *pwmR = 0.4f; if(count > 20 && count <=30); //zwischen Sekunde 2 und 3 im Kreis drehen *pwmL = 0.55f; *pwmR = 0.55f; }