Diff: ZumoRobotManager.cpp

Revision:

4:dcd52a961392

Parent:

3:b1fde0759c94

Child:

5:5e12111ef01f

--- a/ZumoRobotManager.cpp	Thu Nov 13 15:42:46 2014 +0000
+++ b/ZumoRobotManager.cpp	Thu Nov 27 16:08:05 2014 +0000
@@ -8,6 +8,8 @@
 #include "ZumoRobotManager.h"
 #include "mbed.h"
 
+#define maxEngPow 100 // The maximum engine power
+
 Serial console(USBTX, USBRX);
 
 ZumoRobotManager::ZumoRobotManager() {
@@ -35,12 +37,18 @@
 
 void ZumoRobotManager::setVelocityX(float newValue) {
   
-    this -> velocityX = newValue;
+    if (this -> velocityX != newValue) {
+        this -> velocityX = newValue;
+        this -> updateMotors();
+    }
 }
 
 void ZumoRobotManager::setVelocityY(float newValue) {
  
-    this -> velocityY = newValue;
+    if (this -> velocityY != newValue) {
+        this -> velocityY = newValue;
+        this -> updateMotors();
+    }
 }
 
 bool ZumoRobotManager::checkPassword(char toCheck[]) {
@@ -55,4 +63,38 @@
     return false;
 }
 
+void ZumoRobotManager::updateMotors() {
+    
+    float powerLeftEngine = 0;
+    float powerRightEngine = 0;
+    float dist = sqrt(velocityX*velocityX + velocityY*velocityY);
+    
+    if (velocityX >= 0 && velocityY >= 0) {
+        // First chart
+        powerLeftEngine = dist * (float)maxEngPow; 
+        powerRightEngine = powerLeftEngine * (1 - velocityX);
+        
+    } else if (velocityX < 0 && velocityY >= 0) {
+        // Second chart
+        powerRightEngine = dist * (float)maxEngPow;
+        powerLeftEngine = powerRightEngine * (1 + velocityX); 
+    } else if (velocityX < 0 && velocityY < 0) {
+        // Third chart
+        powerRightEngine = dist * (float)maxEngPow;
+        powerLeftEngine = powerRightEngine * (1 + velocityX);
+        
+        powerRightEngine = -powerRightEngine;
+        powerLeftEngine = -powerLeftEngine;
+    } else if (velocityX >= 0 && velocityY < 0) {
+        // Fourth chart
+        powerLeftEngine = dist * (float)maxEngPow; 
+        powerRightEngine = powerLeftEngine * (1 - velocityX);
+        
+        powerLeftEngine = -powerLeftEngine;
+        powerRightEngine = -powerRightEngine;
+    }
+    
+    // Do whatever you want with these powers
+}
+
 #endif // ZumoRobotManagerCpp
\ No newline at end of file