No LED glow when built-in led was still connected.
#define GPIO_ADDR 0x27 // LCD I2C address
// Enable the following line to compile for SPI, comment for I2C
// #define COMPILE_FOR_SPI
// Enable the following line to compile with debug printout
#define DEBUG
#ifdef COMPILE_FOR_SPI
#include <SPI.h>
#endif
#include <LCD.h>
#include <Wire.h>
#include <EEPROM.h>
#include <VarSpeedServo.h>
#include <LiquidCrystal_I2C.h>
#include "MCP23S17.h"
#include "ServoDefaults.h"
LiquidCrystal_I2C lcd(GPIO_ADDR, 2, 1, 0, 4, 5, 6, 7, 3, POSITIVE);
// Comment out the following define to re-load EEPROM
//#define SERVOS_LOADED
#define TWI_FREQ_PCA 400000L // Sets the I2C speed to 400KHz
#define MCP_ADDRESS 0x20 // MCP address (all address bits grounded)
#define NUMBER_SERVOS 8 // Number of servos handled by the MCP23S17
#define ENTRIES_PER_SERVO 4 // Number of EEPROM entries reserved for each servo
// Create lcd instance
// LiquidCrystal_I2C lcd(0x27,20,4); // Set the I2C address and display size.
// Create an instance for each servo
VarSpeedServo varServo[NUMBER_SERVOS]; // Create NUMBER_SERVOS servo instances to control the servos
// A maximum of eight servo objects can be created
uint8_t SERVOpin[] = {4, 5, 6, 7, 8, 9, A0, A1}; // Define the drive pins for each servo
#ifdef COMPILE_FOR_SPI
const uint8_t SPIslave = 0x20; // MCP23S17 address, all ADDR pins are grounded
const uint8_t MCP_WRITE = (SPIslave << 1 | 0x00); // MCP23S17 opcode write has LSB clear
const uint8_t MCP_READ = (SPIslave << 1 | 0x01); // MCP23S17 opcode read has LSB set
#endif
#define MASTER_RESET 120 // THIS IS THE ADDRESS FOR A FULL RESET - Set = 120
// Rotary Encoder defines and global (volatile) variables
#define ENC_SWITCH A3 // Encoder switch pin
#define PIN_A 2 // Hardware interrupt pin 2 (interupt 0)
#define PIN_B 3 // Hardware interrupt pin 3 (interupt 1)
volatile byte aFlag = 0; // Flag to expect a rising edge on pinA to signal a detent
volatile byte bFlag = 0; // Flag to expect a rising edge on pinB to signal a detent (opposite direction to when aFlag is set)
volatile int16_t encoderValue; // Current encoder value.
struct QUEUE {
bool inUse;
uint8_t direction;
uint8_t servoSpeed;
uint8_t straightAngle;
uint8_t divergeAngle;
uint8_t currentAngle;
uint32_t timeOut;
};
QUEUE* servoQueue = new QUEUE[NUMBER_SERVOS];
uint8_t interruptPin = PC2; // Pin A2
uint16_t detachDelay; // Variable for servo detach delay
///////////////////////////////////////////////////////////////////////////////
//
void setup() {
// put your setup code here, to run once:
#ifdef DEBUG
Serial.begin(9600);
delay(250);
Serial.println("DCCpp_Willem_turnoutcontroller_single_MCP_v4.ino");
Serial.println();
#endif
#ifdef COMPILE_FOR_SPI
pinMode(SS, OUTPUT); // Configure controller's Slave Select pin to output
digitalWrite(SS, HIGH); // Disable Slave Select (pin 10)
SPI.begin();
SPI.setClockDivider(SPI_CLOCK_DIV8); // Divide the SPI clock by 8
#endif
Wire.begin();
Wire.setClock(TWI_FREQ_PCA); // Switch the I2C speed to 400KHz
// Setup Rotary Encoder
pinMode(ENC_SWITCH, INPUT_PULLUP);
pinMode(PIN_A, INPUT_PULLUP); // set pinA as an input, pulled HIGH to the logic voltage
pinMode(PIN_B, INPUT_PULLUP); // set pinB as an input, pulled HIGH to the logic voltage
attachInterrupt(0, encoderPinA, RISING); // set an interrupt on PinA, looking for a rising edge signal and
// executing the "PinA" Interrupt Service Routine (below)
attachInterrupt(1, encoderPinB, RISING); // set an interrupt on PinB, looking for a rising edge signal and
// executing the "PinB" Interrupt Service Routine (below)
// DDRB |= (1 << PB0); // Set pins PB0 (pin 8) as output
// If not already saved, save the default Servos values to EEPROM
#ifdef SERVOS_LOADED
if ( readEEPROM(MASTER_RESET) == MASTER_RESET ) // This entry is initialised = 0; Set = MASTER_RESET to force reset
#endif
{
for (uint16_t servo = 0; servo < sizeof(servoDefaults) / sizeof(servoPair); servo++ )
writeEEPROM( servoDefaults[servo].Servo, servoDefaults[servo].Value);
}
// Initialize the MCP23017
MCP_begin();
// Initialize the Queue enabling all servos
Serial.println("Setting up the servoQueue.");
for ( uint8_t servo = 0; servo < NUMBER_SERVOS; servo++ ) {
uint16_t servoOffset = servo * ENTRIES_PER_SERVO;
servoQueue[servo].inUse = true;
servoQueue[servo].servoSpeed = int (readEEPROM(34 + servoOffset));
servoQueue[servo].straightAngle = int (readEEPROM(35 + servoOffset));
servoQueue[servo].divergeAngle = int (readEEPROM(36 + servoOffset));
servoQueue[servo].currentAngle = int (readEEPROM(37 + servoOffset));
servoQueue[servo].direction = 2; // Initially set to non valid value
#ifdef DEBUG
Serial.print("speed = ");
Serial.print(servoQueue[servo].servoSpeed);
Serial.print("; straight = ");
Serial.print(servoQueue[servo].straightAngle);
Serial.print("; divert = ");
Serial.print(servoQueue[servo].divergeAngle);
Serial.print("; current = ");
Serial.println(servoQueue[servo].currentAngle);
#endif
// Write servo position and attach it
varServo[servo].write(servoQueue[servo].currentAngle, 50, false);
varServo[servo].attach(SERVOpin[servo]); // attaches the servo to its pin
}
detachDelay = uint16_t (readEEPROM(33)) * 10;
Serial.print("servoQueue done. - ");
Serial.println(detachDelay);
#ifdef DEBUG
// Temporary debug printout
#ifdef COMPILE_FOR_SPI
uint8_t IntCap = readSPIbyte(INTFA); // Read INTFA
#else
uint8_t IntCap = readMCPbyte(INTFA); // Read INTFA
#endif
Serial.print("INTFA:");
Serial.print(IntCap, HEX);
#ifdef COMPILE_FOR_SPI
IntCap = readSPIbyte(INTFB); // Read INTFB
#else
IntCap = readMCPbyte(INTFB); // Read INTFB
#endif
Serial.print(" B:");
Serial.println(IntCap, HEX);
#endif
}
///////////////////////////////////////////////////////////////////////////////
//
void loop() {
// Check if any servos should be detached
for ( uint8_t servo = 0; servo < NUMBER_SERVOS; servo++ ) {
// Check if servo is in use
if ( servoQueue[servo].inUse ) {
// Has servo reached destination angle? If so, remove from use and set timeout
if ( varServo[servo].read() == servoQueue[servo].currentAngle ) {
servoQueue[servo].inUse = false;
servoQueue[servo].timeOut = millis();
}
} else {
// If servo is still attached and timeout has lapsed, detach
if ( varServo[servo].attached() && ((millis() - servoQueue[servo].timeOut) >= detachDelay) )
varServo[servo].detach();
}
}
// Check for interrupt on MCP23x17
if ( PINC & (1 << interruptPin) ) {
checkMCP();
}
// Check if the emcoder switch was pressed to start setup
if ( readEncSwitch() )
servoMenu();
}
///////////////////////////////////////////////////////////////////////////////
//
void checkMCP() {
#ifdef COMPILE_FOR_SPI
uint16_t intFlags = readSPIword( (uint8_t)INTFA ); // Read both interrupt flag registers
#else
uint16_t intFlags = readMCPword( (uint8_t)INTFA );
#endif
#ifdef DEBUG
Serial.print("INTF:");
Serial.print(intFlags, HEX);
#endif
if ( intFlags > 0 ) {
// separate the interrupts for INTFA and INTFB
uint8_t intFlagsA = intFlags & 0x00FF;
uint8_t intFlagsB = (intFlags >> 8);
#ifdef DEBUG
Serial.print("; flags B:A = ");
Serial.print(intFlagsB, HEX);
Serial.print(":");
Serial.print(intFlagsA, HEX);
#endif
// Read the capture registers which also clears the interrupt flags registers
delay(50); // Add a possible debounce delay
#ifdef COMPILE_FOR_SPI
uint16_t captureStatus = readSPIword( (uint8_t)INTCAPA ); // Read both interrupt capture registers
#else
uint16_t captureStatus = readMCPword( (uint8_t)INTCAPA ); // Read both interrupt capture registers
#endif
#ifdef DEBUG
Serial.print("; INTCAP = ");
Serial.print(captureStatus, HEX);
#endif
// If interrupt is for GPIOA set servo to straight
if ( intFlagsA ) {
for ( int8_t servo = 0; servo < NUMBER_SERVOS; servo++ ) { // Checking NUMBER_SERVOS intFlags bits
if ( intFlagsA & (1 << servo) ) {
// If the captureStatus bit is low update the servo direction - else ignore
if ( !(captureStatus & (1 << servo)) ) { // Check against low byte of captureStatus
// if ( 0b01010101 & (1 << servo) ) {
if ( 0b01010101 & (1 << servo) ) {
setServo(servo / 2, 0);
#ifdef DEBUG
Serial.print("; Servo-");
Serial.print(servo / 2);
Serial.print(" = Straight");
#endif
}
else {
setServo(servo / 2, 1);
#ifdef DEBUG
Serial.print("; Servo-");
Serial.print(servo / 2);
Serial.print(" = Divert");
#endif
}
}
}
}
}
// If interrupt is for GPIOB set servo to divert
if ( intFlagsB ) {
for ( int8_t servo = 0; servo < NUMBER_SERVOS; servo++ ) { // Checking NUMBER_SERVOS intFlags bits
if ( intFlagsB & (1 << servo) ) {
// If the captureStatus bit is low update the servo direction - else ignore
if ( !(captureStatus & (1 << (servo + 8))) ) { // Check against high byte of captureStatus
// if ( 0b01010101 & (1 << servo) ) {
if ( 0b01010101 & (1 << servo) ) {
setServo(4 + servo / 2, 0);
#ifdef DEBUG
Serial.print("; Servo-");
Serial.print(4 + servo / 2);
Serial.print(" = Straight");
#endif
}
else {
setServo(4 + servo / 2, 1);
#ifdef DEBUG
Serial.print("; Servo-");
Serial.print(4 + servo / 2);
Serial.print(" = Divert");
#endif
}
}
}
}
}
}
#ifdef DEBUG
Serial.println();
#endif
}
///////////////////////////////////////////////////////////////////////////////
// Servo direction: 0 - Straight; 1 - Divert
void setServo(int16_t servo, int8_t direction) {
if ( direction != servoQueue[servo].direction ) {
switch ( direction ) {
case 0: // Change to straight
varServo[servo].write(servoQueue[servo].straightAngle, servoQueue[servo].servoSpeed, false);
servoQueue[servo].currentAngle = servoQueue[servo].straightAngle;
servoQueue[servo].direction = 0;
break;
case 1: // Change to divert
varServo[servo].write(servoQueue[servo].divergeAngle, servoQueue[servo].servoSpeed, false);
servoQueue[servo].currentAngle = servoQueue[servo].divergeAngle;
servoQueue[servo].direction = 1;
break;
}
if ( !servoQueue[servo].inUse ) {
servoQueue[servo].inUse = true;
varServo[servo].attach(SERVOpin[servo]);
}
#ifdef DEBUG
Serial.print(" Servo -> Speed = ");
Serial.print(servoQueue[servo].servoSpeed);
Serial.print("; Set Target = ");
Serial.print(servoQueue[servo].currentAngle);
#endif
}
}
///////////////////////////////////////////////////////////////////////////////
//
void MCP_begin() {
// NOTE: The following register addresses assume IOCON.BANK = 0
// Setup MCP23017 Ports - PortA all inputs with pull-up
#ifdef COMPILE_FOR_SPI
// ... - Port A all inputs with pull-up
writeSPIbyte(IODIRA, 0xFF); // Set MCP23017 Port A as all Input (0=output; 1=input)
writeSPIbyte(GPPUA, 0xFF); // Enable internal pullup for Port A inputs
// ... - Port B all inputs with pull-up
writeSPIbyte(IODIRB, 0xFF); // Set MCP23017 Port B as all Input (0=output; 1=input)
writeSPIbyte(GPPUB, 0xFF); // Enable internal pullup for Port B inputs
// Interrupt_On_Change for all pins on both Ports
writeSPIbyte(GPINTENA, 0xFF); // GPINTENA - Enable all bits for Int_On_Change
writeSPIbyte(GPINTENB, 0xFF); // GPINTENB - Enable all bits for Int_On_Change
// Set a default value for when Interrupt_On_Change from default value is used for both Ports
writeSPIbyte(DEFVALA, 0xFF); // DEFVALA - Set default value to all HIGH
writeSPIbyte(DEFVALB, 0xFF); // DEFVALB - Set default value to all HIGH
// Set Interrupt_On_Change from previous for both Ports
writeSPIbyte(INTCONA, 0x00); // INTCONA - Set all bits for trigger on change from previous
writeSPIbyte(INTCONB, 0x00); // INTCONB - Set all bits for trigger on change from previous
// Set INT pins MIRRORed and INT active high
writeSPIbyte(IOCON, 0x42); // IOCON - Interrupt Mirror and Interrupt active high
// Read GPIOA & B to clear all possible interrupts
#ifdef DEBUG
uint16_t intGPIO = readSPIword(GPIOA);
Serial.print("Init GPIOA = ");
Serial.println(intGPIO, HEX);
#else
readSPIword(GPIOA);
#endif
#else
writeMCPbyte(IODIRA, 0xFF); // Set MCP23017 Port A as all Input (0=output; 1=input)
writeMCPbyte(GPPUA, 0xFF); // Enable internal pullup for Port A inputs
// ... - PortB all inputs with pull-up
writeMCPbyte(IODIRB, 0xFF); // Set MCP23017 Port B as all Input (0=output; 1=input)
writeMCPbyte(GPPUB, 0xFF); // Enable internal pullup for Port B inputs
// Interrupt_On_Change for all pins on both Ports
writeMCPbyte(GPINTENA, 0xFF); // GPINTENA - Enable all bits for Int_On_Change
writeMCPbyte(GPINTENB, 0xFF); // GPINTENB - Enable all bits for Int_On_Change
// Set a default value for when Interrupt_On_Change from default value is used for both Ports
writeMCPbyte(DEFVALA, 0xFF); // DEFVALA - Set default value to all HIGH
writeMCPbyte(DEFVALB, 0xFF); // DEFVALB - Set default value to all HIGH
// Set Interrupt_On_Change from previous for both Ports
writeMCPbyte(INTCONA, 0x00); // INTCONA - Set all bits for trigger on change from previous
writeMCPbyte(INTCONB, 0x00); // INTCONB - Set all bits for trigger on change from previous
// Set INT pins MIRRORed and INT active high
writeMCPbyte(IOCON, 0x42); // IOCON - Interrupt Mirror and Interrupt active high
// Read GPIOA & B to clear all possible interrupts
#ifdef DEBUG
uint16_t intGPIO = readMCPword(GPIOA);
Serial.print("Init GPIOA = ");
Serial.println(intGPIO, HEX);
#else
readMCPword(GPIOA);
#endif
#endif
}
///////////////////////////////////////////////////////////////////////////////
//
uint16_t readMCPword(uint8_t port) {
Wire.beginTransmission(MCP_ADDRESS);
Wire.write(port); // set MCP23017 register pointer
Wire.endTransmission();
Wire.requestFrom(MCP_ADDRESS, 2); // request two bytes of data
uint16_t value = Wire.read() & 0xFF; // Read from PortA
value |= (Wire.read() << 8); // Read from PortB
return value; // read and return the requested byte
}
///////////////////////////////////////////////////////////////////////////////
//
uint8_t readMCPbyte(uint8_t port) {
Wire.beginTransmission(MCP_ADDRESS);
Wire.write(port); // set MCP23017 register pointer
Wire.endTransmission();
Wire.requestFrom(MCP_ADDRESS, 1); // request a single byte of data from PortA
return Wire.read(); // read and return the requested byte
}
///////////////////////////////////////////////////////////////////////////////
//
void writeMCPbyte(uint8_t port, uint8_t value) {
Wire.beginTransmission(MCP_ADDRESS);
Wire.write(port); // Set required port
Wire.write(value); // Load value into port
Wire.endTransmission();
}
#ifdef COMPILE_FOR_SPI
///////////////////////////////////////////////////////////////////////////////
//
uint16_t readSPIword(uint8_t port) {
SPI.beginTransaction(SPISettings (SPI_CLOCK_DIV8, MSBFIRST, SPI_MODE0)); // Gain control of SPI bus
PORTB &= ~0x04; // Write SS (pin 10) LOW
SPI.transfer(MCP_READ); // Read command
SPI.transfer(port); // Register address to read data from
uint16_t SPIdata = SPI.transfer(0) & 0xFF; // Save the data (0 is dummy data to send)
SPIdata |= (SPI.transfer(0) << 8);
PORTB |= 0x04; // Write SS HIGH
SPI.endTransaction(); // Release the SPI bus
return SPIdata;
}
///////////////////////////////////////////////////////////////////////////////
//
uint8_t readSPIbyte(byte port) {
SPI.beginTransaction(SPISettings (SPI_CLOCK_DIV8, MSBFIRST, SPI_MODE0)); //gain control of SPI bus
PORTB &= ~0x04; // Write SS LOW
SPI.transfer(MCP_READ); // read command
SPI.transfer(port); // register address to read data from
uint8_t SPIdata = SPI.transfer(0); // save the data (0 is dummy data to send)
PORTB |= 0x04; // Write SS HIGH
SPI.endTransaction(); //release the SPI bus
return SPIdata;
}
///////////////////////////////////////////////////////////////////////////////
//
void writeSPIbyte(uint8_t port, uint8_t value) {
SPI.beginTransaction(SPISettings (SPI_CLOCK_DIV8, MSBFIRST, SPI_MODE0)); //gain control of SPI bus
PORTB &= ~0x04; // Write SS low
SPI.transfer(MCP_WRITE); // read command
SPI.transfer(port); // register address to read data from
SPI.transfer(value); // register address to read data from
PORTB |= 0x04; // Write SS HIGH
SPI.endTransaction(); //release the SPI bus
}
#endif
///////////////////////////////////////////////////////////////////////////////
//
uint8_t readEEPROM( uint16_t Servo ) {
uint8_t Value;
Value = EEPROM.read(Servo);
return Value;
}
///////////////////////////////////////////////////////////////////////////////
//
uint8_t writeEEPROM( uint16_t Servo, uint8_t Value ) {
/* if ( EEPROM.read(Servo) != Value ) {
EEPROM.write(Servo,Value);
} */
EEPROM.update(Servo, Value);
return EEPROM.read(Servo);
}
///////////////////////////////////////////////////////////////////////////////
//
void servoMenu() {
uint8_t encSwitchStatus;
uint8_t oldEncValue; // used as hold limit for the previous encoder position value.
uint8_t servo; // the address of servo to be adjusted
bool exitSet = false;
bool switchPressed = false;
oldEncValue = 0;
encoderValue = 0;
servo = 0;
// Initialise LCD display
lcd.begin(20, 4); // initialize the LCD
lcd.clear();
// lcd.noBacklight(); // Turn off the backlight
showMenu(0, 0);
lcd.setCursor(19, 0);
lcd.print("<");
// lcd.backlight(); // Turn off the backlight
while ( !exitSet ) {
if ( oldEncValue != encoderValue ) { // Encoder was rotated - update the current servo
encoderValue = constrain( encoderValue, 0, 2); // Ensure encoderValue is within bounds
lcd.setCursor(19, oldEncValue);
lcd.print(" ");
lcd.setCursor(19, encoderValue);
lcd.print("<");
oldEncValue = encoderValue;
}
encSwitchStatus = readEncSwitch(); // Read the Encoder switch
if ( encSwitchStatus && !switchPressed ) {
switchPressed = true;
}
if ( switchPressed ) {
switch ( encoderValue ) {
case 0:
servo = selectServo(servo);
showMenu(0, servo);
lcd.setCursor(19, 0);
lcd.print("<");
break;
case 1:
setAngles(servo);
showMenu(0, servo);
lcd.setCursor(19, 1);
lcd.print("<");
break;
case 2:
lcd.clear();
lcd.noBacklight(); // Turn off the backlight
exitSet = true;
break;
}
switchPressed = false;
encoderValue = oldEncValue;
while ( readEncSwitch() ); // ensure encoder switch released
}
}
}
///////////////////////////////////////////////////////////////////////////////
//
uint8_t selectServo(uint8_t servo) {
uint8_t encSwitchStatus;
uint8_t oldEncValue; // used as hold limit for the previous encoder position value.
bool exitSet = false;
oldEncValue = servo;
encoderValue = servo;
// Initialise LCD display for servo ID
showMenu(1, servo);
while ( readEncSwitch() ); // ensure encoder switch released
while ( !exitSet ) {
if ( oldEncValue != encoderValue ) { // Encoder was rotated - update the current servo
encoderValue = constrain( encoderValue, 0, NUMBER_SERVOS - 1); // Ensure encoderValue is within bounds
lcd.setCursor(17, 1);
lcd.print(encoderValue);
lcd.print(" ");
oldEncValue = encoderValue;
}
encSwitchStatus = readEncSwitch(); // Read the Encoder switch
if ( encSwitchStatus )
exitSet = true;
}
return encoderValue;
}
///////////////////////////////////////////////////////////////////////////////
//
void setAngles(uint8_t servo) {
uint32_t timePressed = 0; // Time when switch status changed
uint8_t encSwitchStatus;
uint8_t oldEncValue; // used as hold limit for the previous encoder position value.
bool exitSet = false;
bool switchPressed = false;
uint8_t straightAngle = servoQueue[servo].straightAngle;
uint8_t divertAngle = servoQueue[servo].divergeAngle;
uint8_t direction = 0;
oldEncValue = straightAngle;
encoderValue = straightAngle;
// Move servo to initially straight
sendServoAngle(servo, straightAngle);
// Initialise LCD display for setting angles
showMenu(2, servo);
lcd.setCursor(19, 1);
lcd.print("<");
while ( readEncSwitch() ); // ensure encoder switch released
while ( !exitSet ) {
if ( oldEncValue != encoderValue ) { // Encoder was rotated - update the current servo
encoderValue = constrain( encoderValue, 0, 180); // Ensure encoderValue is within bounds
switch ( direction ) {
case 0: // straight
straightAngle = encoderValue;
lcd.setCursor(15, 1);
break;
case 1: // straight
divertAngle = encoderValue;
lcd.setCursor(15, 2);
break;
}
sendServoAngle(servo, encoderValue);
lcd.print(encoderValue);
lcd.print(" ");
oldEncValue = encoderValue;
}
encSwitchStatus = readEncSwitch(); // Read the Encoder switch
if ( encSwitchStatus && !switchPressed ) {
timePressed = millis();
switchPressed = true;
}
if ( switchPressed ) {
if ( encSwitchStatus ) { // Switch currently pressed
if ( (millis() - timePressed) >= 1200 ) { // Hold for 1.2sec to exit setup function
// Ensure current direction angle gets set before save
switch ( direction ) {
case 0: // Straight
straightAngle = encoderValue;
break;
case 1: // Divert
divertAngle = encoderValue;
break;
}
// Save the new angles to EEPROM
if ( writeAngles(servo, straightAngle, divertAngle) ) {
servoQueue[servo].straightAngle = straightAngle;
servoQueue[servo].divergeAngle = divertAngle;
} else
sendServoAngle(servo, servoQueue[servo].straightAngle);
exitSet = true;
}
} else {
switch ( direction ) { // Save current direction angle and set for new
case 0: // From straight to divert
straightAngle = encoderValue;
encoderValue = divertAngle;
lcd.setCursor(19, 1);
lcd.print(" ");
lcd.setCursor(19, 2);
lcd.print("<");
break;
case 1: // From divert to straight
divertAngle = encoderValue;
encoderValue = straightAngle;
lcd.setCursor(19, 2);
lcd.print(" ");
lcd.setCursor(19, 1);
lcd.print("<");
break;
}
direction = ( direction ) ? 0 : 1; // Toggle active servo direction
switchPressed = false;
}
}
}
}
///////////////////////////////////////////////////////////////////////////////
//
void sendServoAngle(int servo, int angle) {
varServo[servo].write(angle, 0, false);
if ( !servoQueue[servo].inUse ) {
servoQueue[servo].inUse = true;
varServo[servo].attach(SERVOpin[servo]);
}
}
///////////////////////////////////////////////////////////////////////////////
//
bool writeAngles(int servo, int straight, int divert) {
uint8_t encSwitchStatus;
uint8_t oldEncValue; // used as hold limit for the previous encoder position value.
bool exitSet = false;
bool exitStatus = false;
oldEncValue = 1;
encoderValue = 1;
showMenu(3, servo);
lcd.setCursor(19, 1);
lcd.print("<");
while ( readEncSwitch() ); // ensure encoder switch released
while ( !exitSet ) {
if ( oldEncValue != encoderValue ) { // Encoder was rotated - update the current servo
encoderValue = constrain( encoderValue, 1, 2); // Ensure encoderValue is within bounds
lcd.setCursor(19, oldEncValue);
lcd.print(" ");
lcd.setCursor(19, encoderValue);
lcd.print("<");
oldEncValue = encoderValue;
}
encSwitchStatus = readEncSwitch(); // Read the Encoder switch
if ( encSwitchStatus ) {
if ( encoderValue == 2 ) {
uint8_t servoOffset = servo * ENTRIES_PER_SERVO;
writeEEPROM(35 + servoOffset, straight);
writeEEPROM(36 + servoOffset, divert);
exitStatus = true;
}
exitSet = true;
while ( readEncSwitch() ); // ensure encoder switch released
}
}
return exitStatus;
}
///////////////////////////////////////////////////////////////////////////////
//
void showMenu(uint8_t menu, uint8_t servo) {
lcd.clear();
for (int k = 0; k < 4; k++) { // clear LCD
for (int j = 0; j < 16; j++) {
lcd.setCursor(j, k);
lcd.print(" ");
}
}
switch (menu) {
case 0:
lcd.setCursor(0, 0);
lcd.print("Select Servo (");
lcd.print(servo);
lcd.print(") ");
lcd.setCursor(0, 1);
lcd.print("Set Servo Angles");
lcd.setCursor(0, 2);
lcd.print("Exit");
break;
case 1:
lcd.setCursor(3, 0);
lcd.print("Select Servo");
lcd.setCursor(0, 1);
lcd.print("Required Servo - ");
lcd.print(servo);
break;
case 2:
lcd.setCursor(3, 0);
lcd.print("Adjust Servo ");
lcd.print(servo);
lcd.setCursor(0, 1);
lcd.print("Straight Angle ");
lcd.print(servoQueue[servo].straightAngle);
lcd.setCursor(0, 2);
lcd.print("Divert Angle ");
lcd.print(servoQueue[servo].divergeAngle);
lcd.setCursor(0, 3);
lcd.print(" Long press to Exit");
break;
case 3:
lcd.setCursor(2, 0);
lcd.print("Save new angles? ");
lcd.setCursor(10, 1);
lcd.print("No ");
lcd.setCursor(10, 2);
lcd.print("Yes ");
break;
}
}
///////////////////////////////////////////////////////////////////////////////
//
int8_t readEncSwitch() {
// switch debounce values
static uint8_t nDebounce = 150; // 50;
static uint32_t lTimeOfChange = 0; // Time when button state changed
static int8_t lastStatus = 0;
// int encSwitch;
if ( (millis() - lTimeOfChange) >= nDebounce ) {
int encSwitch = !digitalRead(ENC_SWITCH); // Read the Encoder switch (low = pressed so invert)
if ( encSwitch != lastStatus ) {
lastStatus = encSwitch;
lTimeOfChange = millis();
}
}
return lastStatus;
}
///////////////////////////////////////////////////////////////////////////////
//
void encoderPinA() {
byte reading;
cli(); // stop interrupts happening before we read pin values
reading = PIND & 0xC; // read all eight pin values then strip away all but pinA and pinB's values
if (reading == B00001100 && aFlag) { // check that we have both pins at detent (HIGH) and that we are expecting
// detent on this pin's rising edge
encoderValue -= 1; // decrement the encoder's position count
bFlag = 0; // reset flags for the next turn
aFlag = 0; // reset flags for the next turn
}
else if (reading == B00000100) bFlag = 1; // signal that we're expecting pinB to signal the transition to
// detent from free rotation
sei(); //restart interrupts
}
///////////////////////////////////////////////////////////////////////////////
//
void encoderPinB() {
byte reading;
cli(); // stop interrupts happening before we read pin values
reading = PIND & 0xC; // read all eight pin values then strip away all but pinA and pinB's values
if (reading == B00001100 && bFlag) { // check that we have both pins at detent (HIGH) and that we are expecting
// detent on this pin's rising edge
encoderValue += 1; // increment the encoder's position count
bFlag = 0; // reset flags for the next turn
aFlag = 0; // reset flags for the next turn
}
else if (reading == B00001000) aFlag = 1; // signal that we're expecting pinA to signal the transition to
// detent from free rotation
sei(); //restart interrupts
}
