Szilárd Greszler / robotkocsi

Controller firmware for a mobile robot, having a K64F MCU on board. Please read README.md for details.

Dependents:   robotkocsi_OS

sensors.cpp@0:260ca1f1cba7, 2018-05-30 (annotated)

Committer:
dralisz82
Date:
Wed May 30 15:10:20 2018 +0000
Revision:
0:260ca1f1cba7
Controller firmware for a mobile robot, having a K64F MCU on board.; ; See README.md for details;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dralisz82 0:260ca1f1cba7 1 #include "sensors.h"
dralisz82 0:260ca1f1cba7 2
dralisz82 0:260ca1f1cba7 3 extern Sensors* sensors;
dralisz82 0:260ca1f1cba7 4
dralisz82 0:260ca1f1cba7 5 Sensor::Sensor() {
dralisz82 0:260ca1f1cba7 6 }
dralisz82 0:260ca1f1cba7 7
dralisz82 0:260ca1f1cba7 8 Sensor::~Sensor() {
dralisz82 0:260ca1f1cba7 9 }
dralisz82 0:260ca1f1cba7 10
dralisz82 0:260ca1f1cba7 11 void Sensor::setId(char *sId) {
dralisz82 0:260ca1f1cba7 12 strcpy(this->sId, sId);
dralisz82 0:260ca1f1cba7 13 }
dralisz82 0:260ca1f1cba7 14
dralisz82 0:260ca1f1cba7 15 void Sensor::setName(char *name) {
dralisz82 0:260ca1f1cba7 16 strcpy(this->name, name);
dralisz82 0:260ca1f1cba7 17 }
dralisz82 0:260ca1f1cba7 18
dralisz82 0:260ca1f1cba7 19 void Sensor::setMetric(char *metric) {
dralisz82 0:260ca1f1cba7 20 strcpy(this->metric, metric);
dralisz82 0:260ca1f1cba7 21 }
dralisz82 0:260ca1f1cba7 22
dralisz82 0:260ca1f1cba7 23 void Sensor::setFunction(float (*function)(void)) {
dralisz82 0:260ca1f1cba7 24 this->function = function;
dralisz82 0:260ca1f1cba7 25 }
dralisz82 0:260ca1f1cba7 26
dralisz82 0:260ca1f1cba7 27 char* Sensor::getId() {
dralisz82 0:260ca1f1cba7 28 return sId;
dralisz82 0:260ca1f1cba7 29 }
dralisz82 0:260ca1f1cba7 30
dralisz82 0:260ca1f1cba7 31 char* Sensor::getName() {
dralisz82 0:260ca1f1cba7 32 return name;
dralisz82 0:260ca1f1cba7 33 }
dralisz82 0:260ca1f1cba7 34
dralisz82 0:260ca1f1cba7 35 char* Sensor::getMetric() {
dralisz82 0:260ca1f1cba7 36 return metric;
dralisz82 0:260ca1f1cba7 37 }
dralisz82 0:260ca1f1cba7 38
dralisz82 0:260ca1f1cba7 39 float Sensor::readValue() {
dralisz82 0:260ca1f1cba7 40 return (*function)();
dralisz82 0:260ca1f1cba7 41 }
dralisz82 0:260ca1f1cba7 42
dralisz82 0:260ca1f1cba7 43 Sensors::Sensors() {
dralisz82 0:260ca1f1cba7 44 // create peripheral objects
dralisz82 0:260ca1f1cba7 45 analogInputs[0] = new AnalogIn(PTB2);
dralisz82 0:260ca1f1cba7 46 analogInputs[1] = new AnalogIn(PTB3);
dralisz82 0:260ca1f1cba7 47 analogInputs[2] = new AnalogIn(PTB10);
dralisz82 0:260ca1f1cba7 48 analogInputs[3] = new AnalogIn(PTB11);
dralisz82 0:260ca1f1cba7 49 analogInputs[4] = new AnalogIn(PTC11);
dralisz82 0:260ca1f1cba7 50 analogInputs[5] = new AnalogIn(PTC10);
dralisz82 0:260ca1f1cba7 51
dralisz82 0:260ca1f1cba7 52 sensNum = 0;
dralisz82 0:260ca1f1cba7 53 createS1();
dralisz82 0:260ca1f1cba7 54 createS2();
dralisz82 0:260ca1f1cba7 55 createS3();
dralisz82 0:260ca1f1cba7 56 createVB();
dralisz82 0:260ca1f1cba7 57 createVM();
dralisz82 0:260ca1f1cba7 58 createVL();
dralisz82 0:260ca1f1cba7 59 }
dralisz82 0:260ca1f1cba7 60
dralisz82 0:260ca1f1cba7 61 Sensors::~Sensors() {
dralisz82 0:260ca1f1cba7 62 for(int i=0;i<=5;i++)
dralisz82 0:260ca1f1cba7 63 delete analogInputs[i];
dralisz82 0:260ca1f1cba7 64 }
dralisz82 0:260ca1f1cba7 65
dralisz82 0:260ca1f1cba7 66 Sensor* Sensors::getSensor(char *sId) {
dralisz82 0:260ca1f1cba7 67 if(!strcmp(sId, "vBattS1"))
dralisz82 0:260ca1f1cba7 68 return &sensArr[SENS_VBATTS1];
dralisz82 0:260ca1f1cba7 69 else if(!strcmp(sId, "vBattS2"))
dralisz82 0:260ca1f1cba7 70 return &sensArr[SENS_VBATTS2];
dralisz82 0:260ca1f1cba7 71 else if(!strcmp(sId, "vBattS3"))
dralisz82 0:260ca1f1cba7 72 return &sensArr[SENS_VBATTS3];
dralisz82 0:260ca1f1cba7 73 else if(!strcmp(sId, "vBatt"))
dralisz82 0:260ca1f1cba7 74 return &sensArr[SENS_VBATT];
dralisz82 0:260ca1f1cba7 75 else if(!strcmp(sId, "vMotor"))
dralisz82 0:260ca1f1cba7 76 return &sensArr[SENS_VMOTOR];
dralisz82 0:260ca1f1cba7 77 else if(!strcmp(sId, "vLogic"))
dralisz82 0:260ca1f1cba7 78 return &sensArr[SENS_VLOGIC];
dralisz82 0:260ca1f1cba7 79 return NULL;
dralisz82 0:260ca1f1cba7 80 }
dralisz82 0:260ca1f1cba7 81
dralisz82 0:260ca1f1cba7 82 AnalogIn* Sensors::getAnalogIn(int aiId) {
dralisz82 0:260ca1f1cba7 83 if(aiId >= 0 && aiId <= 5)
dralisz82 0:260ca1f1cba7 84 return analogInputs[aiId];
dralisz82 0:260ca1f1cba7 85 return NULL;
dralisz82 0:260ca1f1cba7 86 }
dralisz82 0:260ca1f1cba7 87
dralisz82 0:260ca1f1cba7 88 float readS1() {
dralisz82 0:260ca1f1cba7 89 float factor = 3.3;// TODO
dralisz82 0:260ca1f1cba7 90 return sensors->getAnalogIn(0)->read()*factor;
dralisz82 0:260ca1f1cba7 91 }
dralisz82 0:260ca1f1cba7 92
dralisz82 0:260ca1f1cba7 93 void Sensors::createS1() {
dralisz82 0:260ca1f1cba7 94 Sensor *s = &sensArr[sensNum++];
dralisz82 0:260ca1f1cba7 95 s->setId("vBattS1");
dralisz82 0:260ca1f1cba7 96 s->setName("battery cell #1 voltage");
dralisz82 0:260ca1f1cba7 97 s->setMetric("V");
dralisz82 0:260ca1f1cba7 98 s->setFunction(readS1);
dralisz82 0:260ca1f1cba7 99 }
dralisz82 0:260ca1f1cba7 100
dralisz82 0:260ca1f1cba7 101 float readS2() {
dralisz82 0:260ca1f1cba7 102 float factor = 3.3;// TODO
dralisz82 0:260ca1f1cba7 103 return (sensors->getAnalogIn(1)->read() - sensors->getAnalogIn(0)->read())*factor;
dralisz82 0:260ca1f1cba7 104 }
dralisz82 0:260ca1f1cba7 105
dralisz82 0:260ca1f1cba7 106 void Sensors::createS2() {
dralisz82 0:260ca1f1cba7 107 Sensor *s = &sensArr[sensNum++];
dralisz82 0:260ca1f1cba7 108 s->setId("vBattS2");
dralisz82 0:260ca1f1cba7 109 s->setName("battery cell #2 voltage");
dralisz82 0:260ca1f1cba7 110 s->setMetric("V");
dralisz82 0:260ca1f1cba7 111 s->setFunction(readS2);
dralisz82 0:260ca1f1cba7 112 }
dralisz82 0:260ca1f1cba7 113
dralisz82 0:260ca1f1cba7 114 float readS3() {
dralisz82 0:260ca1f1cba7 115 float factor = 3.3;// TODO
dralisz82 0:260ca1f1cba7 116 return (sensors->getAnalogIn(2)->read() - sensors->getAnalogIn(1)->read())*factor;
dralisz82 0:260ca1f1cba7 117 }
dralisz82 0:260ca1f1cba7 118
dralisz82 0:260ca1f1cba7 119 void Sensors::createS3() {
dralisz82 0:260ca1f1cba7 120 Sensor *s = &sensArr[sensNum++];
dralisz82 0:260ca1f1cba7 121 s->setId("vBattS3");
dralisz82 0:260ca1f1cba7 122 s->setName("battery cell #3 voltage");
dralisz82 0:260ca1f1cba7 123 s->setMetric("V");
dralisz82 0:260ca1f1cba7 124 s->setFunction(readS3);
dralisz82 0:260ca1f1cba7 125 }
dralisz82 0:260ca1f1cba7 126
dralisz82 0:260ca1f1cba7 127 float readVB() {
dralisz82 0:260ca1f1cba7 128 float factor = 3.3;// TODO
dralisz82 0:260ca1f1cba7 129 return sensors->getAnalogIn(2)->read()*factor;
dralisz82 0:260ca1f1cba7 130 }
dralisz82 0:260ca1f1cba7 131
dralisz82 0:260ca1f1cba7 132 void Sensors::createVB() {
dralisz82 0:260ca1f1cba7 133 Sensor *s = &sensArr[sensNum++];
dralisz82 0:260ca1f1cba7 134 s->setId("vBatt");
dralisz82 0:260ca1f1cba7 135 s->setName("battery voltage");
dralisz82 0:260ca1f1cba7 136 s->setMetric("V");
dralisz82 0:260ca1f1cba7 137 s->setFunction(readVB);
dralisz82 0:260ca1f1cba7 138 }
dralisz82 0:260ca1f1cba7 139
dralisz82 0:260ca1f1cba7 140 float readVM() {
dralisz82 0:260ca1f1cba7 141 float factor = 3.3;// TODO
dralisz82 0:260ca1f1cba7 142 return sensors->getAnalogIn(3)->read()*factor;
dralisz82 0:260ca1f1cba7 143 }
dralisz82 0:260ca1f1cba7 144
dralisz82 0:260ca1f1cba7 145 void Sensors::createVM() {
dralisz82 0:260ca1f1cba7 146 Sensor *s = &sensArr[sensNum++];
dralisz82 0:260ca1f1cba7 147 s->setId("vMotor");
dralisz82 0:260ca1f1cba7 148 s->setName("motor voltage");
dralisz82 0:260ca1f1cba7 149 s->setMetric("V");
dralisz82 0:260ca1f1cba7 150 s->setFunction(readVM);
dralisz82 0:260ca1f1cba7 151 }
dralisz82 0:260ca1f1cba7 152
dralisz82 0:260ca1f1cba7 153 float readVL() {
dralisz82 0:260ca1f1cba7 154 float factor = 3.3;// TODO
dralisz82 0:260ca1f1cba7 155 return sensors->getAnalogIn(4)->read()*factor;
dralisz82 0:260ca1f1cba7 156 }
dralisz82 0:260ca1f1cba7 157
dralisz82 0:260ca1f1cba7 158 void Sensors::createVL() {
dralisz82 0:260ca1f1cba7 159 Sensor *s = &sensArr[sensNum++];
dralisz82 0:260ca1f1cba7 160 s->setId("vLogic");
dralisz82 0:260ca1f1cba7 161 s->setName("logic voltage");
dralisz82 0:260ca1f1cba7 162 s->setMetric("V");
dralisz82 0:260ca1f1cba7 163 s->setFunction(readVL);
dralisz82 0:260ca1f1cba7 164 }