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@5:5e12111ef01f, 2014-12-11 (annotated)

Committer:
catalincraciun7
Date:
Thu Dec 11 20:52:06 2014 +0000
Revision:
5:5e12111ef01f
Parent:
4:dcd52a961392
Child:
6:7740c9d8d834
Motors done!

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