Gruppe 3 / Mbed 2 deprecated PES3

hallo

Dependencies:   Servo mbed pixy

Fork of PES1 by Gruppe 3

Roboter.cpp@4:fdcf3b5009c6, 2017-04-11 (annotated)

Committer:
itslinear
Date:
Tue Apr 11 11:47:04 2017 +0000
Revision:
4:fdcf3b5009c6
Parent:
3:24e098715b78
Child:
6:4af735d26b7a
hallo

Who changed what in which revision?

UserRevisionLine numberNew contents of line
itslinear 0:306a2438de17 1 #include "Roboter.h"
itslinear 0:306a2438de17 2
itslinear 0:306a2438de17 3
itslinear 4:fdcf3b5009c6 4 Roboter::Roboter(AnalogIn* distance, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, PwmOut* pwmL, PwmOut* pwmR, Servo* s1, Servo* s2,DigitalIn* switch1, DigitalIn* switch2)
itslinear 4:fdcf3b5009c6 5
itslinear 1:4e84271a70c6 6 {
itslinear 0:306a2438de17 7
itslinear 1:4e84271a70c6 8 sensors[0].init( distance, bit0, bit1, bit2, 0);
itslinear 1:4e84271a70c6 9 sensors[1].init( distance, bit0, bit1, bit2, 1);
itslinear 1:4e84271a70c6 10 sensors[2].init( distance, bit0, bit1, bit2, 2);
itslinear 1:4e84271a70c6 11 sensors[3].init( distance, bit0, bit1, bit2, 3);
itslinear 1:4e84271a70c6 12 sensors[4].init( distance, bit0, bit1, bit2, 4);
itslinear 1:4e84271a70c6 13 sensors[5].init( distance, bit0, bit1, bit2, 5);
itslinear 0:306a2438de17 14
itslinear 1:4e84271a70c6 15 this->pwmR = pwmR;
itslinear 1:4e84271a70c6 16 this->pwmL = pwmL;
itslinear 4:fdcf3b5009c6 17 this->s1 = s1;
itslinear 4:fdcf3b5009c6 18 this->s2 = s2;
itslinear 4:fdcf3b5009c6 19 this->switch1 = switch1;
itslinear 4:fdcf3b5009c6 20 this->switch2 = switch2;
itslinear 4:fdcf3b5009c6 21
itslinear 1:4e84271a70c6 22
itslinear 1:4e84271a70c6 23 pwmL->period(0.00005f); // Setzt die Periode auf 50 μs
itslinear 1:4e84271a70c6 24 pwmR->period(0.00005f);
itslinear 1:4e84271a70c6 25
itslinear 1:4e84271a70c6 26 }
itslinear 1:4e84271a70c6 27
itslinear 1:4e84271a70c6 28 float Roboter::readSensor1()
itslinear 0:306a2438de17 29 {
itslinear 1:4e84271a70c6 30 return sensors[0].read();
itslinear 1:4e84271a70c6 31
itslinear 0:306a2438de17 32 }
itslinear 0:306a2438de17 33
itslinear 1:4e84271a70c6 34
itslinear 1:4e84271a70c6 35 void Roboter::bandeAusweichen()
itslinear 0:306a2438de17 36 {
itslinear 1:4e84271a70c6 37 float offsetDir;
itslinear 1:4e84271a70c6 38 float offsetLin;
itslinear 1:4e84271a70c6 39
itslinear 1:4e84271a70c6 40 float x=0.13f; // Distanz ab welcher sensoren reagieren sollen
itslinear 1:4e84271a70c6 41
itslinear 1:4e84271a70c6 42
itslinear 1:4e84271a70c6 43 offsetDir = 0;
itslinear 1:4e84271a70c6 44 offsetLin = 0.1;
itslinear 1:4e84271a70c6 45
itslinear 0:306a2438de17 46
itslinear 1:4e84271a70c6 47 if(sensors[0].read() < x && sensors[5] > x) { // sensor vorne, roboter dreht nach links
itslinear 1:4e84271a70c6 48 offsetDir = -0.05;
itslinear 1:4e84271a70c6 49 offsetLin = 0;
itslinear 1:4e84271a70c6 50 }
itslinear 1:4e84271a70c6 51 if(sensors[1] < x) { // sensor rechts, roboter dreht nach links
itslinear 1:4e84271a70c6 52 offsetDir = -0.05;
itslinear 1:4e84271a70c6 53 offsetLin = 0;
itslinear 0:306a2438de17 54 }
itslinear 1:4e84271a70c6 55 if(sensors[5] < x && sensors[1]>(x+0.02f)) { // sensor links, roboter dreht nach rechts
itslinear 1:4e84271a70c6 56 offsetDir = 0.05;
itslinear 1:4e84271a70c6 57 offsetLin = 0;
itslinear 1:4e84271a70c6 58 }
itslinear 1:4e84271a70c6 59
itslinear 1:4e84271a70c6 60 *pwmL = 0.5f+offsetDir+offsetLin; // Setzt die Duty-Cycle auf 50%
itslinear 1:4e84271a70c6 61 *pwmR = 0.5f+offsetDir-offsetLin;
itslinear 1:4e84271a70c6 62
itslinear 3:24e098715b78 63 }
schneju2 2:953263712480 64
schneju2 2:953263712480 65
schneju2 2:953263712480 66
itslinear 1:4e84271a70c6 67
itslinear 3:24e098715b78 68 void Roboter::pickup ()
itslinear 3:24e098715b78 69 {
itslinear 3:24e098715b78 70
itslinear 0:306a2438de17 71
schneju2 2:953263712480 72
itslinear 3:24e098715b78 73 }
itslinear 3:24e098715b78 74
itslinear 4:fdcf3b5009c6 75 void Roboter::findBlock() //im Kreis drehen
itslinear 3:24e098715b78 76 {
itslinear 4:fdcf3b5009c6 77 static int count =0;
itslinear 4:fdcf3b5009c6 78 count++;
itslinear 4:fdcf3b5009c6 79
itslinear 4:fdcf3b5009c6 80 if(count <= 10); //1s lang (10*0.1s) im Kreis drehen
itslinear 3:24e098715b78 81 *pwmL = 0.55f;
itslinear 4:fdcf3b5009c6 82 *pwmR = 0.55f;
itslinear 4:fdcf3b5009c6 83
itslinear 4:fdcf3b5009c6 84 if(count > 10 && count <= 20); //zwischen Sekunde 1 und 2 geradeaus fahren
itslinear 4:fdcf3b5009c6 85 *pwmL = 0.6f;
itslinear 4:fdcf3b5009c6 86 *pwmR = 0.4f;
itslinear 4:fdcf3b5009c6 87
itslinear 4:fdcf3b5009c6 88 if(count > 20 && count <=30); //zwischen Sekunde 2 und 3 im Kreis drehen
itslinear 4:fdcf3b5009c6 89 *pwmL = 0.55f;
itslinear 4:fdcf3b5009c6 90 *pwmR = 0.55f;
itslinear 3:24e098715b78 91
itslinear 3:24e098715b78 92 }
itslinear 3:24e098715b78 93