Diff: DCMotorDrive.cpp

Revision:

2:5239659649d1

Parent:

1:0c8cb3439288

--- a/DCMotorDrive.cpp	Wed Jan 15 11:48:01 2014 +0000
+++ b/DCMotorDrive.cpp	Tue Jan 21 08:21:33 2014 +0000
@@ -31,48 +31,48 @@
 #include "mbed.h"
 #include "DCMotorDrive.h"
 
-DCMotorDrive::DCMotorDrive(PinName MotorOut, PinName SensorIn):
-    _MotorPin(MotorOut), _SensorIn(SensorIn)
+DCMotorDrive::DCMotorDrive(PinName MotorOut, PinName BrakeOut, PinName SensorIn):
+    _MotorPin(MotorOut), _BrakePin(BrakeOut), _SensorIn(SensorIn)
 {
     _MotorPin.period_ms(PWM_PERIOD);
     SetMotorPwmAndRevolutions(0,0);
     _SensorIn.mode(PullUp);
     _SensorIn.fall(this, &DCMotorDrive::Counter);
+    _BrakePin = 1;
     PulseClock.start();
     PulseClock.reset();
     ResetOdometer();
-    ClearBuffer();
+}
+
+void DCMotorDrive::SetMotorPwm(int PwmValue)
+{
+    if (PwmValue <= 0)
+    {
+        _MotorPin.pulsewidth_us(0);
+        _BrakePin = 1;
+    } else
+    {
+        _BrakePin = 0;
+        timeout.attach(this, &DCMotorDrive::Stall, MOTOR_STALL_TIME);
+        _MotorPin.pulsewidth_us(PwmValue);
+    }
 }
 
 void DCMotorDrive::SetMotorPwmAndRevolutions(int PwmValue, int MaxRevolutions)
 {
-    _MotorPin.pulsewidth_us(PwmValue);
+    SetMotorPwm(PwmValue);
     RevolutionLimit = MaxRevolutions;
-    CaptureTime = 0;
-}
-
-int DCMotorDrive::GetAveragePulseTime(void)
-{
-    uint32_t    Average = 0;
-    int         BufferPointer;
-    int         Count = 0;
-    
-    for (BufferPointer = 0; BufferPointer < BUFFER_SIZE; BufferPointer++ )
-    {
-        if (ClockBuffer[BufferPointer] != 0)
-            Count += 1;
-        Average += ClockBuffer[BufferPointer];
-    }
-    if (Count == 0)
-        return (0);
-    else
-        return((int)(Average / Count));
 }
 
 int DCMotorDrive::GetLastPulseTime(void)
 {
     return(LastPulseTime);
 }
+
+int DCMotorDrive::GetRevolutionsLeft(void)
+{
+    return(RevolutionLimit);
+}
 void DCMotorDrive::ResetOdometer(void)
 {
     RotationCounter = 0;
@@ -83,38 +83,26 @@
     return(RotationCounter);
 }
 
-int DCMotorDrive::ReadCaptureTime(void)
-{
-    return(CaptureTime);
-}
-void DCMotorDrive::ClearBuffer(void)
-{
-    int         BufferPointer;
-
-    ClockPointer = 0;
-    for (BufferPointer = 0; BufferPointer < BUFFER_SIZE; BufferPointer++ )
-        ClockBuffer[BufferPointer] = 0;
-}
 void DCMotorDrive::Counter(void)
 {
     LastPulseTime = PulseClock.read_us();
     PulseClock.reset();
-    ClockBuffer[ClockPointer] = LastPulseTime;
+    timeout.detach();
     timeout.attach(this, &DCMotorDrive::Stall, MOTOR_STALL_TIME);
     RotationCounter++;
-    if (++ClockPointer == BUFFER_SIZE)
-        ClockPointer = 0;
     if (RevolutionLimit != 0)
         if (--RevolutionLimit == 0)
         {
             SetMotorPwmAndRevolutions(0,0);
-            CaptureTime = GetAveragePulseTime();
+            _BrakePin = 1;
+            timeout.detach();
         }
 }
 
 void DCMotorDrive::Stall(void)
 {
-    ClearBuffer();
+    SetMotorPwmAndRevolutions(0,0);
+    _BrakePin = 1;
     timeout.detach();
 }