diff --git a/.gitignore b/.gitignore index b37af3b..ccad7a1 100644 --- a/.gitignore +++ b/.gitignore @@ -945,6 +945,7 @@ _deps ##### C++ # Prerequisites *.d +*.clangd # Compiled Object files *.slo diff --git a/GEVCU7.ino b/GEVCU7.ino index 9d12a15..89151dd 100644 --- a/GEVCU7.ino +++ b/GEVCU7.ino @@ -239,6 +239,8 @@ void testGEVCUHardware() Serial.print(val); Serial.print(" "); } + + Serial.print("ADC19: " + systemIO.getAnalogIn(19)); Serial.println(); Serial.print("DIN: "); diff --git a/bms.cpp b/bms.cpp new file mode 100644 index 0000000..ab0c014 --- /dev/null +++ b/bms.cpp @@ -0,0 +1 @@ +// \ No newline at end of file diff --git a/src/Heartbeat.cpp b/src/Heartbeat.cpp index 5eb5aaa..27ddde6 100644 --- a/src/Heartbeat.cpp +++ b/src/Heartbeat.cpp @@ -76,16 +76,17 @@ void Heartbeat::handleTick() { Logger::console("Motor Controller Status-> isRunning: %u isFaulted: %u", motorController->isRunning(), motorController->isFaulted()); } - Logger::console("AIN0: %i, AIN1: %i, AIN2: %i, AIN3: %i, AIN4: %i, AIN5: %i, AIN6: %i, AIN7: %i", - systemIO.getAnalogIn(0), systemIO.getAnalogIn(1), systemIO.getAnalogIn(2), systemIO.getAnalogIn(3), - systemIO.getAnalogIn(4), systemIO.getAnalogIn(5), systemIO.getAnalogIn(6), systemIO.getAnalogIn(7)); - Logger::console("DIN0: %d, DIN1: %d, DIN2: %d, DIN3: %d, DIN4: %d, DIN5: %d, DIN6: %d, DIN7: %d, DIN8: %d, DIN9: %d, DIN10: %d, DIN11: %d", + // , AIN1: %i, AIN2: %i", + Logger::console("AIN0: %g, AIN1: %g", ((double)(analogRead(0))*5)/4096, ((double)(analogRead(1))*5)/4096); + // Logger::console("AIN0: %g, AIN1: %g, AIN2: %g, AIN3: %g, AIN4: %g, AIN5: %g, AIN6: %g, AIN7: %g", ((double)(analogRead(0))*5)/4096, ((double)(analogRead(1))*5)/4096, ((double)(analogRead(2))*5)/4096, ((double)(analogRead(3))*5)/4096, + // ((double)(analogRead(4))*5)/4096, ((double)(analogRead(5))*5)/4096, ((double)(analogRead(6))*5)/4096, ((double)(analogRead(7))*5)/4096); + /* Logger::console("DIN0: %d, DIN1: %d, DIN2: %d, DIN3: %d, DIN4: %d, DIN5: %d, DIN6: %d, DIN7: %d, DIN8: %d, DIN9: %d, DIN10: %d, DIN11: %d", systemIO.getDigitalIn(0), systemIO.getDigitalIn(1), systemIO.getDigitalIn(2), systemIO.getDigitalIn(3), systemIO.getDigitalIn(4), systemIO.getDigitalIn(5), systemIO.getDigitalIn(6), systemIO.getDigitalIn(7), systemIO.getDigitalIn(8), systemIO.getDigitalIn(9), systemIO.getDigitalIn(10), systemIO.getDigitalIn(11)); Logger::console("DOUT0: %d, DOUT1: %d, DOUT2: %d, DOUT3: %d,DOUT4: %d, DOUT5: %d, DOUT6: %d, DOUT7: %d", systemIO.getDigitalOutput(0), systemIO.getDigitalOutput(1), systemIO.getDigitalOutput(2), systemIO.getDigitalOutput(3), - systemIO.getDigitalOutput(4), systemIO.getDigitalOutput(5), systemIO.getDigitalOutput(6), systemIO.getDigitalOutput(7)); + systemIO.getDigitalOutput(4), systemIO.getDigitalOutput(5), systemIO.getDigitalOutput(6), systemIO.getDigitalOutput(7));*/ //Logger::console("SD-Detect: %u", digitalRead(5)); BatteryManager *bms = reinterpret_cast(deviceManager.getDeviceByType(DEVICE_BMS)); if (!bms) { diff --git a/src/devices/io/PotThrottle.cpp b/src/devices/io/PotThrottle.cpp index 599cad0..abe6875 100644 --- a/src/devices/io/PotThrottle.cpp +++ b/src/devices/io/PotThrottle.cpp @@ -104,8 +104,9 @@ RawSignalData *PotThrottle::acquireRawSignal() { bool PotThrottle::validateSignal(RawSignalData *rawSignal) { PotThrottleConfiguration *config = (PotThrottleConfiguration *) getConfiguration(); int32_t calcThrottle1, calcThrottle2; - - calcThrottle1 = normalizeInput(rawSignal->input1, config->minimumLevel1, config->maximumLevel1); + + calcThrottle1 = normalizeInput(rawSignal->input1, config->minimumLevel1, config->maximumLevel1 ); + //Logger::console("Value is %d", calcThrottle1); if (config->numberPotMeters == 1 && config->throttleSubType == 2) { // inverted calcThrottle1 = 1000 - calcThrottle1; } @@ -226,6 +227,13 @@ int16_t PotThrottle::calculatePedalPosition(RawSignalData *rawSignal) { calcThrottle2 = 1000 - calcThrottle2; calcThrottle1 = (calcThrottle1 + calcThrottle2) / 2; // now the average of the two } + + // int a = (calcThrottle1/10); + // uint32_t firsthalf = (a & 0xFF); + // uint32_t secondhalf = ((a >> 8) & 0xFF); + + + return calcThrottle1; } @@ -254,14 +262,24 @@ void PotThrottle::loadConfiguration() { //if (prefsHandler->checksumValid()) { //checksum is good, read in the values stored in EEPROM Logger::debug(POTACCELPEDAL, Constants::validChecksum); - prefsHandler->read("ThrottleMin1", (uint16_t *)&config->minimumLevel1, 20); - prefsHandler->read("ThrottleMax1", (uint16_t *)&config->maximumLevel1, 3150); - prefsHandler->read("ThrottleMin2", (uint16_t *)&config->minimumLevel2, 0); - prefsHandler->read("ThrottleMax2", (uint16_t *)&config->maximumLevel2, 0); - prefsHandler->read("NumThrottles", &config->numberPotMeters, 1); + prefsHandler->read("ThrottleMin1", (uint16_t *)&config->minimumLevel1, 0); + //figure out this number --> 3150 + //3150 milivolts --> + + //1 volt around 818 + + //5 volts = 818 when max is 5000 + //5 volts = 1000 when max is 4090 + //910 dif = 18.2% dif + + prefsHandler->read("ThrottleMax1", (uint16_t *)&config->maximumLevel1, 3353); + prefsHandler->read("ThrottleMin2", (uint16_t *)&config->minimumLevel2, 298); + prefsHandler->read("ThrottleMax2", (uint16_t *)&config->maximumLevel2, 1680); + prefsHandler->read("ThrottleMin1", (uint16_t *)&config->minimumLevel1, 598); + prefsHandler->read("NumThrottles", &config->numberPotMeters, 2); prefsHandler->read("ThrottleType", &config->throttleSubType, 1); prefsHandler->read("ADC1", &config->AdcPin1, 0); - prefsHandler->read("ADC2", &config->AdcPin2, 1); + prefsHandler->read("ADC2", &config->AdcPin2, 4); // ** This is potentially a condition that is only met if you don't have the EEPROM hardware ** // If preferences have never been set before, numThrottlePots and throttleSubType @@ -304,7 +322,10 @@ String PotThrottle::describeThrottleType() if (config->throttleSubType == 2) return String("Inverse Linear"); return String("Invalid Value!"); } - +int16_t PotThrottle::getLevel() +{ + return calculatePedalPosition(acquireRawSignal()); +} //creation of a global variable here causes the driver to automatically register itself without external help PotThrottle potThrottle; diff --git a/src/devices/io/PotThrottle.h b/src/devices/io/PotThrottle.h index 051c1ef..7ae65ce 100644 --- a/src/devices/io/PotThrottle.h +++ b/src/devices/io/PotThrottle.h @@ -69,11 +69,13 @@ class PotThrottle: public Throttle { void loadConfiguration(); void saveConfiguration(); + + int16_t getLevel(); protected: bool validateSignal(RawSignalData *); - int16_t calculatePedalPosition(RawSignalData *); String describeThrottleType(); + int16_t calculatePedalPosition(RawSignalData *); private: RawSignalData rawSignal; diff --git a/src/devices/io/Throttle.cpp b/src/devices/io/Throttle.cpp index 3d142c5..42a5261 100644 --- a/src/devices/io/Throttle.cpp +++ b/src/devices/io/Throttle.cpp @@ -155,7 +155,7 @@ int16_t Throttle::mapPedalPosition(int16_t pedalPosition) { status = ERR_MISC; } - if (throttleLevel > 1050) { + if (throttleLevel > 1300) { Logger::error("Generated throttle level (%i) was way too high!", throttleLevel); throttleLevel = 0; status = ERR_MISC; diff --git a/src/devices/io/Throttle.h b/src/devices/io/Throttle.h index c6c7c38..a37d903 100644 --- a/src/devices/io/Throttle.h +++ b/src/devices/io/Throttle.h @@ -57,7 +57,7 @@ //these two should be configuration options instead. #define CFG_CANTHROTTLE_MAX_NUM_LOST_MSG 3 // maximum number of lost messages allowed #define CFG_THROTTLE_TOLERANCE 150 //the max that things can go over or under the min/max without fault - 1/10% each # -#define ThrottleMaxErrValue 150 //tenths of percentage allowable deviation between pedals +#define ThrottleMaxErrValue 100 //tenths of percentage allowable deviation between pedals /* * Data structure to hold raw signal(s) of the throttle. diff --git a/src/devices/misc/CANRecieveTest.cpp b/src/devices/misc/CANRecieveTest.cpp new file mode 100644 index 0000000..acc2f59 --- /dev/null +++ b/src/devices/misc/CANRecieveTest.cpp @@ -0,0 +1,43 @@ +#include "CANRecieveTest.h" + +CANRecieveTest::CANRecieveTest() : Device() { + commonName = "CAN Recieve Test"; + shortName = "CAN"; +} + +void CANRecieveTest::earlyInit() +{ + prefsHandler = new PrefHandler(CANRecieveTestID); +} + +void CANRecieveTest::setup() { + + Logger::info("add device: CANRecieveTest (id: %X, %X)", CANRecieveTestID, this); + + + Device::setup(); // run the parent class version of this function + + setAttachedCANBus(0); + + //Relevant BMS messages are 0x300 - 0x30F + attachedCANBus->attach(this, 0x300, 0x7f0, false); +} + +/*For all multibyte integers the format is MSB first, LSB last +*/ +void CANRecieveTest::handleCanFrame(const CAN_message_t &frame) { + Logger::info("Test id=%X len=%X data=%X,%X,%X,%X,%X,%X,%X,%X", + frame.id, frame.len, + frame.buf[0], frame.buf[1], frame.buf[2], frame.buf[3], + frame.buf[4], frame.buf[5], frame.buf[6], frame.buf[7]); +} + +DeviceId CANRecieveTest::getId() { + return (CANRecieveTestID); +} + +DeviceType CANRecieveTest::getType() { + return (DEVICE_MISC); +} + +CANRecieveTest canRecieveTest; \ No newline at end of file diff --git a/src/devices/misc/CANRecieveTest.h b/src/devices/misc/CANRecieveTest.h new file mode 100644 index 0000000..4011e71 --- /dev/null +++ b/src/devices/misc/CANRecieveTest.h @@ -0,0 +1,33 @@ +#ifndef CANRecieveTest_H_ +#define CANRecieveTest_H_ + +#include +#include "../../config.h" +#include "../Device.h" +#include "../../DeviceManager.h" +#include "../../CanHandler.h" + +#define CANRecieveTestID 0x1234 + +class CANRecieveTestConfiguration: DeviceConfiguration +{ +public: + uint8_t canbusNum; +}; + + +class CANRecieveTest : public Device, CanObserver +{ +public: + CANRecieveTest(); + void setup(); + void earlyInit(); + void handleCanFrame(const CAN_message_t &frame); + DeviceId getId(); + DeviceType getType(); + +protected: +private: +}; + +#endif diff --git a/src/devices/misc/analogTest.cpp b/src/devices/misc/analogTest.cpp new file mode 100644 index 0000000..e69de29 diff --git a/src/devices/misc/testDevice.cpp b/src/devices/misc/testDevice.cpp new file mode 100644 index 0000000..6674a53 --- /dev/null +++ b/src/devices/misc/testDevice.cpp @@ -0,0 +1,65 @@ +#include "testDevice.h" + +testDevice::testDevice() : Device() { + commonName = "testDevice"; + shortName = "test"; +} + +void testDevice::earlyInit() +{ + prefsHandler = new PrefHandler(testDeviceID); +} + +void testDevice::setup() { + tickHandler.detach(this); + Logger::info("add device: testDevice (id: %X, %X)", testDeviceID, this); + + + Device::setup(); // run the parent class version of this function + + setAttachedCANBus(0); + + //Relevant BMS messages are 0x300 - 0x30F + attachedCANBus->attach(this, 0x200, 0x7f0, false); + tickHandler.attach(this, testDeviceTickInt); +} + +/*For all multibyte integers the format is MSB first, LSB last +*/ +void testDevice::handleCanFrame(const CAN_message_t &frame) { + Logger::info("Test id=%X len=%X data=%X,%X,%X,%X,%X,%X,%X,%X", + frame.id, frame.len, + frame.buf[0], frame.buf[1], frame.buf[2], frame.buf[3], + frame.buf[4], frame.buf[5], frame.buf[6], frame.buf[7]); +} + +DeviceId testDevice::getId() { + return (testDeviceID); +} + +DeviceType testDevice::getType() { + return (DEVICE_MISC); +} + +void testDevice::handleTick() +{ + var.len = 3; + var.id = 0x201; + var.buf[0] = 0x51; + // 0x04 to DISABLE + var.buf[1] = 0x04; + // 0x00 to ENABLE + //var.buf[1] = 0x00; + var.buf[2] = 0x00; + attachedCANBus->sendFrame(var); + + // send 5% speed + var.buf[0] = 0x31; + var.buf[1] = 0x66; + var.buf[2] = 0x06; + attachedCANBus->sendFrame(var); + + +} + +testDevice test_device; diff --git a/src/devices/misc/testDevice.h b/src/devices/misc/testDevice.h new file mode 100644 index 0000000..063e2fd --- /dev/null +++ b/src/devices/misc/testDevice.h @@ -0,0 +1,25 @@ +#ifndef testDevice_H_ +#define testDevice_H_ + +#include +#include "../Device.h" +#include "../../DeviceManager.h" +#include "../../CanHandler.h" + +#define testDeviceID 0x321 +#define testDeviceTickInt 1000000 + +class testDevice: public Device, CanObserver{ +public: + testDevice(); //called nearly immediately to initialize your own variables + void setup(); //called only if the device is actually enabled + void earlyInit(); //called early and whether or not the device is enabled. Just used to setup configuration + void handleTick(); + void handleCanFrame(const CAN_message_t &frame); + DeviceId getId(); + DeviceType getType(); +private: + CAN_message_t var; + +}; +#endif diff --git a/src/devices/motorctrl/BamocarMotorController.cpp b/src/devices/motorctrl/BamocarMotorController.cpp new file mode 100644 index 0000000..a0b261c --- /dev/null +++ b/src/devices/motorctrl/BamocarMotorController.cpp @@ -0,0 +1,119 @@ +/* + * BamocarMotorController.cpp + */ +#include "../io/PotThrottle.h" +#include "BamocarMotorController.h" +#include + +BamocarMotorController::BamocarMotorController() : MotorController() { + selectedGear = DRIVE; + commonName = "Bamocar Inverter"; + shortName = "BamocarInverter"; +} + +void BamocarMotorController::earlyInit() +{ + prefsHandler = new PrefHandler(BAMOCARINVERTER); +} + +void BamocarMotorController::setup() { + tickHandler.detach(this); + + Logger::info("add device: Bamocar Inverter (id:%X, %X)", BAMOCARINVERTER, this); + + loadConfiguration(); + MotorController::setup(); // run the parent class version of this function + + running = true; + setPowerMode(modeTorque); + setSelectedGear(DRIVE); + setOpState(ENABLE); + + tickHandler.attach(this, CFG_TICK_INTERVAL_MOTOR_CONTROLLER_BAMOCAR); +} + +void BamocarMotorController::handleTick() { + BamocarMotorControllerConfiguration *config = (BamocarMotorControllerConfiguration *)getConfiguration(); + //if the brake is pressed beyond a certain point set the speed back down to 0 + //if the brake is pressed also send a speed signal but lower than the current speed + //if the throttle is released a little then the speed should still be similar, not immediately down to 0 + //max press accelerate max + //make it as close to a regular car + //brakes regular car as well + //willl never really have a cruising speed + //linear + //max push max acceleration + + MotorController::handleTick(); + if (throttleRequested < 0) throttleRequested = 0; + if (throttleRequested > 1000 && throttleRequested < 1300) throttleRequested = 1000; + if (throttleRequested > 1400) throttleRequested = 0; + + + + // //rounding to nearest 10th + int a = (throttleRequested/10); + a = a*327; + uint32_t firsthalf = (a & 0xFF); + uint32_t secondhalf = ((a >> 8) & 0xFF); + + // Logger::warn("First half %i | Second half %i", firsthalf, secondhalf); + // check if the motor will still spin even if the pedal is released all the way up + + var.len = 3; + var.id = 0x201; + var.buf[0] = 0x51; + // 0x04 to DISABLE + var.buf[1] = 0x04; + // 0x00 to ENABLE + //var.buf[1] = 0x00; + var.buf[2] = 0x00; + attachedCANBus->sendFrame(var); + + // send 5% speed + var.buf[0] = 0x31; + var.buf[1] = secondhalf; + var.buf[2] = firsthalf; + attachedCANBus->sendFrame(var); +} + +void BamocarMotorController::handleCanFrame(const CAN_message_t &frame) { + +} + +void BamocarMotorController::setGear(Gears gear) { + selectedGear = gear; + //if the gear was just set to drive or reverse and the DMOC is not currently in enabled + //op state then ask for it by name + if (selectedGear != NEUTRAL) { + operationState = ENABLE; + } + //should it be set to standby when selecting neutral? I don't know. Doing that prevents regen + //when in neutral and I don't think people will like that. +} + +DeviceId BamocarMotorController::getId() { + return (BAMOCARINVERTER); +} + +uint32_t BamocarMotorController::getTickInterval() +{ + return CFG_TICK_INTERVAL_MOTOR_CONTROLLER_BAMOCAR; +} + +void BamocarMotorController::loadConfiguration() { + BamocarMotorControllerConfiguration *config = (BamocarMotorControllerConfiguration *)getConfiguration(); + + if (!config) { + config = new BamocarMotorControllerConfiguration(); + setConfiguration(config); + } + + MotorController::loadConfiguration(); // call parent +} + +void BamocarMotorController::saveConfiguration() { + MotorController::saveConfiguration(); +} + +BamocarMotorController bamocarMC; \ No newline at end of file diff --git a/src/devices/motorctrl/BamocarMotorController.h b/src/devices/motorctrl/BamocarMotorController.h new file mode 100644 index 0000000..fbb0755 --- /dev/null +++ b/src/devices/motorctrl/BamocarMotorController.h @@ -0,0 +1,54 @@ +/* + * BamocarMotorController.h + */ + +#ifndef _BAMOCARINVERTER_H_ +#define _BAMOCARINVERTER_H_ + +#include +#include "../../config.h" +#include "MotorController.h" +#include "../../sys_io.h" +#include "../../TickHandler.h" +#include "../../CanHandler.h" + +#define BAMOCARINVERTER 0x1010 +#define CFG_TICK_INTERVAL_MOTOR_CONTROLLER_BAMOCAR 40000 + +/* + * Class for DMOC specific configuration parameters + */ +class BamocarMotorControllerConfiguration : public MotorControllerConfiguration { +public: +}; + +class BamocarMotorController: public MotorController, CanObserver { +public: + +public: + virtual void handleTick(); + virtual void setup(); + void earlyInit(); + void setGear(Gears gear); + + BamocarMotorController(); + DeviceId getId(); + uint32_t getTickInterval(); + + void handleCanFrame(const CAN_message_t &frame); + + virtual void loadConfiguration(); + virtual void saveConfiguration(); + +private: + + uint16_t torqueCommand; + void timestamp(); + + CAN_message_t var; // delete this when handleTick is fully functional +}; + +#endif /* DMOC_H_ */ + + + diff --git a/src/devices/motorctrl/MotorController.cpp b/src/devices/motorctrl/MotorController.cpp index 55cb69b..a417ce3 100644 --- a/src/devices/motorctrl/MotorController.cpp +++ b/src/devices/motorctrl/MotorController.cpp @@ -149,19 +149,19 @@ void MotorController::handleTick() { //Throttle check Throttle *accelerator = deviceManager.getAccelerator(); - Throttle *brake = deviceManager.getBrake(); + //Throttle *brake = deviceManager.getBrake(); if (accelerator) throttleRequested = accelerator->getLevel(); - if (brake && brake->getLevel() < -10 && brake->getLevel() < accelerator->getLevel()) //if the brake has been pressed it overrides the accelerator. - throttleRequested = brake->getLevel(); + // if (brake && brake->getLevel() < -10 && brake->getLevel() < accelerator->getLevel()) //if the brake has been pressed it overrides the accelerator. + // throttleRequested = brake->getLevel(); //Logger::debug("Throttle: %d", throttleRequested); - if(skipcounter++ > 30) //A very low priority loop for checks that only need to be done once per second. - { - skipcounter=0; //Reset our laptimer - checkEnableInput(); - checkReverseInput(); - } + // if(skipcounter++ > 30) //A very low priority loop for checks that only need to be done once per second. + // { + // skipcounter=0; //Reset our laptimer + // checkEnableInput(); + // checkReverseInput(); + // } } /* diff --git a/src/sys_io.cpp b/src/sys_io.cpp index ccdca7a..47c5919 100644 --- a/src/sys_io.cpp +++ b/src/sys_io.cpp @@ -45,7 +45,7 @@ SystemIO::SystemIO() numDigIn = NUM_DIGITAL; numDigOut = NUM_OUTPUT; - numAnaIn = NUM_ANALOG; + numAnaIn = 6; numAnaOut = 0; pcaDigitalOutputCache = 0; //all outputs off by default adcMuxSelect = 0; @@ -320,11 +320,13 @@ get value of one of the analog inputs int16_t SystemIO::getAnalogIn(uint8_t which) { int valu; + //numAnaIn = 6 if (which > numAnaIn) { return 0; - } - + } + + //num_analog = 8 if (which < NUM_ANALOG) { valu = _pGetAnalogRaw(which);