Helvijs Kiselis
/
MicromousePES2
main.cpp: Sensoren einlesen und Motoren ansteuern
Diff: Motion.cpp
- Revision:
- 8:862bf9225953
- Parent:
- 5:47262622a9bf
--- a/Motion.cpp Mon Apr 16 12:49:50 2018 +0000 +++ b/Motion.cpp Mon Apr 16 15:23:42 2018 +0000 @@ -30,17 +30,33 @@ */ void Motion::move() { - //distanceL = irSensorL.read(); - //distanceC = irSensorC.read(); - //distanceR = irSensorR.read(); + //distanceL = irSensorL.readL(); + //distanceC = irSensorC.readC(); + //distanceR = irSensorR.readR(); + float korr = 1; + const float kp = 0.28f; + float speedRight = -50.0f; + float speedLeft = 50.0f; while ((countsL - countsLOld) < 1630 || (countsR - countsROld) > -1630) { countsL = counterLeft.read(); countsR = counterRight.read(); + distanceL = irSensorL.readL(); + distanceR = irSensorR.readR(); + + korr = (distanceL-distanceR)*kp; + + if (korr <= 1){ + speedRight = -50.0f * korr; + speedLeft = 50.0f + } + else + speedLeft = 50.0f * -korr; + speedRight = -50.0f; - controller.setDesiredSpeedLeft(50.0f); - controller.setDesiredSpeedRight(-50.0f); + controller.setDesiredSpeedLeft(speedLeft); + controller.setDesiredSpeedRight(speedRight); enableMotorDriver = 1; } //printf("countsLeft: %hd countsRight: %hd\n", countsL, countsR);