diff --git a/DCC.cpp b/DCC.cpp index 79c5b19..2268278 100644 --- a/DCC.cpp +++ b/DCC.cpp @@ -61,8 +61,15 @@ void DCC::begin(const FSH * motorShieldName, MotorDriver * mainDriver, MotorDriv EEStore::init(); DCCWaveform::begin(mainDriver,progDriver); +#ifdef DCdistrict + DCCWaveform::mainTrack.pwmSpeed(0); +#else + DCCWaveform::mainTrack.pwmSpeed(255); +#endif + DCCWaveform::progTrack.pwmSpeed(255); } + void DCC::setJoinRelayPin(byte joinRelayPin) { joinRelay=joinRelayPin; if (joinRelay!=UNUSED_PIN) { @@ -72,6 +79,13 @@ void DCC::setJoinRelayPin(byte joinRelayPin) { } void DCC::setThrottle( uint16_t cab, uint8_t tSpeed, bool tDirection) { +#ifdef DCdistrict + if (cab == DCdistrict) { + DCCWaveform::mainTrack.pwmSpeed(tSpeed, tDirection); + } +#else + #error fooar +#endif byte speedCode = (tSpeed & 0x7F) + tDirection * 128; setThrottle2(cab, speedCode); // retain speed for loco reminders diff --git a/DCCWaveform.cpp b/DCCWaveform.cpp index 0d23897..97ae944 100644 --- a/DCCWaveform.cpp +++ b/DCCWaveform.cpp @@ -65,7 +65,9 @@ void DCCWaveform::interruptHandler() { byte sigProg=progTrackSyncMain? sigMain : signalTransform[progTrack.state]; // Set the signal state for both tracks +#ifndef DCdistrict mainTrack.motorDriver->setSignal(sigMain); +#endif progTrack.motorDriver->setSignal(sigProg); // Move on in the state engine diff --git a/DCCWaveform.h b/DCCWaveform.h index 29d6a29..db58574 100644 --- a/DCCWaveform.h +++ b/DCCWaveform.h @@ -107,6 +107,27 @@ class DCCWaveform { inline void setMaxAckPulseDuration(unsigned int i) { maxAckPulseDuration = i; } +#ifdef DCdistrict + inline void pwmSpeed(uint8_t tSpeed) { + // DCC Speed with 0,1 stop and speed steps 2 to 127 + uint8_t brake; + if (!motorDriver) + return; + if (tSpeed <= 1) + brake = 255; + else if (tSpeed >= 127) + brake = 0; + else + brake = 2 * (128-tSpeed); + motorDriver->setBrake(brake); + } + inline void pwmSpeed(uint8_t tSpeed, bool tDirection) { + if (!motorDriver) + return; + pwmSpeed(tSpeed); + motorDriver->setSignal(tDirection); + } +#endif private: diff --git a/MotorDriver.cpp b/MotorDriver.cpp index f51ee04..6da3b53 100644 --- a/MotorDriver.cpp +++ b/MotorDriver.cpp @@ -31,6 +31,7 @@ bool MotorDriver::commonFaultPin=false; MotorDriver::MotorDriver(byte power_pin, byte signal_pin, byte signal_pin2, int8_t brake_pin, byte current_pin, float sense_factor, unsigned int trip_milliamps, byte fault_pin) { + brakePWM=false; powerPin=power_pin; getFastPin(F("POWER"),powerPin,fastPowerPin); pinMode(powerPin, OUTPUT); @@ -53,7 +54,7 @@ MotorDriver::MotorDriver(byte power_pin, byte signal_pin, byte signal_pin2, int8 brakePin=invertBrake ? 0-brake_pin : brake_pin; getFastPin(F("BRAKE"),brakePin,fastBrakePin); pinMode(brakePin, OUTPUT); - setBrake(false); +// setBrake(0); moved out to DCC::begin } else brakePin=UNUSED_PIN; @@ -89,8 +90,11 @@ void MotorDriver::setPower(bool on) { if (on) { // toggle brake before turning power on - resets overcurrent error // on the Pololu board if brake is wired to ^D2. - setBrake(true); - setBrake(false); + // Yes, this is an ugly special case + if (brakePin == 4 && invertBrake) { + setBrake(255); + setBrake(0); + } setHIGH(fastPowerPin); } else setLOW(fastPowerPin); @@ -104,10 +108,29 @@ void MotorDriver::setPower(bool on) { // (HIGH == release brake) and setBrake does // compensate for that. // -void MotorDriver::setBrake(bool on) { +void MotorDriver::setBrake(uint8_t intensity) { if (brakePin == UNUSED_PIN) return; - if (on ^ invertBrake) setHIGH(fastBrakePin); - else setLOW(fastBrakePin); + DIAG(F("Brake pin=%d val=%d"),brakePin,intensity); + if (invertBrake) + intensity = 255 - intensity; + if (intensity == 255) { + if (brakePWM) { + digitalWrite(brakePin, HIGH); + brakePWM = false; + } else + setHIGH(fastBrakePin); + return; + } + if (intensity == 0) { + if (brakePWM) { + digitalWrite(brakePin, LOW); + brakePWM = false; + } else + setLOW(fastBrakePin); + return; + } + brakePWM = true; + analogWrite(brakePin, intensity); } void MotorDriver::setSignal( bool high) { diff --git a/MotorDriver.h b/MotorDriver.h index 08db049..a750fe4 100644 --- a/MotorDriver.h +++ b/MotorDriver.h @@ -46,7 +46,7 @@ class MotorDriver { byte current_pin, float senseFactor, unsigned int tripMilliamps, byte faultPin); virtual void setPower( bool on); virtual void setSignal( bool high); - virtual void setBrake( bool on); + virtual void setBrake(uint8_t); virtual int getCurrentRaw(); virtual unsigned int raw2mA( int raw); virtual int mA2raw( unsigned int mA); @@ -69,6 +69,7 @@ class MotorDriver { FASTPIN fastPowerPin,fastSignalPin, fastSignalPin2, fastBrakePin,fastFaultPin; bool dualSignal; // true to use signalPin2 bool invertBrake; // brake pin passed as negative means pin is inverted + bool brakePWM; // brake is used for PWM float senseFactor; int senseOffset; unsigned int tripMilliamps; diff --git a/config.example.h b/config.example.h index 9b1855f..e5f18cb 100644 --- a/config.example.h +++ b/config.example.h @@ -23,6 +23,14 @@ The configuration file for DCC-EX Command Station **********************************************************************/ +// TO GET THE DC district feature put this in your config.h: +// +//#define BRAKE_MOTOR_SHIELD F("BRAKE_MOTOR_SHIELD"), \ +// new MotorDriver(3, 12, UNUSED_PIN, 9, A0, 2.99, 2000, UNUSED_PIN), \ +// new MotorDriver(11, 13, UNUSED_PIN, UNUSED_PIN, A1, 2.99, 2000, UNUSED_PIN) +//#define MOTOR_SHIELD_TYPE BRAKE_MOTOR_SHIELD +//#define DCdistrict 2 + ///////////////////////////////////////////////////////////////////////////////////// // NOTE: Before connecting these boards and selecting one in this software // check the quick install guides!!! Some of these boards require a voltage