Netcentric Computing / MotorController

progress...

Dependents:   uva_nc

Fork of MotorController by Thijs Riezebeek

MotorController.cpp@2:70607c6dad36, 2016-01-10 (annotated)

Committer:
Sinterbaas
Date:
Sun Jan 10 23:46:12 2016 +0000
Revision:
2:70607c6dad36
Parent:
1:ed42d9035468 
All of the latest changes (inc. Motor stuck detection)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Sinterbaas 0:ec2f323f6388 1 #include "MotorController.h"
Sinterbaas 0:ec2f323f6388 2
Sinterbaas 0:ec2f323f6388 3 MotorController::MotorController (HBridge &motor, AnalogIn &motorInput) :
Sinterbaas 2:70607c6dad36 4 motor ( motor ), motorInput ( motorInput ), currentStatus ( MotorController::STATUS_OK ),
Sinterbaas 2:70607c6dad36 5 bufferCount ( 0 ), bufferIndex ( 0 ) {
Sinterbaas 0:ec2f323f6388 6 }
Sinterbaas 0:ec2f323f6388 7
Sinterbaas 0:ec2f323f6388 8 void MotorController::start () {
Sinterbaas 0:ec2f323f6388 9 this->motor.start();
Sinterbaas 0:ec2f323f6388 10 }
Sinterbaas 0:ec2f323f6388 11
Sinterbaas 0:ec2f323f6388 12 void MotorController::stop () {
Sinterbaas 0:ec2f323f6388 13 this->motor.stop();
Sinterbaas 0:ec2f323f6388 14 }
Sinterbaas 0:ec2f323f6388 15
Sinterbaas 0:ec2f323f6388 16 void MotorController::turnLeft () {
Sinterbaas 2:70607c6dad36 17 this->CheckMotorStatus();
Sinterbaas 2:70607c6dad36 18
Sinterbaas 0:ec2f323f6388 19 this->motor.start();
Sinterbaas 0:ec2f323f6388 20 this->motor.backward();
Sinterbaas 0:ec2f323f6388 21 }
Sinterbaas 0:ec2f323f6388 22
Sinterbaas 0:ec2f323f6388 23
Sinterbaas 0:ec2f323f6388 24 void MotorController::turnRight () {
Sinterbaas 2:70607c6dad36 25 this->CheckMotorStatus();
Sinterbaas 2:70607c6dad36 26
Sinterbaas 0:ec2f323f6388 27 this->motor.start();
Sinterbaas 0:ec2f323f6388 28 this->motor.forward();
Sinterbaas 0:ec2f323f6388 29 }
Sinterbaas 0:ec2f323f6388 30
Sinterbaas 0:ec2f323f6388 31 void MotorController::setPosition (float position) {
Sinterbaas 0:ec2f323f6388 32 float current_position = this->getPosition();
Sinterbaas 0:ec2f323f6388 33
Sinterbaas 2:70607c6dad36 34 if (current_position <= position * (1 + MARGIN_PERCENTAGE) + MARGIN_FIXED &&
Sinterbaas 2:70607c6dad36 35 current_position >= position * (1 - MARGIN_PERCENTAGE) - MARGIN_FIXED) {
jurgy 1:ed42d9035468 36 this->stop();
Sinterbaas 2:70607c6dad36 37 this->currentStatus = STATUS_OK;
Sinterbaas 2:70607c6dad36 38 return;
Sinterbaas 2:70607c6dad36 39 }
Sinterbaas 2:70607c6dad36 40 else {
Sinterbaas 2:70607c6dad36 41 this->CheckMotorStatus();
Sinterbaas 2:70607c6dad36 42 }
Sinterbaas 2:70607c6dad36 43
Sinterbaas 2:70607c6dad36 44
Sinterbaas 2:70607c6dad36 45 if (current_position > position) {
Sinterbaas 0:ec2f323f6388 46 this->turnLeft();
Sinterbaas 0:ec2f323f6388 47 }
Sinterbaas 0:ec2f323f6388 48 else {
Sinterbaas 0:ec2f323f6388 49 this->turnRight();
Sinterbaas 0:ec2f323f6388 50 }
Sinterbaas 0:ec2f323f6388 51 }
Sinterbaas 0:ec2f323f6388 52
Sinterbaas 0:ec2f323f6388 53
Sinterbaas 0:ec2f323f6388 54 float MotorController::getPosition () {
Sinterbaas 0:ec2f323f6388 55 return this->motorInput.read() * 100.0f;
jurgy 1:ed42d9035468 56 }
jurgy 1:ed42d9035468 57
jurgy 1:ed42d9035468 58 void MotorController::setCurrentAction(char c) {
jurgy 1:ed42d9035468 59 this->currentAction = c;
jurgy 1:ed42d9035468 60 }
jurgy 1:ed42d9035468 61
jurgy 1:ed42d9035468 62 char MotorController::getCurrentAction() {
jurgy 1:ed42d9035468 63 return this->currentAction;
Sinterbaas 2:70607c6dad36 64 }
Sinterbaas 2:70607c6dad36 65
Sinterbaas 2:70607c6dad36 66 void MotorController::setCurrentStatus(char c) {
Sinterbaas 2:70607c6dad36 67 this->currentStatus = c;
Sinterbaas 2:70607c6dad36 68 }
Sinterbaas 2:70607c6dad36 69
Sinterbaas 2:70607c6dad36 70 char MotorController::getCurrentStatus () {
Sinterbaas 2:70607c6dad36 71 return this->currentStatus;
Sinterbaas 2:70607c6dad36 72 }
Sinterbaas 2:70607c6dad36 73
Sinterbaas 2:70607c6dad36 74 void MotorController::addLatestPosition (float position) {
Sinterbaas 2:70607c6dad36 75 this->bufferIndex = (this->bufferIndex + 1) % BUFFER_SIZE;
Sinterbaas 2:70607c6dad36 76
Sinterbaas 2:70607c6dad36 77 this->positionBuffer[bufferIndex] = position;
Sinterbaas 2:70607c6dad36 78
Sinterbaas 2:70607c6dad36 79 if (this->bufferCount < BUFFER_SIZE) {
Sinterbaas 2:70607c6dad36 80 bufferCount += 1;
Sinterbaas 2:70607c6dad36 81 }
Sinterbaas 2:70607c6dad36 82 }
Sinterbaas 2:70607c6dad36 83
Sinterbaas 2:70607c6dad36 84 bool MotorController::isMotorStuck () {
Sinterbaas 2:70607c6dad36 85 float sum = 0;
Sinterbaas 2:70607c6dad36 86 for (int i = 0; i < bufferCount; i++) {
Sinterbaas 2:70607c6dad36 87 sum += this->positionBuffer[i];
Sinterbaas 2:70607c6dad36 88 }
Sinterbaas 2:70607c6dad36 89
Sinterbaas 2:70607c6dad36 90 float average = sum / bufferCount;
Sinterbaas 2:70607c6dad36 91 float allowed_deviation = average * MARGIN_PERCENTAGE + 0.03f;
Sinterbaas 2:70607c6dad36 92 float deviation = 0;
Sinterbaas 2:70607c6dad36 93
Sinterbaas 2:70607c6dad36 94 for (int i = 0; i < bufferCount; i++) {
Sinterbaas 2:70607c6dad36 95 deviation += (average - this->positionBuffer[i]) * (average - this->positionBuffer[i]);
Sinterbaas 2:70607c6dad36 96 }
Sinterbaas 2:70607c6dad36 97
Sinterbaas 2:70607c6dad36 98 printf("Status: %f < %f\r\n", deviation, allowed_deviation);
Sinterbaas 2:70607c6dad36 99
Sinterbaas 2:70607c6dad36 100 return deviation < allowed_deviation;
Sinterbaas 2:70607c6dad36 101 }
Sinterbaas 2:70607c6dad36 102
Sinterbaas 2:70607c6dad36 103 void MotorController::CheckMotorStatus () {
Sinterbaas 2:70607c6dad36 104 this->addLatestPosition(this->getPosition());
Sinterbaas 2:70607c6dad36 105
Sinterbaas 2:70607c6dad36 106 if (this->isMotorStuck()) {
Sinterbaas 2:70607c6dad36 107 this->currentStatus = STATUS_STUCK;
Sinterbaas 2:70607c6dad36 108 }
Sinterbaas 2:70607c6dad36 109 else {
Sinterbaas 2:70607c6dad36 110 this->currentStatus = STATUS_OK;
Sinterbaas 2:70607c6dad36 111 }
Sinterbaas 0:ec2f323f6388 112 }