Hi,
The following code should read the position of steppers from EEPROM, move them to the zero position, make a half turn then stop the motors. In case of an AC fail (opto coupler), it saves the "nowPos" of the steppers on EEPROM for the next boot up.
But if I set a target position of let's say 1024, simulate an AC fail at let's say at 500 and restart the motors go to zero, back to 500 then to 1024 position instead of going to zero and then to 1024.
Where is my mistake ?
/*
Nano | TPIC6B595
5V | 2 (VCC)
GND | GND
D8 | 8 (SRCLR) - blue
D10 | 12 (RCK) - green
D11 | 3 (SER IN) of the first chip - yellow
D13 | 13 (SRCK) - orange
| 9 to GND
| 10, 11,19 to GND
| 18 (SER OUT) to 3 of the next chip
*/
#include <SPI.h>
#include <EEPROM.h>
const byte motors = 96; // sets of four motors
const byte optoPin = 2; // interrupt pin
const byte clrPin = 8; // TPIC6B595 SRCLR pin8
const byte latchPin = 10; // TPIC6B595 RCK pin12
byte rampSteps = 7; // accel/decel steps
byte rampDelay[motors]; // accel/decel delay between steps
byte lag[motors]; // speed reduction
int velocity[motors]; // signed, for direction
int nowPos[motors], newPos[motors]; // int is 16 rotations max
unsigned long prevMillis, prevMicros, interval, startMicros; // timing
byte val[4]; // TPIC write bytes
bool torque = true; // true is full power, false is half power
bool brownOut; // mains failure detected flag
byte sequence, index; // patterns, motor index
const unsigned int startAddress = 0; // option to change for wear leveling
int targetPos = 1024;
const byte Start = 0;
const byte Go_To_Zero = 1;
const byte Go_To_Target = 2;
const byte Idle = 3;
const byte Write_EEPROM = 255;
void setup() {
Serial.begin(115200);
Serial.println("Testing");
SPI.begin();
pinMode (optoPin, INPUT_PULLUP);
pinMode(clrPin, OUTPUT);
pinMode(latchPin, OUTPUT);
digitalWrite(clrPin, HIGH); // clear all shift register data
attachInterrupt(digitalPinToInterrupt(optoPin), trigger, RISING);
EEPROM.get(startAddress, nowPos); // load the saved positions
}
void loop() {
switch (sequence) {
case Start:
for (byte i = 0; i < motors; i++) {
if (nowPos[i] != newPos[i]) break;}
sequence = Go_To_Zero; // case 0 is not called anymore
prevMillis = millis(); // mark start of sequence 1
break;
case Go_To_Zero:
if (millis() - prevMillis > 5000) {
for (byte i = 0; i < motors; i++) newPos[i] = nowPos[i]* -1;
prevMillis = millis();
sequence = Go_To_Target;
}
break;
case Go_To_Target:
for (byte n = 0; n < motors; n++){
nowPos[n] = 0;}
if (millis() - prevMillis > 5000) {
for (byte i = 0; i < motors; i++) newPos[i] = targetPos;
prevMillis = millis();
sequence = Idle;
}
break;
case Idle:
sequence = 22; // no such sequence
break;
case Write_EEPROM: // eeprom write
Serial.println("AC_FAIL");
for (byte i = 0; i < motors; i++) {
if (nowPos[i] != newPos[i]) break;
EEPROM.put(startAddress, nowPos);
while (1); // wait for a reset
}
}
goPos();
}
void trigger() { // ISR
startMicros = micros(); // mark the start of a zero crossing
}
void goPos() {
while (micros() - prevMicros < 1953); // step interval, motor RPM limitation, 1/(15rpm/60sec*2048steps)= ~1953ms, wait if arrived here too early
prevMicros = micros(); // update, for next interval
if (!brownOut && digitalRead(optoPin) && micros() - startMicros > 1200) { // no AC outside the zero crossing
for (byte i = 0; i < motors; i++) {
newPos[i] = nowPos[i]; // stay were you are, or get there after overshoot
lag[i] = 0; // at full speed
}
torque = false; // low power deceleration, to maximise hold-up time
// rampSteps = 3; // shorten deceleration time
sequence = 255; // goto an unused case that manages the eeprom writes
brownOut = true; // no way back, but keep on decelerating
}
for (int i = 0; i < motors; i++) {
if (rampDelay[i]) rampDelay[i]--; // subtract one, do nothing
else { // step
if (newPos[i] > nowPos[i]) { // request forwards
if (velocity[i] >= 0) { // if standing still, or going forwards
nowPos[i]++; // one step forward
if (newPos[i] - nowPos[i] < rampSteps) velocity[i]--; // reduce speed if almost there
else if (velocity[i] < min(rampSteps, rampSteps - lag[i])) velocity[i]++; // increase speed if not there yet
rampDelay[i] = max(lag[i], rampSteps - velocity[i]); // insert ramp delay
} else { // if wrong direction
velocity[i]++; // reduce reverse speed
if (velocity[i] < 0) nowPos[i]--; // if still reverse, keep on going the wrong way
rampDelay[i] = rampSteps - abs(velocity[i]); // insert ramp delay
}
}
if (newPos[i] < nowPos[i]) { // request reverse
if (velocity[i] <= 0) { // if standing still, or going reverse
nowPos[i]--; // one step reverse
if (nowPos[i] - newPos[i] < rampSteps) velocity[i]++; // reduce speed if almost there
else if (abs(velocity[i]) < min(rampSteps, rampSteps - lag[i])) velocity[i]--; // increase speed if not there yet
rampDelay[i] = max(lag[i], rampSteps + velocity[i]); // insert ramp delay
} else { // wrong direction
velocity[i]--; // reduce forward speed
if (velocity[i] > 0) nowPos[i]++; // if still reverse, keep on going the wrong way
rampDelay[i] = rampSteps - velocity[i]; // insert ramp delay
}
}
}
if (newPos[i] == nowPos[i])val[i & 3] = 0; // cut coil power when there
else {
if (torque) { // this block for full torque
switch (nowPos[i] & 3) { // the two LSB translated to motor coils (full step)
case 0: val[i & 3] = B00000011; break; // two coils at the time
case 1: val[i & 3] = B00000110; break;
case 2: val[i & 3] = B00001100; break;
case 3: val[i & 3] = B00001001; break;
}
} else { // this block for low power
switch (nowPos[i] & 3) {
case 0: val[i & 3] = B00000001; break; // one coil at the time
case 1: val[i & 3] = B00000010; break;
case 2: val[i & 3] = B00000100; break;
case 3: val[i & 3] = B00001000; break;
}
}
}
if ((i & 3) == 3) { // process set of four motors (faster)
unsigned int chain = val[0] << 12 | val[1] << 8 | val[2] << 4 | val[3]; // concat
SPI.transfer16(chain); // transfer 4-motor int
}
}
digitalWrite(latchPin, HIGH); // transfer to TPIC driver outputs
digitalWrite(latchPin, LOW); // latch time ~8us
}