Arduino implementation of AlfaSpid ROT2PROG rotator controller

rotator.ino

/*  Rotator controller code for Arduino
 *  ========================================================
 *
 *  This is a very poor attempt at reverse-engineering
 *  the ROT2PROG rotator controller and its SPID protocol
 *  using a TI DRV8432 motor driver and an Arduino Micro.
 *
 *  For information about the TI DRV8432, see
 *      http://www.ti.com/product/drv8432
 *  For information on the SPID protocol, see
 *      http://ryeng.name/blog/3
 *
 *  Important notes
 *  -----------------
 *  - PH and PV are fixed to 1. They cannot be changed.
 *  - Software limit switches are set at compile-time.
 *  - Position data is saved every STATUS command.
 *
 *
 *  Copyright (c) 2015, Calvin Li
 *
 *  Redistribution and use in source and binary forms, with
 *  or without modification, are permitted provided that the
 *  following conditions are met:
 *
 *  1. Redistributions of source code must retain the above
 *     copyright notice, this list of conditions and the
 *     following disclaimer.
 *  2. Redistributions in binary form must reproduce the
 *     above copyright notice, this list of conditions and
 *     the following disclaimer in the documentation and/or
 *     other materials provided with the distribution.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
 *  CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED
 *  WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 *  WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
 *  PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY
 *  DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 *  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 *  PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
 *  USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 *  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 *  NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE
 *  USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
 *  OF SUCH DAMAGE.
 */

/* PWM pins. (See DRV8432 documentation for explanation.)
 *
 * NB: Arduino documentation notes that 5 & 6 may not be
 * accurate at very low duty cycles (which we use!).
 */
#define PWM_EL_F 9
#define PWM_EL_R 10
#define PWM_AZ_F 11
#define PWM_AZ_R 13

/* PWM low must be > 1 for '8432 to function properly.
 *
 * Note that the rotator *really* does not like being
 * PWM'd, so try to not use any intermediate values. */
#define PWM_LO 2
#define PWM_HI 253

/* Sense pins. These must be on interrupt pins.
 * If these are changed, the interrupt numbers
 * in the code below must be changed too. */
#define SENSE_EL 7 /* int.4 */
#define SENSE_AZ 2 /* int.1 */

/* The ROT2PROG has a safety timeout that kills the motor
 * if the sense line doesn't register a pulse for a certain
 * amount of time. I don't know what the timeout is
 * supposed to be, but the slowest the rotator should ever
 * turn is 3°/s. (Ideal speed is 6°/s.) */
#define MOTOR_TIMEOUT_MS 400

/* Software limit switch boundaries
 * Remember there is a +360° offset */
#define AZ_MIN 0    /* relatively unconstrained */
#define AZ_MAX 720
#define EL_MIN 360  /* 0° to +90° strictly */
#define EL_MAX 450

/* Position data persistence */
#include <EEPROM.h>
#define AZ_ADDR 0
#define EL_ADDR 2
#define EEPROM_MAGIC_ADDR 4
#define EEPROM_MAGIC_VAL 0x19


/* Forward declarations */
void brake_az();
void brake_el();
void forward_el();
void forward_az();
void reverse_el();
void reverse_az();
void check_motors();
void az_pulse_monitor();
void el_pulse_monitor();


/*
 * Global variables
 */
char serial_char_in;
char command[14];
char response[12];

/* These are strictly positive (+360° offset) */
volatile unsigned int AZ_POS;
volatile unsigned int EL_POS;

unsigned int AZ_MOVE_TO;
unsigned int EL_MOVE_TO;

volatile unsigned long AZ_LAST_MOVED = 0;
volatile unsigned long EL_LAST_MOVED = 0;

boolean AZ_FORWARD = true;
boolean EL_FORWARD = true;


void setup() {
  interrupts();

  pinMode(SENSE_EL, INPUT_PULLUP);
  pinMode(SENSE_AZ, INPUT_PULLUP);
  /* PWM pins require no setup */
  brake_az(); brake_el();

  /* Read position data from EEPROM, if it's there */
  if (EEPROM.read(EEPROM_MAGIC_ADDR) == EEPROM_MAGIC_VAL) {
    EEPROM.get(AZ_ADDR, AZ_POS);
    EEPROM.get(EL_ADDR, EL_POS);
  } else {
    AZ_POS = 360; /* = 0° IRL */
    EL_POS = 360;
  }
  AZ_MOVE_TO = AZ_POS;
  EL_MOVE_TO = EL_POS;

  /* ROT2PROG is 600 baud, because
     that makes sooo much sense... */
  Serial.begin(600);
}

void loop() {
  check_motors();

  /* Read in SPID command */
  if(Serial.readBytes(command, 14) == 0) {
    // no serial command found
    // do ~not~ send output (obv)
    return;
  }

  /* start byte is always 0x57 and end byte is 0x20
     if this is not the case, throw out the command */
  if (command[0] != 0x57 || command[13] != 0x20) {
    /* and clear the serial buffer while we're at it,
       since it's probably filled with bad data too */
    while(Serial.available()) {
      Serial.read();
    }
    return;
  }

  /* Check for STOP command
   *
   * STOP command returns the position stopped at */
  if (command[11] == 0x0F) {
    brake_az(); brake_el();
  }

  /* Check for STATUS command
   * These can come at any time,
   * even while the rotator is moving.
   * They are expected to always give the current
   * instantaneous position of the rotator.
   */
  if (command[11] == 0x1F) {
    /* nothing needs to be done here, actually;
       but this is a good time to update EEPROM */
    EEPROM.put(0, AZ_POS);
    EEPROM.put(2, EL_POS);
    EEPROM.update(EEPROM_MAGIC_ADDR, EEPROM_MAGIC_VAL);
  }

  /* Check for SET command
   * There is no response to SET commands.
   * SET commands *do not* block.
   */
  if (command[11] == 0x2F) {
    /* ignore H1/V1... shouldn't ever be necessary
       since PH/PV are hardcoded to 1 */
    AZ_MOVE_TO = (command[2] - 0x30) * 100 +
                 (command[3] - 0x30) * 10  +
                 (command[4] - 0x30);

    EL_MOVE_TO = (command[7] - 0x30) * 100 +
                 (command[8] - 0x30) * 10  +
                 (command[9] - 0x30);

    /* software limit switch
       ensure positions are kept within range */
    if (AZ_MOVE_TO < AZ_MIN) {
      AZ_MOVE_TO = AZ_MIN;
    } else if (AZ_MOVE_TO > AZ_MAX) {
      AZ_MOVE_TO = AZ_MAX;
    }
    if (EL_MOVE_TO < EL_MIN) {
      EL_MOVE_TO = EL_MIN;
    } else if (EL_MOVE_TO > EL_MAX) {
      EL_MOVE_TO = EL_MAX;
    }

    AZ_FORWARD = (AZ_MOVE_TO > AZ_POS);
    EL_FORWARD = (EL_MOVE_TO > EL_POS);

    /* start moving */
    if (AZ_FORWARD) {
      forward_az();
    } else if (AZ_POS != AZ_MOVE_TO) {
      reverse_az();
    }
    if (EL_FORWARD) {
      forward_el();
    } else if (EL_POS != EL_MOVE_TO) {
      reverse_el();
    }

    /* These ISRs will stop the motors when
     * they reach their desired positions.
     *
     * We trigger on rising edge because the
     * sense line is active-low, so the ISR
     * will run only ~after~ the pulse is complete.
     * Note: the original ROT2PROG does *not* do this.
     */
    attachInterrupt(1, az_pulse_monitor, RISING);
    attachInterrupt(4, el_pulse_monitor, RISING);

    /* SET command does not expect a response */
    return;
  }

  /* Write out SPID output */
  response[0]  = 0x57;
  response[1]  = AZ_POS / 100;
  response[2]  = (AZ_POS % 100) / 10;
  response[3]  = AZ_POS % 10;
  response[4]  = 0x00;
  response[5]  = 0x01;
  response[6]  = EL_POS / 100;
  response[7]  = (EL_POS % 100) / 10;
  response[8]  = EL_POS % 10;
  response[9]  = 0x00;
  response[10] = 0x01;
  response[11] = 0x20;

  Serial.write((uint8_t*)response, 12);
}

void brake_el() {
  analogWrite(PWM_EL_F, PWM_LO);
  analogWrite(PWM_EL_R, PWM_LO);
}
void brake_az() {
  analogWrite(PWM_AZ_F, PWM_LO);
  analogWrite(PWM_AZ_R, PWM_LO);
}
void forward_el() {
  EL_LAST_MOVED = millis(); /* start counter */
  analogWrite(PWM_EL_R, PWM_LO);
  analogWrite(PWM_EL_F, PWM_HI);
}
void forward_az() {
  AZ_LAST_MOVED = millis();
  analogWrite(PWM_AZ_R, PWM_LO);
  analogWrite(PWM_AZ_F, PWM_HI);
}
void reverse_el() {
  EL_LAST_MOVED = millis();
  analogWrite(PWM_EL_F, PWM_LO);
  analogWrite(PWM_EL_R, PWM_HI);
}
void reverse_az() {
  AZ_LAST_MOVED = millis();
  analogWrite(PWM_AZ_F, PWM_LO);
  analogWrite(PWM_AZ_R, PWM_HI);
}

/* Check that motors aren't stalled / at limit switch. */
void check_motors() {
  if (AZ_LAST_MOVED > 0) {
    if (AZ_LAST_MOVED < millis() - MOTOR_TIMEOUT_MS) {
      brake_az();
      detachInterrupt(1);
    }
  }
  if (EL_LAST_MOVED > 0) {
    if (EL_LAST_MOVED < millis() - MOTOR_TIMEOUT_MS) {
      brake_el();
      detachInterrupt(4);
    }
  }
}

/*  ISR for azimuth control. */
void az_pulse_monitor() {
  AZ_LAST_MOVED = millis();

  if (AZ_FORWARD) {
    AZ_POS++;
  } else {
    AZ_POS--;
  }

  if (AZ_POS == AZ_MOVE_TO) {
    brake_az();
    detachInterrupt(1);
  }
}

/*  ISR for elevation control. */
void el_pulse_monitor() {
  EL_LAST_MOVED = millis();

  if (EL_FORWARD) {
    EL_POS++;
  } else {
    EL_POS--;
  }

  if (EL_POS == EL_MOVE_TO) {
    brake_el();
    detachInterrupt(4);
  }
}