diff --git a/include/robot.h b/include/robot.h index b4fab81..46db7cc 100644 --- a/include/robot.h +++ b/include/robot.h @@ -51,11 +51,14 @@ #define XRP_DATA_AIO 0x04 #define XRP_DATA_GENERAL 0x08 +#define XRP_VIN_MEAS 28 + namespace xrp { void robotInit(); bool robotInitialized(); uint8_t robotPeriodic(); +float getVinMeasured(); // Robot control void robotSetEnabled(bool enabled); diff --git a/src/main.cpp b/src/main.cpp index 8ab86d1..662fb46 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -162,6 +162,7 @@ void sendData() { if (xrp::rangefinderInitialized()) { ptr += wpilibudp::writeAnalogData(2, xrp::getRangefinderDistance5V(), buffer, ptr); } + ptr += wpilibudp::writeAnalogData(3, xrp::getVinMeasured(), buffer, ptr); // ptr should now point to 1 past the last byte size = ptr; diff --git a/src/robot.cpp b/src/robot.cpp index 0781b29..f753d8f 100644 --- a/src/robot.cpp +++ b/src/robot.cpp @@ -327,6 +327,13 @@ float getReflectanceRight5V() { return _readAnalogPinScaled(REFLECT_RIGHT_PIN) * 5.0f; } +float getVinMeasured() { + //ADC reads are 3.3V max + //R1 = 100k, R2 = 33k, voltage divider = R2/(R1+R2) + //reading * Vadc_max / volt divider + return _readAnalogPinScaled(XRP_VIN_MEAS) * 3.3f / (33.f/(100.f+33.f)); +} + void rangefinderInit() { pinMode(ULTRASONIC_TRIG_PIN, OUTPUT); // Trigger Pin digitalWrite(ULTRASONIC_TRIG_PIN, LOW);