File content as of revision 7:d2af97b7cc94:

// Craciun Catalin
//  ZumoRobotManager.cpp
//   © 2014 Catalin Craciun

#include "ZumoRobotManager.h"
#include "mbed.h"

#define maxEngPow 100 // The maximum engine power

Serial console(USBTX, USBRX);

//motor control pins
DigitalOut dirRight(PTC9);
DigitalOut dirLeft(PTA13);
PwmOut pwmRight(PTD5);
PwmOut pwmLeft(PTD0);

ZumoRobotManager::ZumoRobotManager() {
    // Initialising
    this -> velocityX = 0;
    this -> velocityY = 0;
}

ZumoRobotManager::~ZumoRobotManager() {
    // Deinitialising
}

void ZumoRobotManager::applyPowerToLeftEngine(int power) {
    pwmLeft.period_us(60);
    pwmLeft.pulsewidth_us(power);
}

void ZumoRobotManager::applyPowerToRightEngine(int power) {
    pwmRight.period_us(60);
    pwmRight.pulsewidth_us(power);
}

float ZumoRobotManager::getVelocityX() {
    return this -> velocityX;    
}

float ZumoRobotManager::getVelocityY() {
    return this -> velocityY;
}

void ZumoRobotManager::setVelocityX(float newValue) {
    if (this -> velocityX != newValue) {
        this -> velocityX = newValue;
        this -> updateMotors();
    }
}

void ZumoRobotManager::setVelocityY(float newValue) { 
    if (this -> velocityY != newValue) {
        this -> velocityY = newValue;
        this -> updateMotors();
    }
}

bool ZumoRobotManager::checkPassword(char toCheck[]) {
    if (toCheck[0] == '8' &&
        toCheck[1] == '1' &&
        toCheck[2] == '1' &&
        toCheck[3] == '1' &&
        toCheck[4] == '$')
        return true;
    
    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
    pwmLeft.period_us(60);
    pwmRight.period_us(60);

    if (powerLeftEngine >= 0) {
        dirLeft = 0;
        pwmLeft.pulsewidth_us(powerLeftEngine);
    } else {
        dirLeft = 1;
        pwmLeft.pulsewidth_us(-powerLeftEngine);
    }
    
    if (powerRightEngine >= 0) {
        dirRight = 0;
        pwmRight.pulsewidth_us(powerRightEngine);
    } else {
        dirRight = 1;
        pwmRight.pulsewidth_us(-powerRightEngine);
    }
}