Helvijs Kiselis
/
MicromousePES2
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Revision 8:862bf9225953, committed 2018-04-16
- Comitter:
- luethale
- Date:
- Mon Apr 16 15:23:42 2018 +0000
- Parent:
- 7:8d60a2ff4c2b
- Commit message:
- Regelung f?r Spurhaltung
Changed in this revision
| Motion.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- 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);
