diff --git a/code/sender/sender.ino b/code/sender/sender.ino index 682bf248a..af0d1e1ce 100644 --- a/code/sender/sender.ino +++ b/code/sender/sender.ino @@ -87,8 +87,8 @@ int faults = 0; boolean isCharging = false; boolean stateLED = false; unsigned int chargeADCZero = 0; -RunningMedian periCurrentMeasurements; -RunningMedian chargeCurrentMeasurements; +RunningMedian *periCurrentMeasurements; +RunningMedian *chargeCurrentMeasurements; int timeSeconds = 0; @@ -187,7 +187,10 @@ void calibrateChargeCurrentSensor(){ -void setup() { +void setup() { + periCurrentMeasurements = new RunningMedian; + chargeCurrentMeasurements = new RunningMedian; + pinMode(pinIN1, OUTPUT); pinMode(pinIN2, OUTPUT); pinMode(pinEnable, OUTPUT); @@ -312,7 +315,7 @@ void loop(){ float v = 0; // determine charging current (Ampere) if (USE_CHG_CURRENT) { - chargeCurrentMeasurements.getAverage(v); + chargeCurrentMeasurements->getAverage(v); chargeCurrent = ((double)(((int)v) - ((int)chargeADCZero))) / 1023.0 * 1.1; isCharging = (abs(chargeCurrent) >= CHG_CURRENT_MIN); if (isCharging) robotOutOfStationTimeMins = 0; // reset timeout @@ -320,10 +323,10 @@ void loop(){ if (USE_PERI_CURRENT) { // determine perimeter current (Ampere) - periCurrentMeasurements.getAverage(v); + periCurrentMeasurements->getAverage(v); periCurrentAvg = ((double)v) / 1023.0 * 1.1 / 0.525; // 525 mV per amp unsigned int h; - periCurrentMeasurements.getHighest(h); + periCurrentMeasurements->getHighest(h); periCurrentMax = ((double)h) / 1023.0 * 1.1 / 0.525; // 525 mV per amp } @@ -333,7 +336,7 @@ void loop(){ Serial.print(chargeCurrent, 3); Serial.print("\tchgCurrentADC="); v=0; - chargeCurrentMeasurements.getAverage(v); + chargeCurrentMeasurements->getAverage(v); Serial.print( v ); Serial.print("\tisCharging="); Serial.print(isCharging); @@ -358,12 +361,12 @@ void loop(){ } if (USE_PERI_CURRENT) { - periCurrentMeasurements.add( analogRead(pinFeedback) ); + periCurrentMeasurements->add( analogRead(pinFeedback) ); } if (USE_CHG_CURRENT){ // determine charging current (Ampere) - chargeCurrentMeasurements.add( analogRead( pinChargeCurrent) ); + chargeCurrentMeasurements->add( analogRead( pinChargeCurrent) ); } // LED status