From 65a9b589cf9f4e5e7992b190185a8299da31e4c9 Mon Sep 17 00:00:00 2001 From: Michael Neuweiler Date: Tue, 1 Oct 2013 01:27:51 +0200 Subject: [PATCH] moved stuff to MotorController, added new vars --- .cproject | 6 - .project.copy | 16 +- BrusaMotorController.cpp | 86 +++----- BrusaMotorController.h | 33 ++- CanThrottle.cpp | 2 +- DmocMotorController.cpp | 51 ++--- DmocMotorController.h | 8 - MotorController.cpp | 170 ++++++++------- MotorController.h | 154 ++++++++------ SerialConsole.cpp | 434 +++++++++++++++------------------------ SerialConsole.h | 5 - Throttle.cpp | 2 +- Throttle.h | 4 +- ichip_2128.cpp | 19 +- website/src/status.htm | 21 ++ website/src/status.xml | 6 + website/website.img | Bin 14069 -> 14991 bytes 17 files changed, 476 insertions(+), 541 deletions(-) diff --git a/.cproject b/.cproject index 440b579..2eef6ae 100644 --- a/.cproject +++ b/.cproject @@ -25,9 +25,6 @@ - - - @@ -39,9 +36,6 @@ - - - diff --git a/.project.copy b/.project.copy index 2318716..25abf59 100644 --- a/.project.copy +++ b/.project.copy @@ -31,16 +31,6 @@ 2 ArduinoPivateLibPath/DueTimer - - Libraries/Ethernet - 2 - ArduinoHardwareLibPath/Ethernet - - - Libraries/SPI - 2 - ArduinoHardwareLibPath/SPI - Libraries/due_can 2 @@ -70,15 +60,15 @@ ArduinoHardwareLibPath - file:/data/development/arduino/arduino-1.5.2_linux_64bit/hardware/arduino/sam/libraries + file:/Z:/development/arduino/arduino-1.5.2_win_32bit/hardware/arduino/sam/libraries ArduinoPinPath - file:/data/development/arduino/arduino-1.5.2_linux_64bit/hardware/arduino/sam/variants + file:/Z:/development/arduino/arduino-1.5.2_win_32bit/hardware/arduino/sam/variants ArduinoPlatformPath - file:/data/development/arduino/arduino-1.5.2_linux_64bit/hardware/arduino/sam + file:/Z:/development/arduino/arduino-1.5.2_win_32bit/hardware/arduino/sam diff --git a/BrusaMotorController.cpp b/BrusaMotorController.cpp index 31ab7f1..b399613 100644 --- a/BrusaMotorController.cpp +++ b/BrusaMotorController.cpp @@ -36,40 +36,20 @@ ing torque that can't be controlled. */ -BrusaMotorController::BrusaMotorController() { - dmcReady = false; - running = false; - faulted = false; - dmcWarning = false; +BrusaMotorController::BrusaMotorController() : MotorController() { torqueAvailable = 0; - torqueActual = 0; - speedActual = 0; - - dcVoltage = 0; - dcCurrent = 0; - acCurrent = 0; - mechanicalPower = 0; - - temperatureInverter = 0; - temperatureMotor = 0; - temperatureSystem = 0; + maxPositiveTorque = 0; + minNegativeTorque = 0; errorBitField = 0; warningBitField = 0; statusBitField = 0; - - maxPositiveTorque = 0; - minNegativeTorque = 0; limiterStateNumber = 0; tickCounter = 0; - powerMode = modeTorque; - maxTorque = 20; // TODO: only for testing, in tenths Nm, so 2Nm max torque, remove for production use - maxRPM = 2000; // TODO: only for testing, remove for production use - requestedRPM = 0; - requestedTorque = 0; - requestedThrottle = 0; + torqueMax = 20; // TODO: only for testing, in tenths Nm, so 2Nm max torque, remove for production use + speedMax = 2000; // TODO: only for testing, remove for production use } void BrusaMotorController::setup() { @@ -99,11 +79,11 @@ void BrusaMotorController::sendControl() { //TODO: remove ramp testing - requestedTorque = 20; + torqueRequested = 20; if (speedActual < 100) - requestedRPM = 1000; + speedRequested = 1000; if (speedActual > 950) - requestedRPM = 50; + speedRequested = 50; @@ -111,7 +91,7 @@ void BrusaMotorController::sendControl() { if (faulted) { outputFrame.data[0] |= clearErrorLatch; } else { - if (dmcReady || speedActual > 1000) { // see warning about field weakening current to prevent uncontrollable regen + if (running || speedActual > 1000) { // see warning about field weakening current to prevent uncontrollable regen outputFrame.data[0] |= enablePowerStage; if (running) { // outputFrame.data[0] |= enableOscillationLimiter; @@ -122,12 +102,12 @@ void BrusaMotorController::sendControl() { // TODO: check for maxRPM and maxTorque // set the speed in rpm - outputFrame.data[2] = (requestedRPM & 0xFF00) >> 8; - outputFrame.data[3] = (requestedRPM & 0x00FF); + outputFrame.data[2] = (speedRequested & 0xFF00) >> 8; + outputFrame.data[3] = (speedRequested & 0x00FF); // set the torque in 0.01Nm (GEVCU uses 0.1Nm -> multiply by 10) - outputFrame.data[4] = ((requestedTorque * 10) & 0xFF00) >> 8; - outputFrame.data[5] = ((requestedTorque * 10) & 0x00FF); + outputFrame.data[4] = ((torqueRequested * 10) & 0xFF00) >> 8; + outputFrame.data[5] = ((torqueRequested * 10) & 0x00FF); } } } @@ -194,19 +174,15 @@ void BrusaMotorController::handleCanFrame(RX_CAN_FRAME* frame) { switch (frame->id) { case CAN_ID_STATUS: statusBitField = frame->data[1] | (frame->data[0] << 8); - torqueAvailable = frame->data[3] | (frame->data[2] << 8); - torqueActual = frame->data[5] | (frame->data[4] << 8); + torqueAvailable = (frame->data[3] | (frame->data[2] << 8)) / 10; + torqueActual = (frame->data[5] | (frame->data[4] << 8)) / 10; speedActual = frame->data[7] | (frame->data[6] << 8); - //TODO: replace mapping - actualTorque = torqueActual / 10; //in tenths Nm - actualRPM = speedActual; //in RPM - if(Logger::isDebug()) Logger::debug(BRUSA_DMC5, "status: %X, torque avail: %fNm, actual torque: %fNm, speed actual: %drpm", statusBitField, (float)torqueAvailable/100.0F, (float)torqueActual/100.0F, speedActual); - dmcReady = (statusBitField & stateReady) != 0 ? true : false; - if (dmcReady) + ready = (statusBitField & stateReady) != 0 ? true : false; + if (ready) Logger::info(BRUSA_DMC5, "ready"); running = (statusBitField & stateRunning) != 0 ? true : false; @@ -217,8 +193,8 @@ void BrusaMotorController::handleCanFrame(RX_CAN_FRAME* frame) { if (faulted) Logger::error(BRUSA_DMC5, "error is present, see error message"); - dmcWarning = (statusBitField & warningFlag) != 0 ? true : false; - if (dmcWarning) + warning = (statusBitField & warningFlag) != 0 ? true : false; + if (warning) Logger::warn(BRUSA_DMC5, "warning is present, see warning message"); if (statusBitField & motorModelLimitation) @@ -248,11 +224,11 @@ void BrusaMotorController::handleCanFrame(RX_CAN_FRAME* frame) { case CAN_ID_ACTUAL_VALUES: dcVoltage = frame->data[1] | (frame->data[0] << 8); dcCurrent = frame->data[3] | (frame->data[2] << 8); - acCurrent = frame->data[5] | (frame->data[4] << 8); - mechanicalPower = frame->data[7] | (frame->data[6] << 8); + acCurrent = (frame->data[5] | (frame->data[4] << 8)) / 2.5; + mechanicalPower = (frame->data[7] | (frame->data[6] << 8)) / 6.25; if (Logger::isDebug()) - Logger::debug(BRUSA_DMC5, "actual values: DC Volts: %fV, DC current: %fA, AC current: %fA, mechPower: %fkW", (float)dcVoltage / 10.0F, (float)dcCurrent / 10.0F, (float)acCurrent / 4.0F, (float)mechanicalPower / 62.5F); + Logger::debug(BRUSA_DMC5, "actual values: DC Volts: %fV, DC current: %fA, AC current: %fA, mechPower: %fkW", (float)dcVoltage / 10.0F, (float)dcCurrent / 10.0F, (float)acCurrent / 10.0F, (float)mechanicalPower / 10.0F); break; case CAN_ID_ERRORS: @@ -344,25 +320,21 @@ void BrusaMotorController::handleCanFrame(RX_CAN_FRAME* frame) { break; case CAN_ID_TORQUE_LIMIT: - maxPositiveTorque = frame->data[1] | (frame->data[0] << 8); - minNegativeTorque = frame->data[3] | (frame->data[2] << 8); + maxPositiveTorque = (frame->data[1] | (frame->data[0] << 8)) / 10; + minNegativeTorque = (frame->data[3] | (frame->data[2] << 8)) / 10; limiterStateNumber = frame->data[4]; if (Logger::isDebug()) - Logger::debug(BRUSA_DMC5, "torque limit: max positive: %fNm, min negative: %fNm", (float) maxPositiveTorque / 100.0F, (float) minNegativeTorque / 100.0F, limiterStateNumber); + Logger::debug(BRUSA_DMC5, "torque limit: max positive: %fNm, min negative: %fNm", (float) maxPositiveTorque / 10.0F, (float) minNegativeTorque / 10.0F, limiterStateNumber); break; case CAN_ID_TEMP: - temperatureInverter = frame->data[1] | (frame->data[0] << 8); - temperatureMotor = frame->data[3] | (frame->data[2] << 8); - temperatureSystem = frame->data[4]; - - //TODO: replace mapping - motorTemp = temperatureMotor * 5; //temperature of motor in tenths of degree C - inverterTemp = temperatureInverter * 5; //temperature of inverter in tenths deg C + temperatureInverter = (frame->data[1] | (frame->data[0] << 8)) * 5; + temperatureMotor = (frame->data[3] | (frame->data[2] << 8)) * 5; + temperatureSystem = (frame->data[4] - 50) * 10; if (Logger::isDebug()) - Logger::debug(BRUSA_DMC5, "temperature: inverter: %f°C, motor: %f°C, system: %d°C", (float)temperatureInverter / 2.0F, (float)temperatureMotor / 2.0F, temperatureSystem - 50); + Logger::debug(BRUSA_DMC5, "temperature: inverter: %fC, motor: %fC, system: %fC", (float)temperatureInverter / 10.0F, (float)temperatureMotor / 10.0F, (float)temperatureSystem / 10.0F); break; default: diff --git a/BrusaMotorController.h b/BrusaMotorController.h index 40bc3d0..182d1c4 100644 --- a/BrusaMotorController.h +++ b/BrusaMotorController.h @@ -134,11 +134,6 @@ class BrusaMotorController: public MotorController, CanObserver { enablePowerStage = 1 << 7 // 0x80 }; - enum PowerMode { - modeTorque, - modeSpeed - }; - void handleTick(); void handleCanFrame(RX_CAN_FRAME *frame); void setup(); @@ -147,24 +142,24 @@ class BrusaMotorController: public MotorController, CanObserver { private: // DMC_TRQS - boolean dmcReady; // indicates that the controller is ready, meaning no error is present and HV is available - //boolean running; // indicates that the controller's powerstage is operating - //boolean faulted; // indicates the presence of an error (refer to errorBitField, reported in separate message), enabling powerstage is not possible - boolean dmcWarning; // indicates the presence of a warning (refer to warningBitField, reported in separate message) - int16_t torqueAvailable; // the maximum available torque in 0.01Nm -> divide by 100 to get Nm - int16_t torqueActual; // the actual torque in 0.01Nm -> divide by 100 to get Nm - int16_t speedActual; // the actual speed of the motor in rpm +// boolean dmcReady; // indicates that the controller is ready, meaning no error is present and HV is available +// boolean dmcRunning; // indicates that the controller's powerstage is operating +// boolean dmcError; // indicates the presence of an error (refer to errorBitField, reported in separate message), enabling powerstage is not possible +// boolean dmcWarning; // indicates the presence of a warning (refer to warningBitField, reported in separate message) +// int16_t torqueAvailable; // the maximum available torque in 0.01Nm -> divide by 100 to get Nm +// int16_t torqueActual; // the actual torque in 0.01Nm -> divide by 100 to get Nm +// int16_t speedActual; // the actual speed of the motor in rpm // DMC_ACTV - uint16_t dcVoltage; // DC voltage in 0.1 Volts -> divide value by 10 to get Volts - int16_t dcCurrent; // DC current in 0.1 Amps -> divide value by 10 to get Amps - uint16_t acCurrent; // AC current in 0.25 Amps -> divide value by 4 to get Amps - int16_t mechanicalPower; // mechanical power of the motor in 0.016 kW -> divide value by 62.5 to get kW +// uint16_t dcVoltage; // DC voltage in 0.1 Volts -> divide value by 10 to get Volts +// int16_t dcCurrent; // DC current in 0.1 Amps -> divide value by 10 to get Amps +// uint16_t acCurrent; // AC current in 0.25 Amps -> divide value by 4 to get Amps +// int16_t mechanicalPower; // mechanical power of the motor in 0.016 kW -> divide value by 62.5 to get kW // DMC_TEMP - int16_t temperatureInverter; // the temperature of the controller powerstage in 0.5°C -> divide by 2 to get °C - int16_t temperatureMotor; // the motor temperature in 0.5°C - uint8_t temperatureSystem; // the temperature of the controller system in °C with offset of +50°C -> subtract 50°C +// int16_t temperatureInverter; // the temperature of the controller powerstage in 0.5�C -> divide by 2 to get �C +// int16_t temperatureMotor; // the motor temperature in 0.5�C +// uint8_t temperatureSystem; // the temperature of the controller system in �C with offset of +50�C -> subtract 50�C // DMC_ERR uint32_t errorBitField; // bits holding the errors defined in enum Error diff --git a/CanThrottle.cpp b/CanThrottle.cpp index 7618072..6d99b37 100644 --- a/CanThrottle.cpp +++ b/CanThrottle.cpp @@ -94,7 +94,7 @@ void CanThrottle::handleCanFrame(RX_CAN_FRAME *frame) { } } -Device::DeviceId CanThrottle::getId() { +DeviceId CanThrottle::getId() { return CANACCELPEDAL; } diff --git a/DmocMotorController.cpp b/DmocMotorController.cpp index f8a14fd..45fc34a 100644 --- a/DmocMotorController.cpp +++ b/DmocMotorController.cpp @@ -53,7 +53,6 @@ DmocMotorController::DmocMotorController() : MotorController() { operationState = DISABLED; actualState = DISABLED; online = 0; - powerMode = MODE_TORQUE; // maxTorque = 2000; } @@ -75,20 +74,20 @@ void DmocMotorController::handleCanFrame(RX_CAN_FRAME *frame) { RotorTemp = frame->data[0]; invTemp = frame->data[1]; StatorTemp = frame->data[2]; - inverterTemp = invTemp * 10; + temperatureInverter = invTemp * 10; //now pick highest of motor temps and report it if (RotorTemp > StatorTemp) { - motorTemp = RotorTemp * 10; + temperatureMotor = RotorTemp * 10; } else { - motorTemp = StatorTemp * 10; + temperatureMotor = StatorTemp * 10; } break; case 0x23A: //torque report - actualTorque = ((frame->data[0] * 256) + frame->data[1]) - 30000; + torqueActual = ((frame->data[0] * 256) + frame->data[1]) - 30000; break; case 0x23B: //speed and current operation status - actualRPM = ((frame->data[0] * 256) + frame->data[1]) - 20000; + speedActual = ((frame->data[0] * 256) + frame->data[1]) - 20000; temp = (OperationState) (frame->data[6] >> 4); //actually, the above is an operation status report which doesn't correspond //to the state enum so translate here. @@ -158,7 +157,7 @@ void DmocMotorController::handleTick() { //if (getDigital(0)) { //setGear(DRIVE); //runThrottle = true; - setPowerMode(MODE_TORQUE); + setPowerMode(modeTorque); //} //but, if the second input is high we cancel the whole thing and disable the drive. @@ -189,12 +188,12 @@ void DmocMotorController::sendCmd1() { output.ide = 0; //standard frame output.rtr = 0; - if (requestedThrottle > 0 && operationState == ENABLE && selectedGear != NEUTRAL && powerMode == MODE_RPM) - requestedRPM = 20000 + (((long) requestedThrottle * (long) maxRPM) / 1000); + if (throttleRequested > 0 && operationState == ENABLE && selectedGear != NEUTRAL && powerMode == modeSpeed) + speedRequested = 20000 + (((long) throttleRequested * (long) speedMax) / 1000); else - requestedRPM = 20000; - output.data[0] = (requestedRPM & 0xFF00) >> 8; - output.data[1] = (requestedRPM & 0x00FF); + speedRequested = 20000; + output.data[0] = (speedRequested & 0xFF00) >> 8; + output.data[1] = (speedRequested & 0x00FF); output.data[2] = 0; //not used output.data[3] = 0; //not used output.data[4] = 0; //not used @@ -232,25 +231,25 @@ void DmocMotorController::sendCmd2() { //MaxTorque is in tenths like it should be. //Requested throttle is [-1000, 1000] //data 0-1 is upper limit, 2-3 is lower limit. They are set to same value to lock torque to this value - //requestedTorque = 30000L + (((long)requestedThrottle * (long)MaxTorque) / 1000L); + //torqueRequested = 30000L + (((long)throttleRequested * (long)MaxTorque) / 1000L); - requestedTorque = 30000; //set upper torque to zero if not drive enabled - if (powerMode == MODE_TORQUE) { + torqueRequested = 30000; //set upper torque to zero if not drive enabled + if (powerMode == modeTorque) { if (actualState == ENABLE) { //don't even try sending torque commands until the DMOC reports it is ready if (selectedGear == DRIVE) //Logger::debug("Drive - ENABLED - Torque Mode - Go!"); - requestedTorque = 30000L + (((long) requestedThrottle * (long) maxTorque) / 1000L); + torqueRequested = 30000L + (((long) throttleRequested * (long) torqueMax) / 1000L); if (selectedGear == REVERSE) - requestedTorque = 30000L - (((long) requestedThrottle * (long) maxTorque) / 1000L); + torqueRequested = 30000L - (((long) throttleRequested * (long) torqueMax) / 1000L); } - output.data[0] = (requestedTorque & 0xFF00) >> 8; - output.data[1] = (requestedTorque & 0x00FF); + output.data[0] = (torqueRequested & 0xFF00) >> 8; + output.data[1] = (torqueRequested & 0x00FF); output.data[2] = output.data[0]; output.data[3] = output.data[1]; } else { //RPM mode so request max torque as upper limit and zero torque as lower limit - output.data[0] = ((30000L + maxTorque) & 0xFF00) >> 8; - output.data[1] = ((30000L + maxTorque) & 0x00FF); + output.data[0] = ((30000L + torqueMax) & 0xFF00) >> 8; + output.data[1] = ((30000L + torqueMax) & 0x00FF); output.data[2] = 0x75; output.data[3] = 0x30; } @@ -263,7 +262,7 @@ void DmocMotorController::sendCmd2() { //Logger::debug("max torque: %i", maxTorque); - //Logger::debug("requested torque: %i",(((long) requestedThrottle * (long) maxTorque) / 1000L)); + //Logger::debug("requested torque: %i",(((long) throttleRequested * (long) maxTorque) / 1000L)); CanHandler::getInstanceEV()->sendFrame(output); } @@ -368,14 +367,6 @@ DeviceId DmocMotorController::getId() { return (DMOC645); } -void DmocMotorController::setPowerMode(PowerMode mode) { - powerMode = mode; -} - -DmocMotorController::PowerMode DmocMotorController::getPowerMode() { - return powerMode; -} - uint32_t DmocMotorController::getTickInterval() { return CFG_TICK_INTERVAL_MOTOR_CONTROLLER_DMOC; diff --git a/DmocMotorController.h b/DmocMotorController.h index 897be73..a036106 100644 --- a/DmocMotorController.h +++ b/DmocMotorController.h @@ -50,11 +50,6 @@ class DmocMotorController: public MotorController, CanObserver { CHAL_RESP }; - enum PowerMode { - MODE_TORQUE, - MODE_RPM - }; - enum KeyState { OFF = 0, ON = 1, @@ -78,8 +73,6 @@ class DmocMotorController: public MotorController, CanObserver { DmocMotorController(); DeviceId getId(); - void setPowerMode(PowerMode mode); - PowerMode getPowerMode(); uint32_t getTickInterval(); @@ -91,7 +84,6 @@ class DmocMotorController: public MotorController, CanObserver { int step; byte online; //counter for whether DMOC appears to be operating byte alive; - PowerMode powerMode; void sendCmd1(); void sendCmd2(); diff --git a/MotorController.cpp b/MotorController.cpp index d395b89..c871bd0 100644 --- a/MotorController.cpp +++ b/MotorController.cpp @@ -30,18 +30,33 @@ SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. MotorController::MotorController() : Device() { prefsHandler = new PrefHandler(EE_MOTORCTL_START); - faulted = false; + + ready = false; running = false; - motorTemp = 0; - inverterTemp = 0; - requestedRPM = 0; - requestedThrottle = 0; - requestedTorque = 0; - actualTorque = 0; - actualRPM = 0; - maxTorque = 0; - maxRPM = 0; + faulted = false; + warning = false; + + temperatureMotor = 0; + temperatureInverter = 0; + temperatureSystem = 0; + + powerMode = modeTorque; + throttleRequested = 0; + speedRequested = 0; + speedActual = 0; + speedMax = 0; + torqueRequested = 0; + torqueActual = 0; + torqueMax = 0; + torqueAvailable = 0; + mechanicalPower = 0; + gearSwitch = GS_FAULT; + + dcVoltage = 0; + dcCurrent = 0; + acCurrent = 0; + prechargeC = 0; prechargeR = 0; prechargeTime = 0; @@ -58,33 +73,14 @@ DeviceType MotorController::getType() { void MotorController::handleTick() { uint8_t forwardSwitch, reverseSwitch; - //I'm pretty sure this whole block of code is worthless. - //The inputs that this code uses aren't even actual, valid inputs to the GEVCU box. - /* - if (digitalRead(MOTORCTL_INPUT_DRIVE_EN) == LOW) - running = true; - else - running = false; - - forwardSwitch = digitalRead(MOTORCTL_INPUT_FORWARD); - reverseSwitch = digitalRead(MOTORCTL_INPUT_REVERSE); - - gearSwitch = GS_FAULT; - if (forwardSwitch == LOW && reverseSwitch == HIGH) - gearSwitch = GS_FORWARD; - if (forwardSwitch == HIGH && reverseSwitch == LOW) - gearSwitch = GS_REVERSE; - */ - - running = true; gearSwitch = GS_FORWARD; Throttle *accelerator = DeviceManager::getInstance()->getAccelerator(); Throttle *brake = DeviceManager::getInstance()->getBrake(); if (accelerator) - requestedThrottle = accelerator->getLevel(); + throttleRequested = accelerator->getLevel(); if (brake && brake->getLevel() < -10 && brake->getLevel() < accelerator->getLevel()) //if the brake has been pressed it overrides the accelerator. - requestedThrottle = brake->getLevel(); + throttleRequested = brake->getLevel(); if (prechargeTime == 0) donePrecharge = true; @@ -102,7 +98,7 @@ void MotorController::handleTick() { } } - //Logger::debug("Throttle: %d", requestedThrottle); + //Logger::debug("Throttle: %d", throttleRequested); } @@ -118,8 +114,8 @@ void MotorController::setup() { */ #ifndef USE_HARD_CODED if (prefsHandler->checksumValid()) { //checksum is good, read in the values stored in EEPROM - prefsHandler->read(EEMC_MAX_RPM, &maxRPM); - prefsHandler->read(EEMC_MAX_TORQUE, &maxTorque); + prefsHandler->read(EEMC_MAX_RPM, &speedMax); + prefsHandler->read(EEMC_MAX_TORQUE, &torqueMax); prefsHandler->read(EEMC_PRECHARGE_C, &prechargeC); prefsHandler->read(EEMC_PRECHARGE_R, &prechargeR); prefsHandler->read(EEMC_NOMINAL_V, &nominalVolt); @@ -128,8 +124,8 @@ void MotorController::setup() { prefsHandler->read(EEMC_REVERSE_LIMIT, &reversePercent); } else { //checksum invalid. Reinitialize values and store to EEPROM - maxRPM = MaxRPMValue; - maxTorque = MaxTorqueValue; + speedMax = MaxRPMValue; + torqueMax = MaxTorqueValue; prechargeC = PrechargeC; prechargeR = PrechargeR; nominalVolt = NominalVolt; @@ -140,8 +136,8 @@ void MotorController::setup() { } #else - maxRPM = MaxRPMValue; - maxTorque = MaxTorqueValue; + speedMax = MaxRPMValue; + torqueMax = MaxTorqueValue; prechargeC = PrechargeC; prechargeR = PrechargeR; nominalVolt = NominalVolt; @@ -149,7 +145,7 @@ void MotorController::setup() { mainContactorRelay = MainContactorRelay; #endif - Logger::debug("MaxTorque: %i MaxRPM: %i", maxTorque, maxRPM); + Logger::debug("MaxTorque: %i MaxRPM: %i", torqueMax, speedMax); if (prechargeC> 0 && prechargeRelay < NUM_OUTPUT) { //precharge time is 5RC which is (R*C / 1000) ms * 5 = RC/200 but ohms is in tenths so divide by another 10 = RC/2000 prechargeTime = ((int)prechargeC * prechargeR) / 2000; @@ -162,62 +158,98 @@ void MotorController::setup() { } } -int MotorController::getThrottle() { - return (requestedThrottle); -} - bool MotorController::isRunning() { - return (running); + return running; } bool MotorController::isFaulted() { - return (faulted); + return faulted; +} + +bool MotorController::isWarning() { + return warning; +} + +MotorController::PowerMode MotorController::getPowerMode() { + return powerMode; +} + +void MotorController::setPowerMode(PowerMode mode) { + powerMode = mode; } -uint16_t MotorController::getActualRpm() { - return actualRPM; +int16_t MotorController::getThrottle() { + return throttleRequested; } -uint16_t MotorController::getActualTorque() { - return actualTorque; +int16_t MotorController::getSpeedRequested() { + return speedRequested; +} + +int16_t MotorController::getSpeedActual() { + return speedActual; +} + +uint16_t MotorController::getSpeedMax() { + return speedMax; +} + +void MotorController::setSpeedMax(uint16_t speedMax) +{ + this->speedMax = speedMax; +} + +int16_t MotorController::getTorqueRequested() { + return torqueRequested; +} + +int16_t MotorController::getTorqueActual() { + return torqueActual; +} + +uint16_t MotorController::getTorqueMax() { + return torqueMax; +} + +void MotorController::setTorqueMax(uint16_t maxTorque) +{ + this->torqueMax = maxTorque; } MotorController::GearSwitch MotorController::getGearSwitch() { return gearSwitch; } -signed int MotorController::getInverterTemp() { - return inverterTemp; +int16_t MotorController::getTorqueAvailable() { + return torqueAvailable; } -uint16_t MotorController::getMaxRpm() { - return maxRPM; +uint16_t MotorController::getDcVoltage() { + return dcVoltage; } -uint16_t MotorController::getMaxTorque() { - return maxTorque; +int16_t MotorController::getDcCurrent() { + return dcCurrent; } -signed int MotorController::getMotorTemp() { - return motorTemp; +uint16_t MotorController::getAcCurrent() { + return acCurrent; } -uint16_t MotorController::getRequestedRpm() { - return requestedRPM; +int16_t MotorController::getMechanicalPower() { + return mechanicalPower; } -uint16_t MotorController::getRequestedTorque() { - return requestedTorque; +int16_t MotorController::getTemperatureMotor() { + return temperatureMotor; } -void MotorController::setMaxRpm(uint16_t maxRPM) -{ - this->maxRPM = maxRPM; +int16_t MotorController::getTemperatureInverter() { + return temperatureInverter; } -void MotorController::setMaxTorque(uint16_t maxTorque) -{ - this->maxTorque = maxTorque; +int16_t MotorController::getTemperatureSystem() { + return temperatureSystem; } void MotorController::setPrechargeC(uint16_t c) @@ -252,8 +284,8 @@ void MotorController::setReversePercent(uint8_t perc) void MotorController::saveEEPROM() { - prefsHandler->write(EEMC_MAX_RPM, maxRPM); - prefsHandler->write(EEMC_MAX_TORQUE, maxTorque); + prefsHandler->write(EEMC_MAX_RPM, speedMax); + prefsHandler->write(EEMC_MAX_TORQUE, torqueMax); prefsHandler->write(EEMC_PRECHARGE_C, prechargeC); prefsHandler->write(EEMC_PRECHARGE_R, prechargeR); prefsHandler->write(EEMC_NOMINAL_V, nominalVolt); diff --git a/MotorController.h b/MotorController.h index e515383..1570170 100644 --- a/MotorController.h +++ b/MotorController.h @@ -1,31 +1,31 @@ /* * MotorController.h - * + * * Parent class for all motor controllers. * -Copyright (c) 2013 Collin Kidder, Michael Neuweiler, Charles Galpin - -Permission is hereby granted, free of charge, to any person obtaining -a copy of this software and associated documentation files (the -"Software"), to deal in the Software without restriction, including -without limitation the rights to use, copy, modify, merge, publish, -distribute, sublicense, and/or sell copies of the Software, and to -permit persons to whom the Software is furnished to do so, subject to -the following conditions: - -The above copyright notice and this permission notice shall be included -in all copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, -EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF -MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. -IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY -CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, -TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE -SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - - */ - + Copyright (c) 2013 Collin Kidder, Michael Neuweiler, Charles Galpin + + Permission is hereby granted, free of charge, to any person obtaining + a copy of this software and associated documentation files (the + "Software"), to deal in the Software without restriction, including + without limitation the rights to use, copy, modify, merge, publish, + distribute, sublicense, and/or sell copies of the Software, and to + permit persons to whom the Software is furnished to do so, subject to + the following conditions: + + The above copyright notice and this permission notice shall be included + in all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, + EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF + MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. + IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY + CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, + TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE + SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + + */ + #ifndef MOTORCTRL_H_ #define MOTORCTRL_H_ @@ -41,55 +41,87 @@ SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. #define MOTORCTL_INPUT_REVERSE 5 #define MOTORCTL_INPUT_LIMP 6 -class MotorController : public Device { - - public: - enum GearSwitch { - GS_NEUTRAL, - GS_FORWARD, - GS_REVERSE, - GS_FAULT - }; - MotorController(); +class MotorController: public Device { + +public: + enum GearSwitch { + GS_NEUTRAL, + GS_FORWARD, + GS_REVERSE, + GS_FAULT + }; + + enum PowerMode { + modeTorque, + modeSpeed + }; + + MotorController(); DeviceType getType(); - virtual void setup(); - void handleTick(); - int getThrottle(); - void setThrottle(int newthrottle); + virtual void setup(); + void handleTick(); + + bool isReady(); bool isRunning(); bool isFaulted(); - uint16_t getActualRpm(); - uint16_t getActualTorque(); + bool isWarning(); + + void setPowerMode(PowerMode mode); + PowerMode getPowerMode(); + int16_t getThrottle(); + int16_t getSpeedRequested(); + int16_t getSpeedActual(); + uint16_t getSpeedMax(); + void setSpeedMax(uint16_t); + int16_t getTorqueRequested(); + int16_t getTorqueActual(); + uint16_t getTorqueMax(); + void setTorqueMax(uint16_t); + int16_t getTorqueAvailable(); + + uint16_t getDcVoltage(); + int16_t getDcCurrent(); + uint16_t getAcCurrent(); + int16_t getMechanicalPower(); + int16_t getTemperatureMotor(); + int16_t getTemperatureInverter(); + int16_t getTemperatureSystem(); + GearSwitch getGearSwitch(); - signed int getInverterTemp(); - uint16_t getMaxRpm(); - uint16_t getMaxTorque(); - void setMaxRpm(uint16_t maxRPM); - void setMaxTorque(uint16_t maxTorque); void setPrechargeC(uint16_t c); void setPrechargeR(uint16_t r); void setNominalV(uint16_t v); void setPrechargeRelay(uint8_t relay); void setMainRelay(uint8_t relay); void setReversePercent(uint8_t perc); - signed int getMotorTemp(); - uint16_t getRequestedRpm(); - uint16_t getRequestedTorque(); void saveEEPROM(); - protected: - int requestedThrottle; - bool running; - bool faulted; - signed int motorTemp; //temperature of motor in tenths of degree C - signed int inverterTemp; //temperature of inverter in tenths deg C - uint16_t requestedTorque; //in tenths of Nm - uint16_t requestedRPM; //in RPM - uint16_t actualTorque; //in tenths Nm - uint16_t actualRPM; //in RPM - uint16_t maxTorque; //maximum torque in 0.1 Nm - uint16_t maxRPM; //in RPM - GearSwitch gearSwitch; +protected: + bool ready; // indicates if the controller is ready to enable the power stage + bool running; // indicates if the power stage of the inverter is operative + bool faulted; // indicates a error condition is present in the controller + bool warning; // indicates a warning condition is present in the controller + + PowerMode powerMode; + int16_t throttleRequested; // -1000 to 1000 (per mille of throttle level) + int16_t speedRequested; // in rpm + int16_t speedActual; // in rpm + uint16_t speedMax; // in rpm + int16_t torqueRequested; // in 0.1 Nm + int16_t torqueActual; // in 0.1 Nm + uint16_t torqueMax; // maximum torque in 0.1 Nm + int16_t torqueAvailable; // the maximum available torque in 0.1Nm + + GearSwitch gearSwitch; + + uint16_t dcVoltage; // DC voltage in 0.1 Volts + int16_t dcCurrent; // DC current in 0.1 Amps + uint16_t acCurrent; // AC current in 0.1 Amps + int16_t mechanicalPower; // mechanical power of the motor 0.1 kW + int16_t temperatureMotor; // temperature of motor in 0.1 degree C + int16_t temperatureInverter; // temperature of inverter power stage in 0.1 degree C + int16_t temperatureSystem; // temperature of controller in 0.1 degree C + uint16_t prechargeC; //capacitance of motor controller input in uf uint16_t prechargeR; //resistance of precharge resistor in tenths of ohm uint16_t prechargeTime; //time in ms that precharge should last diff --git a/SerialConsole.cpp b/SerialConsole.cpp index e4ad6d6..f63eff7 100644 --- a/SerialConsole.cpp +++ b/SerialConsole.cpp @@ -1,50 +1,43 @@ /* * SerialConsole.cpp * -Copyright (c) 2013 Collin Kidder, Michael Neuweiler, Charles Galpin + Copyright (c) 2013 Collin Kidder, Michael Neuweiler, Charles Galpin -Permission is hereby granted, free of charge, to any person obtaining -a copy of this software and associated documentation files (the -"Software"), to deal in the Software without restriction, including -without limitation the rights to use, copy, modify, merge, publish, -distribute, sublicense, and/or sell copies of the Software, and to -permit persons to whom the Software is furnished to do so, subject to -the following conditions: + Permission is hereby granted, free of charge, to any person obtaining + a copy of this software and associated documentation files (the + "Software"), to deal in the Software without restriction, including + without limitation the rights to use, copy, modify, merge, publish, + distribute, sublicense, and/or sell copies of the Software, and to + permit persons to whom the Software is furnished to do so, subject to + the following conditions: -The above copyright notice and this permission notice shall be included -in all copies or substantial portions of the Software. + The above copyright notice and this permission notice shall be included + in all copies or substantial portions of the Software. -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, -EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF -MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. -IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY -CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, -TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE -SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, + EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF + MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. + IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY + CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, + TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE + SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ #include "SerialConsole.h" -SerialConsole::SerialConsole(MemCache* memCache) - : memCache(memCache), heartbeat(NULL) -{ - init(); +SerialConsole::SerialConsole(MemCache* memCache) : + memCache(memCache), heartbeat(NULL) { + init(); } -SerialConsole::SerialConsole(MemCache* memCache, Heartbeat* heartbeat) - : memCache(memCache), heartbeat(heartbeat) -{ - init(); +SerialConsole::SerialConsole(MemCache* memCache, Heartbeat* heartbeat) : + memCache(memCache), heartbeat(heartbeat) { + init(); } void SerialConsole::init() { - handlingEvent = false; - - // temp - runRamp = false; - runStatic = false; - runThrottle = false; + handlingEvent = false; //State variables for serial console ptrBuffer = 0; @@ -52,12 +45,12 @@ void SerialConsole::init() { } void SerialConsole::loop() { - - if ( handlingEvent == false ) { - if (SerialUSB.available()) { - serialEvent(); - } - } + + if (handlingEvent == false) { + if (SerialUSB.available()) { + serialEvent(); + } + } } void SerialConsole::printMenu() { @@ -70,19 +63,7 @@ void SerialConsole::printMenu() { SerialUSB.println(); SerialUSB.println("Short Commands:"); SerialUSB.println("h = help (displays this message)"); -//These commented out lines really can't be used any more so there is no point in advertising them right now. -/* - SerialUSB.println("D = disabled op state"); - SerialUSB.println("S = standby op state"); - SerialUSB.println("E = enabled op state"); - SerialUSB.println("n = neutral gear"); - SerialUSB.println("d = DRIVE gear"); - SerialUSB.println("r = reverse gear"); - SerialUSB.println(" = start/stop ramp test"); - SerialUSB.println("x = lock ramp at current value (toggle)"); - SerialUSB.println("t = Use accelerator pedal? (toggle)"); -*/ - if ( heartbeat != NULL ) { + if (heartbeat != NULL) { SerialUSB.println("L = output raw input values (toggle)"); } SerialUSB.println("K = set all outputs high"); @@ -123,204 +104,189 @@ void SerialConsole::printMenu() { /* There is a help menu (press H or h or ?) - This is no longer going to be a simple single character console. - Now the system can handle up to 80 input characters. Commands are submitted - by sending line ending (LF, CR, or both) -*/ -void SerialConsole::serialEvent() -{ + This is no longer going to be a simple single character console. + Now the system can handle up to 80 input characters. Commands are submitted + by sending line ending (LF, CR, or both) + */ +void SerialConsole::serialEvent() { int incoming; incoming = SerialUSB.read(); if (incoming == -1) { //false alarm.... - return; + return; } if (incoming == 10 || incoming == 13) { //command done. Parse it. handleConsoleCmd(); ptrBuffer = 0; //reset line counter once the line has been processed - } - else { + } else { cmdBuffer[ptrBuffer++] = (unsigned char) incoming; - if (ptrBuffer > 79) ptrBuffer = 79; + if (ptrBuffer > 79) + ptrBuffer = 79; } } -void SerialConsole::handleConsoleCmd() -{ +void SerialConsole::handleConsoleCmd() { handlingEvent = true; - + if (state == STATE_ROOT_MENU) { if (ptrBuffer == 1) { //command is a single ascii character handleShortCmd(); - } - else { //if cmd over 1 char then assume (for now) that it is a config line + } else { //if cmd over 1 char then assume (for now) that it is a config line handleConfigCmd(); } } - handlingEvent = false; + handlingEvent = false; } /*For simplicity the configuration setting code uses four characters for each configuration choice. This makes things easier for -comparison purposes. -*/ -void SerialConsole::handleConfigCmd() -{ + comparison purposes. + */ +void SerialConsole::handleConfigCmd() { int i; int newValue; //Logger::debug("Cmd size: %i", ptrBuffer); - if (ptrBuffer < 6) return; //4 digit command, =, value is at least 6 characters + if (ptrBuffer < 6) + return; //4 digit command, =, value is at least 6 characters cmdBuffer[ptrBuffer] = 0; //make sure to null terminate String cmdString = String(); i = 0; - while (cmdBuffer[i] != '=' && i < ptrBuffer) cmdString.concat(String(cmdBuffer[i++])); + while (cmdBuffer[i] != '=' && i < ptrBuffer) + cmdString.concat(String(cmdBuffer[i++])); i++; //skip the = - if (i >= ptrBuffer) return; //or, we could use this to display the parameter instead of setting + if (i >= ptrBuffer) + return; //or, we could use this to display the parameter instead of setting cmdString.toUpperCase(); if (cmdString == String("TORQ")) { - newValue = atoi((char *)(cmdBuffer + i)); + newValue = atoi((char *) (cmdBuffer + i)); Logger::debug("Setting Torque Limit to %i", newValue); - DeviceManager::getInstance()->getMotorController()->setMaxTorque(newValue); + DeviceManager::getInstance()->getMotorController()->setTorqueMax(newValue); DeviceManager::getInstance()->getMotorController()->saveEEPROM(); - } - else if (cmdString == String("RPMS")) { - newValue = atoi((char *)(cmdBuffer + i)); + } else if (cmdString == String("RPMS")) { + newValue = atoi((char *) (cmdBuffer + i)); Logger::debug("Setting RPM Limit to %i", newValue); - DeviceManager::getInstance()->getMotorController()->setMaxRpm(newValue); + DeviceManager::getInstance()->getMotorController()->setSpeedMax(newValue); DeviceManager::getInstance()->getMotorController()->saveEEPROM(); } else if (cmdString == String("REVLIM")) { - newValue = atoi((char *)(cmdBuffer + i)); + newValue = atoi((char *) (cmdBuffer + i)); Logger::debug("Setting Reverse Limit to %i", newValue); DeviceManager::getInstance()->getMotorController()->setReversePercent(newValue); DeviceManager::getInstance()->getMotorController()->saveEEPROM(); } else if (cmdString == String("TPOT")) { - newValue = atoi((char *)(cmdBuffer + i)); + newValue = atoi((char *) (cmdBuffer + i)); Logger::debug("Setting # of Throttle Pots to %i", newValue); DeviceManager::getInstance()->getAccelerator()->setNumThrottlePots(newValue); DeviceManager::getInstance()->getAccelerator()->saveEEPROM(); } else if (cmdString == String("TTYPE")) { - newValue = atoi((char *)(cmdBuffer + i)); + newValue = atoi((char *) (cmdBuffer + i)); Logger::debug("Setting Throttle Subtype to %i", newValue); DeviceManager::getInstance()->getAccelerator()->setSubtype(newValue); DeviceManager::getInstance()->getAccelerator()->saveEEPROM(); } else if (cmdString == String("T1MN")) { - newValue = atoi((char *)(cmdBuffer + i)); + newValue = atoi((char *) (cmdBuffer + i)); Logger::debug("Setting Throttle1 Min to %i", newValue); DeviceManager::getInstance()->getAccelerator()->setT1Min(newValue); DeviceManager::getInstance()->getAccelerator()->saveEEPROM(); - } - else if (cmdString == String("T1MX")) { - newValue = atoi((char *)(cmdBuffer + i)); + } else if (cmdString == String("T1MX")) { + newValue = atoi((char *) (cmdBuffer + i)); Logger::debug("Setting Throttle1 Max to %i", newValue); DeviceManager::getInstance()->getAccelerator()->setT1Max(newValue); DeviceManager::getInstance()->getAccelerator()->saveEEPROM(); - } - else if (cmdString == String("T2MN")) { - newValue = atoi((char *)(cmdBuffer + i)); + } else if (cmdString == String("T2MN")) { + newValue = atoi((char *) (cmdBuffer + i)); Logger::debug("Setting Throttle2 Min to %i", newValue); DeviceManager::getInstance()->getAccelerator()->setT2Min(newValue); DeviceManager::getInstance()->getAccelerator()->saveEEPROM(); - } - else if (cmdString == String("T2MX")) { - newValue = atoi((char *)(cmdBuffer + i)); + } else if (cmdString == String("T2MX")) { + newValue = atoi((char *) (cmdBuffer + i)); Logger::debug("Setting Throttle2 Max to %i", newValue); DeviceManager::getInstance()->getAccelerator()->setT2Max(newValue); DeviceManager::getInstance()->getAccelerator()->saveEEPROM(); - } - else if (cmdString == String("TRGN")) { - newValue = atoi((char *)(cmdBuffer + i)); + } else if (cmdString == String("TRGN")) { + newValue = atoi((char *) (cmdBuffer + i)); Logger::debug("Setting Throttle Regen End to %i", newValue); DeviceManager::getInstance()->getAccelerator()->setRegenEnd(newValue); DeviceManager::getInstance()->getAccelerator()->saveEEPROM(); - } - else if (cmdString == String("TFWD")) { - newValue = atoi((char *)(cmdBuffer + i)); + } else if (cmdString == String("TFWD")) { + newValue = atoi((char *) (cmdBuffer + i)); Logger::debug("Setting Throttle Forward Start to %i", newValue); DeviceManager::getInstance()->getAccelerator()->setFWDStart(newValue); DeviceManager::getInstance()->getAccelerator()->saveEEPROM(); - } - else if (cmdString == String("TMAP")) { - newValue = atoi((char *)(cmdBuffer + i)); + } else if (cmdString == String("TMAP")) { + newValue = atoi((char *) (cmdBuffer + i)); Logger::debug("Setting Throttle MAP Point to %i", newValue); DeviceManager::getInstance()->getAccelerator()->setMAP(newValue); DeviceManager::getInstance()->getAccelerator()->saveEEPROM(); - } - else if (cmdString == String("TMRN")) { - newValue = atoi((char *)(cmdBuffer + i)); + } else if (cmdString == String("TMRN")) { + newValue = atoi((char *) (cmdBuffer + i)); Logger::debug("Setting Throttle Regen Strength to %i", newValue); DeviceManager::getInstance()->getAccelerator()->setMaxRegen(newValue); DeviceManager::getInstance()->getAccelerator()->saveEEPROM(); } else if (cmdString == String("BMAXR")) { - newValue = atoi((char *)(cmdBuffer + i)); + newValue = atoi((char *) (cmdBuffer + i)); Logger::debug("Setting Max Brake Regen to %i", newValue); DeviceManager::getInstance()->getBrake()->setMaxRegen(newValue); DeviceManager::getInstance()->getBrake()->saveEEPROM(); } else if (cmdString == String("BMINR")) { - newValue = atoi((char *)(cmdBuffer + i)); + newValue = atoi((char *) (cmdBuffer + i)); Logger::debug("Setting Min Brake Regen to %i", newValue); DeviceManager::getInstance()->getBrake()->setMinRegen(newValue); DeviceManager::getInstance()->getBrake()->saveEEPROM(); } else if (cmdString == String("B1MX")) { - newValue = atoi((char *)(cmdBuffer + i)); + newValue = atoi((char *) (cmdBuffer + i)); Logger::debug("Setting Brake Max to %i", newValue); DeviceManager::getInstance()->getBrake()->setT1Max(newValue); DeviceManager::getInstance()->getBrake()->saveEEPROM(); - } - else if (cmdString == String("B1MN")) { - newValue = atoi((char *)(cmdBuffer + i)); + } else if (cmdString == String("B1MN")) { + newValue = atoi((char *) (cmdBuffer + i)); Logger::debug("Setting Brake Min to %i", newValue); DeviceManager::getInstance()->getBrake()->setT1Min(newValue); DeviceManager::getInstance()->getBrake()->saveEEPROM(); } else if (cmdString == String("PREC")) { - newValue = atoi((char *)(cmdBuffer + i)); + newValue = atoi((char *) (cmdBuffer + i)); Logger::debug("Setting Precharge Capacitance to %i", newValue); DeviceManager::getInstance()->getMotorController()->setPrechargeC(newValue); DeviceManager::getInstance()->getMotorController()->saveEEPROM(); - } - else if (cmdString == String("PRER")) { - newValue = atoi((char *)(cmdBuffer + i)); + } else if (cmdString == String("PRER")) { + newValue = atoi((char *) (cmdBuffer + i)); Logger::debug("Setting Precharge Resistance to %i", newValue); DeviceManager::getInstance()->getMotorController()->setPrechargeR(newValue); DeviceManager::getInstance()->getMotorController()->saveEEPROM(); - } - else if (cmdString == String("NOMV")) { - newValue = atoi((char *)(cmdBuffer + i)); + } else if (cmdString == String("NOMV")) { + newValue = atoi((char *) (cmdBuffer + i)); Logger::debug("Setting Nominal Voltage to %i", newValue); DeviceManager::getInstance()->getMotorController()->setNominalV(newValue); DeviceManager::getInstance()->getMotorController()->saveEEPROM(); - } - else if (cmdString == String("MRELAY")) { - newValue = atoi((char *)(cmdBuffer + i)); + } else if (cmdString == String("MRELAY")) { + newValue = atoi((char *) (cmdBuffer + i)); Logger::debug("Setting Main Contactor relay to %i", newValue); DeviceManager::getInstance()->getMotorController()->setMainRelay(newValue); DeviceManager::getInstance()->getMotorController()->saveEEPROM(); - } - else if (cmdString == String("PRELAY")) { - newValue = atoi((char *)(cmdBuffer + i)); + } else if (cmdString == String("PRELAY")) { + newValue = atoi((char *) (cmdBuffer + i)); Logger::debug("Setting Precharge Relay to %i", newValue); DeviceManager::getInstance()->getMotorController()->setPrechargeRelay(newValue); DeviceManager::getInstance()->getMotorController()->saveEEPROM(); - } - else if (cmdString == String("LOGLEVEL")) { - newValue = atoi((char *)(cmdBuffer + i)); + } else if (cmdString == String("LOGLEVEL")) { + newValue = atoi((char *) (cmdBuffer + i)); switch (newValue) { case 0: Logger::setLoglevel(Logger::Debug); @@ -344,148 +310,90 @@ void SerialConsole::handleConfigCmd() } } -void SerialConsole::handleShortCmd() -{ +void SerialConsole::handleShortCmd() { uint8_t val; - DmocMotorController* dmoc = (DmocMotorController*) DeviceManager::getInstance()->getMotorController(); //TODO: direct reference to dmoc must be removed + MotorController* motorController = (MotorController*) DeviceManager::getInstance()->getMotorController(); switch (cmdBuffer[0]) { - case 'h': - case '?': - case 'H': - printMenu(); - break; - case ' ': - runRamp = !runRamp; - if (runRamp) { - Logger::info("Start Ramp Test"); - dmoc->setPowerMode(DmocMotorController::MODE_RPM); //TODO: direct reference to dmoc must be removed - } - else { - Logger::info("End Ramp Test"); - dmoc->setPowerMode(DmocMotorController::MODE_TORQUE); //TODO: direct reference to dmoc must be removed - } - break; - case 'd': - dmoc->setGear(DmocMotorController::DRIVE); //TODO: direct reference to dmoc must be removed - Logger::info("forward"); - break; - case 'n': - dmoc->setGear(DmocMotorController::NEUTRAL); //TODO: direct reference to dmoc must be removed - Logger::info("neutral"); - break; - case 'r': - dmoc->setGear(DmocMotorController::REVERSE); //TODO: direct reference to dmoc must be removed - Logger::info("reverse"); - break; - case 'D': - dmoc->setOpState(DmocMotorController::DISABLED); //TODO: direct reference to dmoc must be removed - Logger::info("disabled"); - break; - case 'S': - dmoc->setOpState(DmocMotorController::STANDBY); //TODO: direct reference to dmoc must be removed - Logger::info("standby"); - break; - case 'E': - dmoc->setOpState(DmocMotorController::ENABLE); //TODO: direct reference to dmoc must be removed - Logger::info("enabled"); - break; - case 'x': - runStatic = !runStatic; - if (runStatic) { - Logger::info("Lock RPM rate"); - } - else - Logger::info("Unlock RPM rate"); - break; - case 't': - runThrottle = !runThrottle; - if (runThrottle) { - Logger::info("Use Throttle Pedal"); - dmoc->setPowerMode(DmocMotorController::MODE_TORQUE); //TODO: direct reference to dmoc must be removed - } - else { - Logger::info("Ignore throttle pedal"); - dmoc->setPowerMode(DmocMotorController::MODE_RPM); //TODO: direct reference to dmoc must be removed - } - break; - case 'L': - if ( heartbeat != NULL ) { - heartbeat->setThrottleDebug(!heartbeat->getThrottleDebug()); - if (heartbeat->getThrottleDebug()) { - Logger::info("Output raw throttle"); - } - else { - Logger::info("Cease raw throttle output"); - } - } - break; - case 'U': - Logger::info("Adding a sequence of values from 0 to 255 into eeprom"); - for (int i = 0; i < 256; i++) { - memCache->Write(1000 + i, (uint8_t) i); - } - Logger::info("Flushing cache"); - memCache->FlushAllPages(); //write everything to eeprom - memCache->InvalidateAll(); //remove all data from cache - Logger::info("Operation complete."); - break; - case 'I': - Logger::info("Retrieving data previously saved"); - for (int i = 0; i < 256; i++) { - memCache->Read(1000 + i, &val); - Logger::info("%d: %d", i, val); + case 'h': + case '?': + case 'H': + printMenu(); + break; + case 'L': + if (heartbeat != NULL) { + heartbeat->setThrottleDebug(!heartbeat->getThrottleDebug()); + if (heartbeat->getThrottleDebug()) { + Logger::info("Output raw throttle"); + } else { + Logger::info("Cease raw throttle output"); } - break; - case 'A': - Logger::info("Retrieving System EEPROM values"); - for (int i = 0; i < 256; i++) { - memCache->Read(EE_SYSTEM_START + i, &val); - Logger::info("%d: %d", i, val); + } + break; + case 'U': + Logger::info("Adding a sequence of values from 0 to 255 into eeprom"); + for (int i = 0; i < 256; i++) { + memCache->Write(1000 + i, (uint8_t) i); + } + Logger::info("Flushing cache"); + memCache->FlushAllPages(); //write everything to eeprom + memCache->InvalidateAll(); //remove all data from cache + Logger::info("Operation complete."); + break; + case 'I': + Logger::info("Retrieving data previously saved"); + for (int i = 0; i < 256; i++) { + memCache->Read(1000 + i, &val); + Logger::info("%d: %d", i, val); + } + break; + case 'A': + Logger::info("Retrieving System EEPROM values"); + for (int i = 0; i < 256; i++) { + memCache->Read(EE_SYSTEM_START + i, &val); + Logger::info("%d: %d", i, val); + } + break; + case 'K': //set all outputs high + setOutput(0, true); + setOutput(1, true); + setOutput(2, true); + setOutput(3, true); + Logger::info("all outputs: ON"); + break; + case 'J': //set the four outputs low + setOutput(0, false); + setOutput(1, false); + setOutput(2, false); + setOutput(3, false); + Logger::info("all outputs: OFF"); + break; + case 'z': // detect throttle min/max & other details + DeviceManager::getInstance()->getAccelerator()->detectThrottle(); + break; + case 'Z': // save throttle settings + DeviceManager::getInstance()->getAccelerator()->saveConfiguration(); + break; + case 'b': + DeviceManager::getInstance()->getBrake()->detectThrottle(); + break; + case 'B': + DeviceManager::getInstance()->getBrake()->saveConfiguration(); + break; + case 'p': + Logger::info("PASSTHROUGH MODE - All traffic Serial3 <-> SerialUSB"); + //this never stops so basically everything dies. you will have to reboot. + int inSerialUSB, inSerial3; + while (1 == 1) { + inSerialUSB = SerialUSB.read(); + inSerial3 = Serial3.read(); + if (inSerialUSB > -1) { + Serial3.write((char) inSerialUSB); } - break; - case 'K': //set all outputs high - setOutput(0, true); - setOutput(1, true); - setOutput(2, true); - setOutput(3, true); - Logger::info("all outputs: ON"); - break; - case 'J': //set the four outputs low - setOutput(0, false); - setOutput(1, false); - setOutput(2, false); - setOutput(3, false); - Logger::info("all outputs: OFF"); - break; - case 'z': // detect throttle min/max & other details - DeviceManager::getInstance()->getAccelerator()->detectThrottle(); - break; - case 'Z': // save throttle settings - DeviceManager::getInstance()->getAccelerator()->saveConfiguration(); - break; - case 'b': - DeviceManager::getInstance()->getBrake()->detectThrottle(); - break; - case 'B': - DeviceManager::getInstance()->getBrake()->saveConfiguration(); - break; - case 'p': - Logger::info("PASSTHROUGH MODE - All traffic Serial3 <-> SerialUSB"); - //this never stops so basically everything dies. you will have to reboot. - int inSerialUSB, inSerial3; - while (1==1) { - inSerialUSB = SerialUSB.read(); - inSerial3 = Serial3.read(); - if (inSerialUSB > -1) - { - Serial3.write((char)inSerialUSB); - } - if (inSerial3 > -1) - { - SerialUSB.write((char)inSerial3); - } + if (inSerial3 > -1) { + SerialUSB.write((char) inSerial3); } - break; + } + break; } } diff --git a/SerialConsole.h b/SerialConsole.h index 0b87f0e..ea8a4a1 100644 --- a/SerialConsole.h +++ b/SerialConsole.h @@ -58,11 +58,6 @@ class SerialConsole { int ptrBuffer; int state; - // temp - bool runRamp; - bool runStatic; - bool runThrottle; - void init(); void serialEvent(); void handleConsoleCmd(); diff --git a/Throttle.cpp b/Throttle.cpp index 965b0ab..521590d 100644 --- a/Throttle.cpp +++ b/Throttle.cpp @@ -57,7 +57,7 @@ DeviceType Throttle::getType() return DEVICE_THROTTLE; } -int Throttle::getLevel() +int16_t Throttle::getLevel() { return level; } diff --git a/Throttle.h b/Throttle.h index c65b149..10d2f36 100644 --- a/Throttle.h +++ b/Throttle.h @@ -48,7 +48,7 @@ class Throttle: public Device { virtual DeviceType getType(); virtual void handleTick(); - virtual int getLevel(); + virtual int16_t getLevel(); virtual int getRawThrottle1(); virtual int getRawThrottle2(); @@ -76,7 +76,7 @@ class Throttle: public Device { virtual void saveEEPROM(); protected: - signed int level; //the final signed throttle level. [-1000, 1000] in tenths of a percent of maximum + int16_t level; //the final signed throttle level. [-1000, 1000] in tenths of a percent of maximum uint16_t throttleRegen, throttleFwd, throttleMap; //Value at which regen finishes, forward motion starts, and the mid point of throttle uint16_t throttleMaxRegen; //Percentage of max torque allowable for regen uint16_t brakeMaxRegen; //percentage of max torque allowable for regen at brake pedal diff --git a/ichip_2128.cpp b/ichip_2128.cpp index 5056275..857e02b 100644 --- a/ichip_2128.cpp +++ b/ichip_2128.cpp @@ -67,16 +67,23 @@ void ICHIPWIFI::handleTick() { if (motorController) { setParam("statusMillis", millis()); setParam("statusThrottle", motorController->getThrottle()); - setParam("statusTorqueReq", motorController->getRequestedTorque() / 10.0f, 1); - setParam("statusTorqueActual", motorController->getActualTorque() / 10.0f, 1); - setParam("statusSpeedRequested", motorController->getRequestedRpm()); - setParam("statusSpeedActual", motorController->getActualRpm()); + setParam("statusTorqueReq", motorController->getTorqueRequested() / 10.0f, 1); + setParam("statusTorqueActual", motorController->getTorqueActual() / 10.0f, 1); + setParam("statusSpeedRequested", motorController->getSpeedRequested()); + setParam("statusSpeedActual", motorController->getSpeedActual()); + setParam("statusDcVoltage", motorController->getDcVoltage()); + setParam("statusDcCurrent", motorController->getDcCurrent()); + setParam("statusAcCurrent", motorController->getAcCurrent()); + if (motorController->getId() == BRUSA_DMC5) + setParam("statusMechPower", motorController->getMechanicalPower()); if (tickCounter++ > 3) { setParam("statusRunning", (motorController->isRunning() ? "true" : "false")); setParam("statusFaulted", (motorController->isFaulted() ? "true" : "false")); - setParam("statusTempMotor", motorController->getMotorTemp() / 10.0f, 1); - setParam("statusTempInverter", motorController->getInverterTemp() / 10.0f, 1); + setParam("statusWarning", (motorController->isWarning() ? "true" : "false")); + setParam("statusTempMotor", motorController->getTemperatureMotor() / 10.0f, 1); + setParam("statusTempInverter", motorController->getTemperatureInverter() / 10.0f, 1); + setParam("statusTempSystem", motorController->getTemperatureInverter() / 10.0f, 1); setParam("statusGear", motorController->getGearSwitch()); tickCounter = 0; } diff --git a/website/src/status.htm b/website/src/status.htm index c70fb8b..dd58313 100644 --- a/website/src/status.htm +++ b/website/src/status.htm @@ -16,6 +16,13 @@ Faulted : + + Warning + : + + Temperature System + : + Motor Temp : °C @@ -44,4 +51,18 @@ Actual Speed : rpm + + DC Voltage + : Volts + + DC Current + : Amps + + + AC Current + : Amps + + Mechanical Power + : kW + diff --git a/website/src/status.xml b/website/src/status.xml index 8475894..5aaeb30 100644 --- a/website/src/status.xml +++ b/website/src/status.xml @@ -3,12 +3,18 @@ ~statusMillis~ ~statusRunning~ ~statusFaulted~ + ~statusWarning~ ~statusTempMotor~ ~statusTempInverter~ + ~statusTempSystem~ ~statusThrottle~ ~statusGear~ ~statusTorqueReq~ ~statusTorqueActual~ ~statusSpeedRequested~ ~statusSpeedActual~ + ~statusDcVoltage~ + ~statusDcCurrent~ + ~statusAcCurrent~ + ~statusMechPower~ \ No newline at end of file diff --git a/website/website.img b/website/website.img index b7ee73f9c7e5bbfa18b2be5c98c0cbc67e9845ca..df5c4fec72777c211815732f81639779ab67c2f6 100644 GIT binary patch delta 831 zcmeyG+h1Dm>>nP&00oQ;%nXd2g$(N%IT#jvUC+qC;2m|Bg@M8FTpu3;L)6k8atsU$ z;(r239f3@B28NK#OS%jUSG7zH85jf^RxtoIpnwGoTnuZ0csCHA1mf#J{2YjX0Wmit z7lRZdmmGsCkYNhM9zYxm#Q8v655)aIJbz-V5qo%IQC??B<8D=^RJP4u z)Or}1ftE~ut1io24y1WC)P+KTR;3mJEhtJ=fLJ*>P)kYxAqo{dsHwWSLE|{nWCwL# zE*ED7sADF7RF{-UfjLdd&PKnuATiGlEL}YLp_UX#E!1Hkwc;r-XCl-(<`xuBHdL44 zazs^{h(jsRQ~{9h67w>X6LS>6o&=d~mJ1gIiy>^t4!6-SNwMSQ;snw~KpIFTCgr38 zS@F!1gSFQv0D}V*_I7oUz=cLdosB+JV)8HTMl>nP&00oQ;%nWiIg$x#q91IJ-S}-y&ct@qNFfbUN6X#=Kh+66>$H1^4 zz6nU`2z*jwU&d!G>