Diff: DCMotorDrive.cpp

Revision:

1:0c8cb3439288

Parent:

0:34800070e9dc

Child:

2:5239659649d1

--- a/DCMotorDrive.cpp	Tue Jan 14 10:45:19 2014 +0000
+++ b/DCMotorDrive.cpp	Wed Jan 15 11:48:01 2014 +0000
@@ -35,34 +35,44 @@
     _MotorPin(MotorOut), _SensorIn(SensorIn)
 {
     _MotorPin.period_ms(PWM_PERIOD);
-    SetMotorPwm(0);
+    SetMotorPwmAndRevolutions(0,0);
     _SensorIn.mode(PullUp);
     _SensorIn.fall(this, &DCMotorDrive::Counter);
+    PulseClock.start();
     PulseClock.reset();
     ResetOdometer();
     ClearBuffer();
 }
 
-void DCMotorDrive::SetMotorPwm(int PwmValue)
+void DCMotorDrive::SetMotorPwmAndRevolutions(int PwmValue, int MaxRevolutions)
 {
     _MotorPin.pulsewidth_us(PwmValue);
+    RevolutionLimit = MaxRevolutions;
+    CaptureTime = 0;
 }
 
 int DCMotorDrive::GetAveragePulseTime(void)
 {
     uint32_t    Average = 0;
     int         BufferPointer;
-    int         ValueCount = 0;
+    int         Count = 0;
     
-    for (BufferPointer = 0; BufferPointer <= 15; BufferPointer++ )
+    for (BufferPointer = 0; BufferPointer < BUFFER_SIZE; BufferPointer++ )
     {
         if (ClockBuffer[BufferPointer] != 0)
-            ValueCount ++;
+            Count += 1;
         Average += ClockBuffer[BufferPointer];
     }
-    return((int)(Average / ValueCount));
+    if (Count == 0)
+        return (0);
+    else
+        return((int)(Average / Count));
 }
 
+int DCMotorDrive::GetLastPulseTime(void)
+{
+    return(LastPulseTime);
+}
 void DCMotorDrive::ResetOdometer(void)
 {
     RotationCounter = 0;
@@ -73,22 +83,33 @@
     return(RotationCounter);
 }
 
+int DCMotorDrive::ReadCaptureTime(void)
+{
+    return(CaptureTime);
+}
 void DCMotorDrive::ClearBuffer(void)
 {
     int         BufferPointer;
 
-    ClockBuffer[ClockPointer = 0] = 0;
-    for (BufferPointer = 1; BufferPointer <= 15; BufferPointer++ )
+    ClockPointer = 0;
+    for (BufferPointer = 0; BufferPointer < BUFFER_SIZE; BufferPointer++ )
         ClockBuffer[BufferPointer] = 0;
 }
 void DCMotorDrive::Counter(void)
 {
-    ClockBuffer[ClockPointer] = PulseClock.read_us();
+    LastPulseTime = PulseClock.read_us();
     PulseClock.reset();
+    ClockBuffer[ClockPointer] = LastPulseTime;
     timeout.attach(this, &DCMotorDrive::Stall, MOTOR_STALL_TIME);
     RotationCounter++;
-    if (++ClockPointer > 15)
+    if (++ClockPointer == BUFFER_SIZE)
         ClockPointer = 0;
+    if (RevolutionLimit != 0)
+        if (--RevolutionLimit == 0)
+        {
+            SetMotorPwmAndRevolutions(0,0);
+            CaptureTime = GetAveragePulseTime();
+        }
 }
 
 void DCMotorDrive::Stall(void)