Code for AS5600

ATtiny85/ATtiny85.ino

@@ -0,0 +1,235 @@
+/*
+ * Final setting:
+ * 0 - motor PWM1 / RX during setup
+ * 1 - motor PWM2 / TX during setup
+ * 2 - step input
+ * 3 - encoder A
+ * 4 - encoder B
+ * 5 - direction
+ * 
+ *  To fix fuses after final program is uploaded:
+ * avrdude -v -v -v -v -pattiny85 -cusbasp -Pusb -U lfuse:w:0xc1:m -U hfuse:w:0x5c:m -U efuse:w:0xff:m
+ *
+ * For Tiny Debug Serial use boards definition from
+ * https://storage.googleapis.com/google-code-archive-downloads/v2/code.google.com/arduino-tiny/arduino-tiny-0150-0020.zip
+ *
+ * Should you need to get back to programming (like adjust motor parameters),
+ * use HVSP programming - I used Arduino Nano with sketch from here:
+ * https://arduinodiy.wordpress.com/2015/05/16/high-voltage-programmingunbricking-for-attiny/
+*/
+#include "avr/interrupt.h"
+#include <PID_v1.h>
+
+volatile long encoder0Pos = 0;  // real position from encoder
+volatile long target1 = 0;      // destination location at any moment
+volatile int moveCoef = 1;
+static byte newPort;
+
+#define M1            0  // only PWM capable pins on Tiny is 0 and 1
+#define M2            1
+
+// please adjust these 3 values for your motor!!!
+// Y axis config: 
+// const double kp = 12, ki = 8, kd = 0.02;
+// X axis config:
+const double kp = 12, ki = 4, kd = 0.02;
+// Z axis config
+// const double kp = 12, ki = 8, kd = 0.02;
+#undef stepKoef
+// #define stepKoef 5
+//#undef minSearch
+#define minSearch
+
+double input = 0, output = 0, setpoint = 0;
+PID myPID(&input, &output, &setpoint, kp, ki, kd, DIRECT);
+
+boolean autoReport = true;
+unsigned long previousMillis = 0; // will store last time LED was updated
+long lastEncPos = 0;              // will store the encoded value when last max output was measured
+unsigned long curTime = 0UL;      // will store current time to avoid multiple millis() calls
+unsigned long lastSafeCheck = 0;  // will store last value when 255 output was measured
+unsigned long motorSafe = 4000UL; // will store the interval to protect the motor - 4seconds
+int lastMax = 0;                  // have to also store the max to avoid +-255 values
+
+
+// when using ATtiny85 on 16MHz, serial pin is 2, not 3!!!
+
+void setup() {
+  // real code
+  pinMode(2, INPUT); // used for step interrupt
+  pinMode(5, INPUT); // used for step direction
+  // Setting the PWM pins as output
+  pinMode(M1, OUTPUT);
+  pinMode(M2, OUTPUT);
+  // Set the PWM frequencies
+  TCCR0A = TCCR0A & 0b11111000 | 0x01;
+  TCCR0B = TCCR0B & 0b11111000 | 0x01;
+
+  // interrupts
+  GIMSK = 0b00100000;    // turns on pin change interrupts
+  PCMSK = 0b00011100;    // turn on interrupts on pins PB3 and PB4
+  sei();
+
+  getMinimal();
+
+  myPID.SetMode(AUTOMATIC);
+  myPID.SetSampleTime(1);
+  myPID.SetControllerDirection((moveCoef > 0)?DIRECT:REVERSE);
+  myPID.SetOutputLimits(-255, 255);
+}
+
+void loop() {
+  curTime = millis();
+  input = encoder0Pos;
+  setpoint = target1;
+  myPID.Compute();
+
+  pwmOut(output);
+
+  /* Motor safe code */
+  if (abs(output) == 255) {
+    if (lastSafeCheck == 0) {
+      lastSafeCheck = curTime;
+      lastEncPos = encoder0Pos;
+    } else {
+      if (lastEncPos != encoder0Pos) {
+        lastSafeCheck = 0;
+      } else {
+        if (curTime - motorSafe > lastSafeCheck) {
+          // we have to protect the motor - looks like even with max output we are not moving
+          // we will set the target1 to current position to stop output
+          target1 = encoder0Pos - (output / 255 * 5);
+          lastEncPos = 0;
+          lastSafeCheck = 0;
+        }
+      }
+    }
+  } else {
+    lastSafeCheck = 0;
+    lastEncPos = 0;
+  }
+}
+
+
+// interrupt handling - have to select which triggered!!!
+const int QEM [16] = {0, -1, 1, 2, 1, 0, 2, -1, -1, 2, 0, 1, 2, 1, -1, 0}; // Quadrature Encoder Matrix
+
+ISR(PCINT0_vect)
+{
+  byte oldInput, newInput, oldPort;
+  // reading port status and storing it
+  oldPort = newPort;
+  newPort = PINB;
+
+  // determining if encoder position changed
+  newInput = (newPort & 0b00011000) >> 3;
+  oldInput = (oldPort & 0b00011000) >> 3;
+  if (newInput != oldInput) {
+    // we have change in Encoder position!
+    encoder0Pos += QEM[oldInput * 4 + newInput];
+  }
+
+  // determining if step input occured
+  // let's check only the step pin,
+  // we'll care about direction later
+  newInput = newPort & 0b0100;
+  oldInput = oldPort & 0b0100;
+
+  // we have to have change comparing to previous state
+  // and we need raising change on input value for step
+  // here I do 10 steps per impusle!!!
+  if ((newInput != 0) && (oldInput == 0)) {
+    // we have step!
+    #ifndef stepKoef
+    target1 += (newPort & 0b100000) ? -1 : 1;
+    #else
+    target1 += (newPort & 0b100000) ? -1 * stepKoef : stepKoef;
+    #endif
+  }
+}
+
+void pwmOut(int out) {
+  if (out < 0) {
+    analogWrite(M2, 255);
+    analogWrite(M1, 255 - abs(out));
+  }
+  else {
+    analogWrite(M2, 255 - abs(out));
+    analogWrite(M1, 255);
+  }
+}
+
+
+void getMinimal() {
+
+  // goal is to:
+  // 1. measure minimal speed
+  // 2.a if we have no encoder input, perhaps we are at the end
+  //     of possible movement track and we need to reverse
+  // 2.b if we have no encoder input after reverse, we need to
+  //     stop everything - either the motor is disconnected
+  //     or the encoder is and whatever we send can cause damage
+  // 3. verify encoder direction - to ensure positive movement
+  //    of encoder input is also positive direction of motor
+  // 4. if we did not have a movement, the motor is disconnected
+  //    and we are probably in a setup mode - we need to turn on
+  //    the serial on respective pins
+
+  // first, let's find minimum moving speed
+  int minSpeed = minSpd(1);
+
+  // did we moved?
+  // if we did not move, we have to try the other way
+  // perhaps we were at the end of the path
+  // and we need to go back
+  if (encoder0Pos == 0) minSpeed = minSpd(-1);
+
+  // stop the motor
+  pwmOut(0);
+
+  // did we moved now?
+  // if we did not, we have to end - probably we need to turn on the serial
+  // port to debug, so we do it and quit the setup routine
+  if (encoder0Pos == 0) {
+    // Fatal error
+    // no input from encoder
+    // will halt
+    while (1) {
+      pwmOut(125);
+      delay(200);
+      pwmOut(-125);
+      delay(200);
+    }
+  }
+
+  // we moved, variable i has the minimum moving speed
+  // we need to verify if the encoder is aligned with
+  // the direction of the motor
+  moveCoef = (encoder0Pos / abs(encoder0Pos)) * (minSpeed / abs(minSpeed));
+
+  #ifdef minSearch
+  // now move to minimum position
+  minSpeed = abs(minSpeed) + (255 - abs(minSpeed)) / 3;
+  long encoderOld = 0;
+    pwmOut(-minSpeed * (moveCoef));
+    while (encoderOld != encoder0Pos) {
+      encoderOld = encoder0Pos;
+      delay(50);
+    }
+    pwmOut(0);
+    encoder0Pos = -75;
+    #ifdef stepKoef
+    encoder0Pos = -75 * stepKoef;
+    #endif
+  #endif
+}
+
+int minSpd(int dirSpd) {
+  int minSpeed = 0;
+  while (( abs(minSpeed) <= 255) && (abs(encoder0Pos) < 5)) {
+    pwmOut(minSpeed * dirSpd);
+    delay(10);
+    minSpeed += 5;
+  }
+  return (minSpeed * dirSpd);
+}

README.md

@@ -1,39 +1,28 @@
 # dcservo
-by misan
+For original documentation, please, visit misan/dcservo.
 
-This project uses and Arduino (or similar) to create a closed-loop position control for a DC motor to act 
-as a replacement of a stepper motor and its drive electronics. In order to be compatible with stepper logic
-controller accepts two inputs STEP and DIRECTION so an external trajectory controller can operate the motor
-as it would do with a stepper.
+#ATtiny85 version
+I added some modifications.
 
-This code depends on the excellent Arduino PID library by Brett Beauregard: https://github.com/br3ttb/Arduino-PID-Library/
+#1. Determining the direction of the motor
+As I was often modifying my design, I tend to plug in the motor with different polarity causing madness every time I tried to do first movement.
+Now, when the code starts, it will try to move the motor and find out in which direction does it move in correlation to the encoder input.
+If it finds no encoder inputs, it will consider either disconnected motor or disconnected encoder - for both cases the "error behavior" is simply to move the motor back-and-forth to signalize this problem.
 
-For easy configuration of PID parameters, serial port communication is used when adjusting them. PID values can
-be stored in EEPROM.
+#2. Determining minimum movement speed and minimal position
+As soon as I determine the motor direction, I proceed forward and try to find minimum position of the current axis. What I do is I move the motor in reverse direction with a small multiplier of minimal movement speed until I reach a point, where the encoder no longer receives input - this means I've hit physical end of the path.
+At that moment I will reset the position to -75 (and not to zero) - this will allow to set the "real life" zero slightly above the minimal position of the axis to avoid loud bang everytime user wants to move to 0 position. The -75 is hardcoded for now and works OK for my current usage, but feel free to change it either to 0 or any other number you find OK for your printer.
 
-Servostrap project and Michael Ball's work sparked my curiosity to go create a simple solution for anyone to use.
+#3. Motor protection
+I am monitoring the encoder input within regular time interval. If there is a situation where I am sending full power to the motors and the encoder position did not change for more then the predefined interval, I am trying to stop the motion. This will prevent situations whete the motor hits some problem (e.g. maximum axis position, huge obstacle, ...) and is unable to move, while the firmware still feeds full power - the motor would overheat and/or cause other significant damages.
 
-I want to thank Brook Drum for his support on this project that started in here: https://www.youmagine.com/designs/dc-motor-closed-loop-control-software
+#4. Removal of serial debug
+Due to lack of pins on ATtiny85 and lack of storage, too, it made very little sense to keep the serial communication code and EEPROM code. Therefore I removed it and hardcoded the values for PID. You can use original code from Misan and his PID tuning tool to tune your motor and then use the final values within this sketch.
 
-Thanks to Mauro Manco for providing a NodeMCU. Now there is a version than can work with a ESP12E module, with support to accept commands over Wifi. Please note EEPROM storage code will not work in this platform :-(
+#5. Converting values to LONG
+In some cases the 32767 values of encoder position and target step may not be enough. This is especially the case with geared motors and high resolution encoders. Therefore I moved the code to LONG, providing enough room for all situations.
 
-AMS provided free samples of magnetic encoder. Now the AS6500 encoder is supported for 12-bit/revolution resolution. 
-
-# usage
-<pre>
-Available serial commands: (lines end with CRLF or LF) 
-P123.34 sets proportional term to 123.34
-I123.34 sets integral term to 123.34
-D123.34 sets derivative term to 123.34
-? prints out current encoder, output and setpoint values
-X123 sets the target destination for the motor to 123 encoder pulses
-T will start a sequence of random destinations (between 0 and 2000) every 3 seconds. T again will disable that
-Q will print out the current values of P, I and D parameters
-W will store current values of P, I and D parameters into EEPROM
-H will print this help message again
-A will toggle on/off showing regulator status every second
-</pre>
-
-#tuning tool
-Written in Processing it allows you to see graphically the PID response to a step input while you can tune it by pressing capital P, I and D keys to increase values or p, i and d to lower them.
-![Screenshot](http://i.imgur.com/3c8WySu.png "Tuning tool")
+#6. ATtiny85 specifics
+You have to use the avrdude code to fix the reset pin - otherwise your driver will have a reset situation whenever there is a step input.
+To restore the original status - e.g. when reprogramming is needed, I provided a link to HVSP fuse reset project done by Arduino, 1 NPN transistor and 6 resistors.
+Also, note you SHOULD NOT use the attinycore as the core base for the tiny - for some reason (I did not investigate for now) it does not work and compiled code will not execute the way it should. Use this URL to get the needed tiny core into Arduino IDE: http://highlowtech.org/?p=1695