ZumoRobotManager.cpp

00001 // Craciun Catalin
00002 //  ZumoRobotManager.cpp
00003 //   © 2014 Catalin Craciun
00004 
00005 #include "ZumoRobotManager.h"
00006 #include "mbed.h"
00007 
00008 #define maxEngPow 100 // The maximum engine power
00009 
00010 Serial console(USBTX, USBRX);
00011 
00012 //motor control pins
00013 DigitalOut dirRight(PTC9);
00014 DigitalOut dirLeft(PTA13);
00015 PwmOut pwmRight(PTD5);
00016 PwmOut pwmLeft(PTD0);
00017 
00018 ZumoRobotManager::ZumoRobotManager() {
00019     // Initialising
00020     this -> velocityX = 0;
00021     this -> velocityY = 0;
00022 }
00023 
00024 ZumoRobotManager::~ZumoRobotManager() {
00025     // Deinitialising
00026 }
00027 
00028 void ZumoRobotManager::applyPowerToLeftEngine(int power) {
00029     pwmLeft.period_us(60);
00030     pwmLeft.pulsewidth_us(power);
00031 }
00032 
00033 void ZumoRobotManager::applyPowerToRightEngine(int power) {
00034     pwmRight.period_us(60);
00035     pwmRight.pulsewidth_us(power);
00036 }
00037 
00038 float ZumoRobotManager::getVelocityX() {
00039     return this -> velocityX;    
00040 }
00041 
00042 float ZumoRobotManager::getVelocityY() {
00043     return this -> velocityY;
00044 }
00045 
00046 void ZumoRobotManager::setVelocityX(float newValue) {
00047     if (this -> velocityX != newValue) {
00048         this -> velocityX = newValue;
00049         this -> updateMotors();
00050     }
00051 }
00052 
00053 void ZumoRobotManager::setVelocityY(float newValue) { 
00054     if (this -> velocityY != newValue) {
00055         this -> velocityY = newValue;
00056         this -> updateMotors();
00057     }
00058 }
00059 
00060 bool ZumoRobotManager::checkPassword(char toCheck[]) {
00061     if (toCheck[0] == '8' &&
00062         toCheck[1] == '1' &&
00063         toCheck[2] == '1' &&
00064         toCheck[3] == '1' &&
00065         toCheck[4] == '$')
00066         return true;
00067     
00068     return false;
00069 }
00070 
00071 void ZumoRobotManager::updateMotors() {
00072     float powerLeftEngine = 0;
00073     float powerRightEngine = 0;
00074     float dist = sqrt(velocityX*velocityX + velocityY*velocityY);
00075     
00076     if (velocityX >= 0 && velocityY >= 0) {
00077         // First chart
00078         powerLeftEngine = dist * (float)maxEngPow; 
00079         powerRightEngine = powerLeftEngine * (1 - velocityX);
00080         
00081     } else if (velocityX < 0 && velocityY >= 0) {
00082         // Second chart
00083         powerRightEngine = dist * (float)maxEngPow;
00084         powerLeftEngine = powerRightEngine * (1 + velocityX); 
00085     } else if (velocityX < 0 && velocityY < 0) {
00086         // Third chart
00087         powerRightEngine = dist * (float)maxEngPow;
00088         powerLeftEngine = powerRightEngine * (1 + velocityX);
00089         
00090         powerRightEngine = -powerRightEngine;
00091         powerLeftEngine = -powerLeftEngine;
00092     } else if (velocityX >= 0 && velocityY < 0) {
00093         // Fourth chart
00094         powerLeftEngine = dist * (float)maxEngPow; 
00095         powerRightEngine = powerLeftEngine * (1 - velocityX);
00096         
00097         powerLeftEngine = -powerLeftEngine;
00098         powerRightEngine = -powerRightEngine;
00099     }
00100     
00101     // Do whatever you want with these powers
00102     pwmLeft.period_us(60);
00103     pwmRight.period_us(60);
00104 
00105     if (powerLeftEngine >= 0) {
00106         dirLeft = 0;
00107         pwmLeft.pulsewidth_us(powerLeftEngine);
00108     } else {
00109         dirLeft = 1;
00110         pwmLeft.pulsewidth_us(-powerLeftEngine);
00111     }
00112     
00113     if (powerRightEngine >= 0) {
00114         dirRight = 0;
00115         pwmRight.pulsewidth_us(powerRightEngine);
00116     } else {
00117         dirRight = 1;
00118         pwmRight.pulsewidth_us(-powerRightEngine);
00119     }
00120 }