Catalin Craciun / ZumoRobotUtilities

Utilities classes for the Zumo Robot

Dependents:   ZumoRobotBluetoothControlled Fsl_Zumo

This library represents some useful code for controlling your Zumo Robot.

ZumoRobotManager.cpp@7:d2af97b7cc94, 2017-07-23 (annotated)

Committer:
catalincraciun7
Date:
Sun Jul 23 12:51:35 2017 +0000
Revision:
7:d2af97b7cc94
Parent:
6:7740c9d8d834 
Minor changes to code style;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
catalincraciun7 0:8ebbd2bd34f5 1 // Craciun Catalin
catalincraciun7 0:8ebbd2bd34f5 2 // ZumoRobotManager.cpp
catalincraciun7 0:8ebbd2bd34f5 3 // © 2014 Catalin Craciun
catalincraciun7 0:8ebbd2bd34f5 4
catalincraciun7 0:8ebbd2bd34f5 5 #include "ZumoRobotManager.h"
catalincraciun7 2:036da44b023e 6 #include "mbed.h"
catalincraciun7 2:036da44b023e 7
catalincraciun7 4:dcd52a961392 8 #define maxEngPow 100 // The maximum engine power
catalincraciun7 4:dcd52a961392 9
catalincraciun7 3:b1fde0759c94 10 Serial console(USBTX, USBRX);
catalincraciun7 0:8ebbd2bd34f5 11
catalincraciun7 5:5e12111ef01f 12 //motor control pins
catalincraciun7 5:5e12111ef01f 13 DigitalOut dirRight(PTC9);
catalincraciun7 5:5e12111ef01f 14 DigitalOut dirLeft(PTA13);
catalincraciun7 5:5e12111ef01f 15 PwmOut pwmRight(PTD5);
catalincraciun7 5:5e12111ef01f 16 PwmOut pwmLeft(PTD0);
catalincraciun7 5:5e12111ef01f 17
catalincraciun7 0:8ebbd2bd34f5 18 ZumoRobotManager::ZumoRobotManager() {
catalincraciun7 0:8ebbd2bd34f5 19 // Initialising
catalincraciun7 2:036da44b023e 20 this -> velocityX = 0;
catalincraciun7 2:036da44b023e 21 this -> velocityY = 0;
catalincraciun7 0:8ebbd2bd34f5 22 }
catalincraciun7 0:8ebbd2bd34f5 23
catalincraciun7 0:8ebbd2bd34f5 24 ZumoRobotManager::~ZumoRobotManager() {
catalincraciun7 0:8ebbd2bd34f5 25 // Deinitialising
catalincraciun7 7:d2af97b7cc94 26 }
catalincraciun7 7:d2af97b7cc94 27
catalincraciun7 7:d2af97b7cc94 28 void ZumoRobotManager::applyPowerToLeftEngine(int power) {
catalincraciun7 7:d2af97b7cc94 29 pwmLeft.period_us(60);
catalincraciun7 7:d2af97b7cc94 30 pwmLeft.pulsewidth_us(power);
catalincraciun7 7:d2af97b7cc94 31 }
catalincraciun7 7:d2af97b7cc94 32
catalincraciun7 7:d2af97b7cc94 33 void ZumoRobotManager::applyPowerToRightEngine(int power) {
catalincraciun7 7:d2af97b7cc94 34 pwmRight.period_us(60);
catalincraciun7 7:d2af97b7cc94 35 pwmRight.pulsewidth_us(power);
catalincraciun7 0:8ebbd2bd34f5 36 }
catalincraciun7 0:8ebbd2bd34f5 37
catalincraciun7 2:036da44b023e 38 float ZumoRobotManager::getVelocityX() {
catalincraciun7 2:036da44b023e 39 return this -> velocityX;
catalincraciun7 2:036da44b023e 40 }
catalincraciun7 2:036da44b023e 41
catalincraciun7 2:036da44b023e 42 float ZumoRobotManager::getVelocityY() {
catalincraciun7 2:036da44b023e 43 return this -> velocityY;
catalincraciun7 2:036da44b023e 44 }
catalincraciun7 2:036da44b023e 45
catalincraciun7 2:036da44b023e 46 void ZumoRobotManager::setVelocityX(float newValue) {
catalincraciun7 4:dcd52a961392 47 if (this -> velocityX != newValue) {
catalincraciun7 4:dcd52a961392 48 this -> velocityX = newValue;
catalincraciun7 4:dcd52a961392 49 this -> updateMotors();
catalincraciun7 4:dcd52a961392 50 }
catalincraciun7 2:036da44b023e 51 }
catalincraciun7 2:036da44b023e 52
catalincraciun7 7:d2af97b7cc94 53 void ZumoRobotManager::setVelocityY(float newValue) {
catalincraciun7 4:dcd52a961392 54 if (this -> velocityY != newValue) {
catalincraciun7 4:dcd52a961392 55 this -> velocityY = newValue;
catalincraciun7 4:dcd52a961392 56 this -> updateMotors();
catalincraciun7 4:dcd52a961392 57 }
catalincraciun7 2:036da44b023e 58 }
catalincraciun7 2:036da44b023e 59
catalincraciun7 3:b1fde0759c94 60 bool ZumoRobotManager::checkPassword(char toCheck[]) {
catalincraciun7 3:b1fde0759c94 61 if (toCheck[0] == '8' &&
catalincraciun7 3:b1fde0759c94 62 toCheck[1] == '1' &&
catalincraciun7 3:b1fde0759c94 63 toCheck[2] == '1' &&
catalincraciun7 3:b1fde0759c94 64 toCheck[3] == '1' &&
catalincraciun7 3:b1fde0759c94 65 toCheck[4] == '$')
catalincraciun7 3:b1fde0759c94 66 return true;
catalincraciun7 3:b1fde0759c94 67
catalincraciun7 3:b1fde0759c94 68 return false;
catalincraciun7 3:b1fde0759c94 69 }
catalincraciun7 3:b1fde0759c94 70
catalincraciun7 4:dcd52a961392 71 void ZumoRobotManager::updateMotors() {
catalincraciun7 4:dcd52a961392 72 float powerLeftEngine = 0;
catalincraciun7 4:dcd52a961392 73 float powerRightEngine = 0;
catalincraciun7 4:dcd52a961392 74 float dist = sqrt(velocityX*velocityX + velocityY*velocityY);
catalincraciun7 4:dcd52a961392 75
catalincraciun7 4:dcd52a961392 76 if (velocityX >= 0 && velocityY >= 0) {
catalincraciun7 4:dcd52a961392 77 // First chart
catalincraciun7 4:dcd52a961392 78 powerLeftEngine = dist * (float)maxEngPow;
catalincraciun7 4:dcd52a961392 79 powerRightEngine = powerLeftEngine * (1 - velocityX);
catalincraciun7 4:dcd52a961392 80
catalincraciun7 4:dcd52a961392 81 } else if (velocityX < 0 && velocityY >= 0) {
catalincraciun7 4:dcd52a961392 82 // Second chart
catalincraciun7 4:dcd52a961392 83 powerRightEngine = dist * (float)maxEngPow;
catalincraciun7 4:dcd52a961392 84 powerLeftEngine = powerRightEngine * (1 + velocityX);
catalincraciun7 4:dcd52a961392 85 } else if (velocityX < 0 && velocityY < 0) {
catalincraciun7 4:dcd52a961392 86 // Third chart
catalincraciun7 4:dcd52a961392 87 powerRightEngine = dist * (float)maxEngPow;
catalincraciun7 4:dcd52a961392 88 powerLeftEngine = powerRightEngine * (1 + velocityX);
catalincraciun7 4:dcd52a961392 89
catalincraciun7 4:dcd52a961392 90 powerRightEngine = -powerRightEngine;
catalincraciun7 4:dcd52a961392 91 powerLeftEngine = -powerLeftEngine;
catalincraciun7 4:dcd52a961392 92 } else if (velocityX >= 0 && velocityY < 0) {
catalincraciun7 4:dcd52a961392 93 // Fourth chart
catalincraciun7 4:dcd52a961392 94 powerLeftEngine = dist * (float)maxEngPow;
catalincraciun7 4:dcd52a961392 95 powerRightEngine = powerLeftEngine * (1 - velocityX);
catalincraciun7 4:dcd52a961392 96
catalincraciun7 4:dcd52a961392 97 powerLeftEngine = -powerLeftEngine;
catalincraciun7 4:dcd52a961392 98 powerRightEngine = -powerRightEngine;
catalincraciun7 4:dcd52a961392 99 }
catalincraciun7 4:dcd52a961392 100
catalincraciun7 4:dcd52a961392 101 // Do whatever you want with these powers
catalincraciun7 6:7740c9d8d834 102 pwmLeft.period_us(60);
catalincraciun7 6:7740c9d8d834 103 pwmRight.period_us(60);
catalincraciun7 6:7740c9d8d834 104
catalincraciun7 5:5e12111ef01f 105 if (powerLeftEngine >= 0) {
catalincraciun7 5:5e12111ef01f 106 dirLeft = 0;
catalincraciun7 5:5e12111ef01f 107 pwmLeft.pulsewidth_us(powerLeftEngine);
catalincraciun7 5:5e12111ef01f 108 } else {
catalincraciun7 5:5e12111ef01f 109 dirLeft = 1;
catalincraciun7 5:5e12111ef01f 110 pwmLeft.pulsewidth_us(-powerLeftEngine);
catalincraciun7 5:5e12111ef01f 111 }
catalincraciun7 5:5e12111ef01f 112
catalincraciun7 5:5e12111ef01f 113 if (powerRightEngine >= 0) {
catalincraciun7 5:5e12111ef01f 114 dirRight = 0;
catalincraciun7 5:5e12111ef01f 115 pwmRight.pulsewidth_us(powerRightEngine);
catalincraciun7 5:5e12111ef01f 116 } else {
catalincraciun7 5:5e12111ef01f 117 dirRight = 1;
catalincraciun7 5:5e12111ef01f 118 pwmRight.pulsewidth_us(-powerRightEngine);
catalincraciun7 5:5e12111ef01f 119 }
catalincraciun7 4:dcd52a961392 120 }