From 1a1429bf7286f53435580158d3fc4bafd67fe5a5 Mon Sep 17 00:00:00 2001 From: Asbelos Date: Tue, 23 Jun 2020 20:01:43 +0100 Subject: [PATCH] Servo Turnouts --- CVReader.ino | 10 ++---- Config.h | 2 +- Hardware.cpp | 5 +-- Hardware.h | 1 - PWMServoDriver.cpp | 89 ++++++++++++++++++++++++++++++++++++++++++++++ PWMServoDriver.h | 20 +++++++++++ Turnouts.cpp | 4 ++- 7 files changed, 116 insertions(+), 15 deletions(-) create mode 100644 PWMServoDriver.cpp create mode 100644 PWMServoDriver.h diff --git a/CVReader.ino b/CVReader.ino index e42d284..ed2822a 100644 --- a/CVReader.ino +++ b/CVReader.ino @@ -15,11 +15,7 @@ // The filter must be enabled by calling the DCC EXParser::setFilter method, see use in setup(). void myFilter(Stream & stream, byte & opcode, byte & paramCount, int p[]) { - switch (opcode) { - case 'T': // Intercept turnout functions because I have special hardware - DIAG(F("\nStop messing with Turnouts!")); - opcode=0; // tell parssr to ignore ithis command - break; + switch (opcode) { case 'F': // Invent new command to call the new Loco Function API DIAG(F("Setting loco %d F%d %S"),p[0],p[1],p[2]?F("ON"):F("OFF")); DCC::setFn(p[0],p[1],p[2]==1); @@ -51,7 +47,7 @@ DCCEXParser serialParser; // Try monitoring the memory #include "freeMemory.h" -int minMemory; +int minMemory=32767; void setup() { Serial.begin(SERIAL_BAUD_RATE); @@ -65,8 +61,6 @@ void setup() { DCCEXParser::setFilter(myFilter); malloc(1); - minMemory=freeMemory(); - DIAG(F("\nFree memory=%d"),minMemory); DIAG(F("\nReady for JMRI commands\n")); } diff --git a/Config.h b/Config.h index 07be53a..d06c941 100644 --- a/Config.h +++ b/Config.h @@ -25,7 +25,7 @@ const byte PROG_BRAKE_PIN = 10; const float PROG_SENSE_FACTOR=1; // analgRead(PROG_SENSE_PIN) * PROG_SENSE_FACTOR = milliamps // Allocations with memory implications..! -// Base system takes approx 500 bytes + 8 per loco. Turnouts, Sensors etc are dynamically created +// Base system takes approx 700 bytes + 8 per loco. Turnouts, Sensors etc are dynamically created const byte MAX_LOCOS=50; #endif diff --git a/Hardware.cpp b/Hardware.cpp index d767a54..c276e8d 100644 --- a/Hardware.cpp +++ b/Hardware.cpp @@ -4,6 +4,7 @@ #include "Config.h" #include "DIAG.h" + #if defined(ARDUINO_ARCH_AVR) #include // use IDE menu Tools..Manage Libraries to locate and install DIO2 #define WritePin digitalWrite2 @@ -52,10 +53,6 @@ void Hardware::setCallback(int duration, void (*isr)()) { Timer1.attachInterrupt(isr); } -void Hardware::setServo(byte pin, int angle) { - // TBD -} - // Railcom support functions, not yet implemented void Hardware::setSingleCallback(int duration, void (*isr)()) { diff --git a/Hardware.h b/Hardware.h index 1f6e741..7b68466 100644 --- a/Hardware.h +++ b/Hardware.h @@ -11,6 +11,5 @@ class Hardware { static void setCallback(int duration, void (*isr)()); static void setSingleCallback(int duration, void (*isr)()); static void resetSingleCallback(int duration); - static void setServo(byte pin, int angle); }; #endif diff --git a/PWMServoDriver.cpp b/PWMServoDriver.cpp new file mode 100644 index 0000000..8871a75 --- /dev/null +++ b/PWMServoDriver.cpp @@ -0,0 +1,89 @@ + + +/*! + * @file PWMServoDriver.cpp + * + * @mainpage Adafruit 16-channel PWM & Servo driver, based on Adafruit_PWMServoDriver + * + * @section intro_sec Introduction + * + * This is a library for the 16-channel PWM & Servo driver. + * + * Designed specifically to work with the Adafruit PWM & Servo driver. + * This class contains a very small subset of the Adafruit version which + * is relevant to driving simple servos at 50Hz through a number of chained + * servo driver boards (ie servos 0-15 on board 0x40, 16-31 on board 0x41 etc.) + * + * @section author Author + * Chris Harlow (TPL) + * original by Limor Fried/Ladyada (Adafruit Industries). + * + * @section license License + * + * BSD license, all text above must be included in any redistribution + */ +#include +#include +#include "PWMServoDriver.h" +#define DIAG_ENABLED true +#include "DIAG.h" + + +// REGISTER ADDRESSES +const byte PCA9685_MODE1=0x00; // Mode Register +const byte PCA9685_FIRST_SERVO=0x06; /** low byte first servo register ON*/ +const byte PCA9685_PRESCALE=0xFE; /** Prescale register for PWM output frequency */ +// MODE1 bits +const byte MODE1_SLEEP=0x10; /**< Low power mode. Oscillator off */ +const byte MODE1_AI=0x20; /**< Auto-Increment enabled */ +const byte MODE1_RESTART=0x80; /**< Restart enabled */ + +const byte PCA9685_I2C_ADDRESS=0x40; /** First PCA9685 I2C Slave Address */ +const float FREQUENCY_OSCILLATOR=25000000.0; /** Accurate enough for our purposes */ +const uint8_t PRESCALE_50HZ = (uint8_t)(((FREQUENCY_OSCILLATOR / (50.0 * 4096.0)) + 0.5) - 1); + +/*! + * @brief Sets the PWM frequency for a chip to 50Hz for servos + */ + +byte PWMServoDriver::setupFlags=0; // boards that have been initialised + +void PWMServoDriver::setup(int board) { + if (board>3 || setupFlags & (1<3) return; // safe dropout + setup(board); // in case not already done + + Wire.beginTransmission(PCA9685_I2C_ADDRESS + board); + Wire.write(PCA9685_FIRST_SERVO + 4 * pin); // 4 registers per pin + Wire.write(0); + Wire.write(0); + Wire.write(value); + Wire.write(value >> 8); + Wire.endTransmission(); +} + +void PWMServoDriver::writeRegister(uint8_t i2caddr,uint8_t hardwareRegister, uint8_t d) { + Wire.beginTransmission(i2caddr); + Wire.write(hardwareRegister); + Wire.write(d); + Wire.endTransmission(); + delay(5); // allow registers to settle before continuing +} diff --git a/PWMServoDriver.h b/PWMServoDriver.h new file mode 100644 index 0000000..ca8b304 --- /dev/null +++ b/PWMServoDriver.h @@ -0,0 +1,20 @@ +/*! + * @file PWMServoDriver.h + * + * Used to set servo positions on an I2C bus with 1 or more PCA96685 boards. + */ +#ifndef PWMServoDriver_H +#define PWMServoDriver_H + + +class PWMServoDriver { +public: + static void setServo(short servoNum, uint16_t pos); + +private: + static byte setupFlags; + static void setup(int board); + static void writeRegister(uint8_t i2caddr,uint8_t hardwareRegister, uint8_t d); +}; + +#endif diff --git a/Turnouts.cpp b/Turnouts.cpp index 47ca972..3d6fd29 100644 --- a/Turnouts.cpp +++ b/Turnouts.cpp @@ -2,6 +2,8 @@ #include "EEStore.h" #include "StringFormatter.h" #include "Hardware.h" +#include "PWMServoDriver.h" + bool Turnout::activate(int n,bool state){ Turnout * tt=get(n); if (tt==NULL) return false; @@ -14,7 +16,7 @@ void Turnout::activate(bool state) { if (state) data.tStatus|=STATUS_ACTIVE; else data.tStatus &= ~STATUS_ACTIVE; - if (data.tStatus & STATUS_PWM) Hardware::setServo(data.tStatus & STATUS_PWMPIN, (data.inactiveAngle+state?data.moveAngle:0)); + if (data.tStatus & STATUS_PWM) PWMServoDriver::setServo(data.tStatus & STATUS_PWMPIN, (data.inactiveAngle+state?data.moveAngle:0)); else DCC::setAccessory(data.address,data.subAddress, state); } ///////////////////////////////////////////////////////////////////////////////