Diff: MotorController.cpp

Revision:

1:548ee2d0bb89

Parent:

0:4aa0a6b134ea

--- a/MotorController.cpp	Wed Jan 15 11:48:41 2014 +0000
+++ b/MotorController.cpp	Tue Jan 21 08:22:01 2014 +0000
@@ -31,50 +31,103 @@
 #include "mbed.h"
 #include "MotorController.h"
 
-MotorController::MotorController(PinName LMotorOut, PinName LSensorIn, PinName RMotorOut, PinName RSensorIn):
-    _LeftMotor(p5, p7), _RightMotor(p6, p8)
+MotorController::MotorController(PinName LMotorOut, PinName LBrakeOut, PinName LSensorIn, PinName RMotorOut, PinName RBrakeOut, PinName RSensorIn):
+    _LeftMotor(LMotorOut, LBrakeOut, LSensorIn), _RightMotor(RMotorOut, RBrakeOut, RSensorIn)
 {
-    _LeftMotor.SetMotorPwmAndRevolutions(18000,32);
-    _RightMotor.SetMotorPwmAndRevolutions(18000,32);
+    trackomatic.attach_us(this, &MotorController::MatchSpeeds, PWM_PERIOD * 2000);
+    LeftMotorSpeed = 0;
+    RightMotorSpeed = 0;
+    LeftToRightRatio = 1;
+    ResetOdometer();
+    _LeftMotor.SetMotorPwmAndRevolutions(LeftMotorSpeed,0);
+    _RightMotor.SetMotorPwmAndRevolutions(RightMotorSpeed,0);
+
+/*    _LeftMotor.SetMotorPwmAndRevolutions(LeftMotorFullSpeed,20);
+    _RightMotor.SetMotorPwmAndRevolutions(RightMotorFullSpeed,20);
+    while (_LeftMotor.GetRevolutionsLeft() != 0) {};
+    while (_RightMotor.GetRevolutionsLeft() != 0) {};
+
+    while (_LeftMotor.ReadCaptureTime() < _RightMotor.ReadCaptureTime()) {
+        // Left motor is faster, adjust to match right motor
+        LeftMotorFullSpeed -= 1;
+    _LeftMotor.SetMotorPwmAndRevolutions(LeftMotorFullSpeed,20);
+    _RightMotor.SetMotorPwmAndRevolutions(RightMotorFullSpeed,20);
+        _LeftMotor.ResetOdometer();
+    while (_LeftMotor.GetRevolutionsLeft() != 0) {};
+    while (_RightMotor.GetRevolutionsLeft() != 0) {};
+    }
+    while (_LeftMotor.ReadCaptureTime() > _RightMotor.ReadCaptureTime()) {
+        // Right motor is faster, adjust to match left motor
+        RightMotorFullSpeed -= 1;
+    _LeftMotor.SetMotorPwmAndRevolutions(LeftMotorFullSpeed,20);
+    _RightMotor.SetMotorPwmAndRevolutions(RightMotorFullSpeed,20);
+        _RightMotor.ResetOdometer();
+    while (_LeftMotor.GetRevolutionsLeft() != 0) {};
+    while (_RightMotor.GetRevolutionsLeft() != 0) {};
+    }*/
 }
 
-void MotorController::LeftResetOdometer(void)
+void MotorController::Forwards(int ForwardCount)
+{
+    LeftMotorSpeed = 18000;
+    RightMotorSpeed = 18000;
+    LeftToRightRatio = 1;
+    DistanceToTravel = ForwardCount;
+    ResetOdometer();
+    _LeftMotor.SetMotorPwmAndRevolutions(LeftMotorSpeed,ForwardCount);
+    _RightMotor.SetMotorPwmAndRevolutions(RightMotorSpeed,ForwardCount);
+}
+
+void MotorController::UpdatePwms(void)
+{
+    _LeftMotor.SetMotorPwm(LeftMotorSpeed);
+    _RightMotor.SetMotorPwm(RightMotorSpeed);
+}
+
+void MotorController::ResetOdometer(void)
 {
     _LeftMotor.ResetOdometer();
+    _RightMotor.ResetOdometer();
 }
 
-int MotorController::LeftReadOdometer(void)
+int MotorController::ReadOdometer(void)
+{
+    return((_LeftMotor.ReadOdometer() + _RightMotor.ReadOdometer())/2);
+}
+
+int MotorController::ReadLeftOdometer(void)
 {
     return(_LeftMotor.ReadOdometer());
 }
 
-int MotorController::LeftReadCaptureTime(void)
-{
-    return(_LeftMotor.ReadCaptureTime());
-}
-
-int MotorController::LeftReadLastTime(void)
-{
-    return(_LeftMotor.GetLastPulseTime());
-}
-
-void MotorController::RightResetOdometer(void)
-{
-    _RightMotor.ResetOdometer();
-}
-
-int MotorController::RightReadOdometer(void)
+int MotorController::ReadRightOdometer(void)
 {
     return(_RightMotor.ReadOdometer());
 }
 
-int MotorController::RightReadCaptureTime(void)
+void MotorController::MatchSpeeds(void)
 {
-    return(_RightMotor.ReadCaptureTime());
+    if (ReadOdometer() >= DistanceToTravel)
+    {
+        LeftMotorSpeed = 0;
+        RightMotorSpeed = 0;
+        _LeftMotor.SetMotorPwmAndRevolutions(LeftMotorSpeed,0);
+        _RightMotor.SetMotorPwmAndRevolutions(RightMotorSpeed,0);
+    }
+    else
+    {
+        if (_LeftMotor.ReadOdometer() < _RightMotor.ReadOdometer())
+        {
+                LeftMotorSpeed+=10;
+                RightMotorSpeed-=10;
+                UpdatePwms();
+        } else
+        {
+                LeftMotorSpeed-=10;
+                RightMotorSpeed+=10;
+                UpdatePwms();
+        }
+    }
 }
 
-int MotorController::RightReadLastTime(void)
-{
-    return(_RightMotor.GetLastPulseTime());
-}
 #endif
\ No newline at end of file