Revision 2:70607c6dad36, committed 2016-01-10

Comitter:

Sinterbaas

Date:

Sun Jan 10 23:46:12 2016 +0000

Parent:

1:ed42d9035468

Commit message:

All of the latest changes (inc. Motor stuck detection)

Changed in this revision

diff -r ed42d9035468 -r 70607c6dad36 MotorController.cpp
--- a/MotorController.cpp	Thu Jan 07 16:19:14 2016 +0000
+++ b/MotorController.cpp	Sun Jan 10 23:46:12 2016 +0000
@@ -1,7 +1,8 @@
 #include "MotorController.h"
 
 MotorController::MotorController (HBridge &motor, AnalogIn &motorInput) :
-    motor ( motor ), motorInput ( motorInput ) {
+    motor ( motor ), motorInput ( motorInput ), currentStatus ( MotorController::STATUS_OK ),
+    bufferCount ( 0 ), bufferIndex ( 0 ) {
 }
 
 void MotorController::start () {
@@ -13,12 +14,16 @@
 }
 
 void MotorController::turnLeft () {
+    this->CheckMotorStatus();
+    
     this->motor.start();
     this->motor.backward();
 }
 
 
 void MotorController::turnRight () {
+    this->CheckMotorStatus();
+    
     this->motor.start();
     this->motor.forward();
 }
@@ -26,21 +31,22 @@
 void MotorController::setPosition (float position) {
     float current_position = this->getPosition();
     
-    if (current_position <= position * (1 + this->margin_percentage) + this->margin_fixed && current_position >= position * (1 - this->margin_percentage) - this->margin_fixed) {
+    if (current_position <= position * (1 + MARGIN_PERCENTAGE) + MARGIN_FIXED &&
+            current_position >= position * (1 - MARGIN_PERCENTAGE) - MARGIN_FIXED) {
         this->stop();
-    } else if (current_position > position) {
+        this->currentStatus = STATUS_OK;
+        return;
+    }
+    else {
+        this->CheckMotorStatus();
+    }
+    
+    
+    if (current_position > position) {
         this->turnLeft();
-        
-        if (this->getPosition() <= position) {
-            this->stop();
-        }
     }
     else {
         this->turnRight();
-        
-        if (this->getPosition() >= position) {
-            this->stop();
-        }
     }
 }
 
@@ -55,4 +61,52 @@
 
 char MotorController::getCurrentAction() {
     return this->currentAction;
+}
+
+void MotorController::setCurrentStatus(char c) {
+    this->currentStatus = c;
+}
+
+char MotorController::getCurrentStatus () {
+    return this->currentStatus;
+}
+
+void MotorController::addLatestPosition (float position) {
+    this->bufferIndex = (this->bufferIndex + 1) % BUFFER_SIZE;
+    
+    this->positionBuffer[bufferIndex] = position;
+    
+    if (this->bufferCount < BUFFER_SIZE) {
+        bufferCount += 1;
+    }
+}
+
+bool MotorController::isMotorStuck () {
+    float sum = 0;
+    for (int i = 0; i < bufferCount; i++) {
+        sum += this->positionBuffer[i];
+    }
+    
+    float average = sum / bufferCount;
+    float allowed_deviation = average * MARGIN_PERCENTAGE + 0.03f;
+    float deviation = 0;
+    
+    for (int i = 0; i < bufferCount; i++) {
+        deviation += (average - this->positionBuffer[i]) * (average - this->positionBuffer[i]);
+    }
+    
+    printf("Status: %f < %f\r\n", deviation, allowed_deviation);
+    
+    return deviation < allowed_deviation;
+}
+
+void MotorController::CheckMotorStatus () {
+    this->addLatestPosition(this->getPosition());
+        
+    if (this->isMotorStuck()) {
+        this->currentStatus = STATUS_STUCK;    
+    }
+    else {
+        this->currentStatus = STATUS_OK;
+    }
 }
\ No newline at end of file

diff -r ed42d9035468 -r 70607c6dad36 MotorController.h
--- a/MotorController.h	Thu Jan 07 16:19:14 2016 +0000
+++ b/MotorController.h	Sun Jan 10 23:46:12 2016 +0000
@@ -17,8 +17,11 @@
         
         float getPosition ();
         
-        void setCurrentAction(char c);
-        char getCurrentAction();
+        void setCurrentAction (char c);
+        char getCurrentAction ();
+        
+        void setCurrentStatus (char c);
+        char getCurrentStatus ();
         
         static const float POSITION_0  = 0.0f;
         static const float POSITION_1  = 0.65f;
@@ -32,13 +35,27 @@
         static const float POSITION_9  = 98.0f;
         static const float POSITION_10 = 100.0f;
         
-        static const float margin_percentage = 0.01f;
-        static const float margin_fixed = 0.05f;
+        static const float MARGIN_PERCENTAGE = 0.013f;
+        static const float MARGIN_FIXED = 0.13f;
+        
+        static const float STATUS_OK = 'o';
+        static const float STATUS_STUCK = 's';
     
     private:
+        static const int BUFFER_SIZE = 20;
         HBridge &motor;
         AnalogIn &motorInput;
+        
         char currentAction;
+        char currentStatus;
+        
+        int bufferCount;
+        int bufferIndex;
+        float positionBuffer[BUFFER_SIZE];
+        
+        bool isMotorStuck ();
+        void addLatestPosition (float position);
+        void CheckMotorStatus ();
 };
 
 #endif
\ No newline at end of file