I bought some time ago an Adafruit Motor shield to use in some projects, I have to say I had no problem at all untill now.
Im building a Camera Motion control for a friend, using two stepper motor, the hardware is properly built & tested and have been working ok with our simple sketches, but we decided to adapt the system so it can run with DRAGONFRAME software http://www.dragonframe.com/ which allows for an incredible accurate motion control.
http://www.dragonframe.com/moco-3d.php#arduino
little explanation about Dragonframe:
This software is used for motion control, time lapses and other VIDEO shooting process, being able to control the movement of up to 8 axis trough an arduino. The software also recognize digital cameras allow you to control, focus, exposure, shuter speed etc. from the computer.
For this to work they use their own sketch called "DFMoco" and suggest you to use a stepper driver like "easydriver"... The code just allows for step and direction calls, and I dont know if this is possible at all using the adafruit motorshield.
Ive been looking everywhere but cannot finad any information in how to solve this (apart from buying 2 easydriver boards).
Thanks in advance!
Julian
PD: DFMoco Sketch
Code: Select all
#define DFMOCO_VERSION 1
#define DFMOCO_VERSION_STRING "1.0.2"
/*
DFMoco version 1.0.2
Multi-axis motion control.
For use with the Arc motion control system in Dragonframe 3.
Generates step and direction signals, which can be sent to stepper motor drivers.
Control up to four axes with an Uno or Duemilanove board.
Control up to eight axes with a Mega or Mega 2560 board.
Version History
Version 1.0.2 Moved pulses into interrupt handler
Version 1.0.1 Added delay for pulse widths
Version 1.0.0 Initial public release.
Pin configuration:
channel 1
PIN 4 step
PIN 5 direction
channel 2
PIN 6 step
PIN 7 direction
channel 3
PIN 8 step
PIN 9 direction
channel 4
PIN 10 step
PIN 11 direction
channel 5
PIN 22 step
PIN 23 direction
channel 6
PIN 24 step
PIN 25 direction
channel 7
PIN 26 step
PIN 27 direction
channel 8
PIN 28 step
PIN 29 direction
*/
// output the pre-calculated move data
#define DEBUG_MOVE 0
// if you do not want a kill switch, comment out this line
#define KILL_SWITCH_INTERRUPT 0
// define maple board
#define ARDUINO 1
//#define MAPLE 1
#if defined(MAPLE)
#define SERIAL_DEVICE SerialUSB
#else
#define SERIAL_DEVICE Serial
#endif
// Arduino Uno/Duemilanove -> 4 MOTORS MAX
// Arduino Mega 2560 / Mega -> 8 MOTORS MAX
#if defined(MAPLE) || defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
#define MOTOR_COUNT 8
#else
#define MOTOR_COUNT 4
#endif
#ifdef MAPLE
#define TIME_CHUNK 10
#define SEND_POSITION_COUNT 100000
#else
#define TIME_CHUNK 25
#define SEND_POSITION_COUNT 40000
#endif
// setup step and direction pins
#if defined(MAPLE)
#define MOTOR0_STEP_PORT GPIOB
#define MOTOR0_STEP_PIN 5
#define MOTOR1_STEP_PORT GPIOB
#define MOTOR1_STEP_PIN 6
#define MOTOR2_STEP_PORT GPIOA
#define MOTOR2_STEP_PIN 8
#define MOTOR3_STEP_PORT GPIOA
#define MOTOR3_STEP_PIN 9
#define MOTOR4_STEP_PORT GPIOC
#define MOTOR4_STEP_PIN 14
#define MOTOR5_STEP_PORT GPIOC
#define MOTOR5_STEP_PIN 15
#define MOTOR6_STEP_PORT GPIOC
#define MOTOR6_STEP_PIN 6
#define MOTOR7_STEP_PORT GPIOC
#define MOTOR7_STEP_PIN 7
#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
#define MOTOR0_STEP_PORT PORTG
#define MOTOR0_STEP_PIN B00100000
#define MOTOR1_STEP_PORT PORTH
#define MOTOR1_STEP_PIN B00001000
#define MOTOR2_STEP_PORT PORTH
#define MOTOR2_STEP_PIN B00100000
#define MOTOR3_STEP_PORT PORTB
#define MOTOR3_STEP_PIN B00010000
#define MOTOR4_STEP_PORT PORTA
#define MOTOR4_STEP_PIN B00000001
#define MOTOR5_STEP_PORT PORTA
#define MOTOR5_STEP_PIN B00000100
#define MOTOR6_STEP_PORT PORTA
#define MOTOR6_STEP_PIN B00010000
#define MOTOR7_STEP_PORT PORTA
#define MOTOR7_STEP_PIN B01000000
#else
#define MOTOR0_STEP_PORT PORTD
#define MOTOR0_STEP_PIN B00010000
#define MOTOR1_STEP_PORT PORTD
#define MOTOR1_STEP_PIN B01000000
#define MOTOR2_STEP_PORT PORTB
#define MOTOR2_STEP_PIN B00000001
#define MOTOR3_STEP_PORT PORTB
#define MOTOR3_STEP_PIN B00000100
#endif
/**
* Serial output specialization
*/
#if defined(UBRRH)
#define TX_UCSRA UCSRA
#define TX_UDRE UDRE
#define TX_UDR UDR
#else
#define TX_UCSRA UCSR0A
#define TX_UDRE UDRE0
#define TX_UDR UDR0
#endif
char txBuf[32];
char *txBufPtr;
#define TX_MSG_BUF_SIZE 16
#define MSG_HI 01
#define MSG_MM 02
#define MSG_MP 03
#define MSG_MS 04
#define MSG_PR 05
#define MSG_SM 06
#define MSG_SA 07
struct txMsg
{
byte msg;
byte motor;
};
struct TxMsgBuffer
{
txMsg buffer[TX_MSG_BUF_SIZE];
byte head;
byte tail;
};
volatile TxMsgBuffer txMsgBuffer;
struct MoveElement
{
unsigned long steps;
byte rate;
};
/*
Motor data.
*/
long motorPos0;
long motorPos1;
long motorPos2;
long motorPos3;
#if MOTOR_COUNT > 4
long motorPos4;
long motorPos5;
long motorPos6;
long motorPos7;
#endif
long* motorPos[] =
{
&motorPos0, &motorPos1, &motorPos2, &motorPos3,
#if MOTOR_COUNT > 4
&motorPos4, &motorPos5, &motorPos6, &motorPos7
#endif
};
long motorMoveDestination0;
long motorMoveDestination1;
long motorMoveDestination2;
long motorMoveDestination3;
long motorMoveDestination4;
#if MOTOR_COUNT > 4
long motorMoveDestination5;
long motorMoveDestination6;
long motorMoveDestination7;
#endif
long* motorMoveDestination[] =
{
&motorMoveDestination0, &motorMoveDestination1, &motorMoveDestination2, &motorMoveDestination3,
#if MOTOR_COUNT > 4
&motorMoveDestination4, &motorMoveDestination5, &motorMoveDestination6, &motorMoveDestination7
#endif
};
byte motorNextStep0;
byte motorNextStep1;
byte motorNextStep2;
byte motorNextStep3;
#if MOTOR_COUNT > 4
byte motorNextStep4;
byte motorNextStep5;
byte motorNextStep6;
byte motorNextStep7;
#endif
byte* motorNextStep[MOTOR_COUNT]=
{
&motorNextStep0, &motorNextStep1, &motorNextStep2, &motorNextStep3,
#if MOTOR_COUNT > 4
&motorNextStep4, &motorNextStep5, &motorNextStep6, &motorNextStep7
#endif
};
signed char motorDirection0;
signed char motorDirection1;
signed char motorDirection2;
signed char motorDirection3;
#if MOTOR_COUNT > 4
signed char motorDirection4;
signed char motorDirection5;
signed char motorDirection6;
signed char motorDirection7;
#endif
signed char* motorDirection[MOTOR_COUNT] =
{
&motorDirection0, &motorDirection1, &motorDirection2, &motorDirection3,
#if MOTOR_COUNT > 4
&motorDirection4, &motorDirection5, &motorDirection6, &motorDirection7
#endif
};
unsigned long motorMoveSteps0;
unsigned long motorMoveSteps1;
unsigned long motorMoveSteps2;
unsigned long motorMoveSteps3;
#if MOTOR_COUNT > 4
unsigned long motorMoveSteps4;
unsigned long motorMoveSteps5;
unsigned long motorMoveSteps6;
unsigned long motorMoveSteps7;
#endif
unsigned long* motorMoveSteps[MOTOR_COUNT] =
{
&motorMoveSteps0, &motorMoveSteps1, &motorMoveSteps2, &motorMoveSteps3,
#if MOTOR_COUNT > 4
&motorMoveSteps4, &motorMoveSteps5, &motorMoveSteps6, &motorMoveSteps7
#endif
};
byte motorMoveIndex[MOTOR_COUNT];
byte motorMoveRate0;
byte motorMoveRate1;
byte motorMoveRate2;
byte motorMoveRate3;
#if MOTOR_COUNT > 4
byte motorMoveRate4;
byte motorMoveRate5;
byte motorMoveRate6;
byte motorMoveRate7;
#endif
byte* motorMoveRate[MOTOR_COUNT] =
{
&motorMoveRate0, &motorMoveRate1, &motorMoveRate2, &motorMoveRate3,
#if MOTOR_COUNT > 4
&motorMoveRate4, &motorMoveRate5, &motorMoveRate6, &motorMoveRate7
#endif
};
byte motorStepPin[MOTOR_COUNT];
byte motorDirPin[MOTOR_COUNT];
byte motorMinDelay[MOTOR_COUNT];
unsigned int motorPulsesPerSecond[MOTOR_COUNT];
MoveElement motorMove[MOTOR_COUNT][43];
MoveElement moveHolder[43];
unsigned int movingMotors;
#define FIND_NEXT_POSITION(MOTORINDEX) { MoveElement *move = motorMove[MOTORINDEX]; \
byte moveIndex = motorMoveIndex[MOTORINDEX]; \
while (moveIndex < 41 && move[moveIndex].steps == 0) \
{ \
moveIndex++; \
} \
motorMoveIndex[MOTORINDEX] = moveIndex; \
motorNextStep##MOTORINDEX = motorMoveRate##MOTORINDEX = move[moveIndex].rate; \
motorMoveSteps##MOTORINDEX = move[moveIndex].steps; \
}
#ifdef KILL_SWITCH_INTERRUPT
void killSwitch()
{
hardStop(false);
}
#endif
/*
* setup() gets called once, at the start of the program.
*/
void setup()
{
movingMotors = 0;
for (int i = 0; i < 32; i++)
txBuf[i] = 0;
txBufPtr = txBuf;
#ifdef KILL_SWITCH_INTERRUPT
attachInterrupt(KILL_SWITCH_INTERRUPT, killSwitch, CHANGE);
#endif
// initialize motor structures
for (int i = 0; i < MOTOR_COUNT; i++)
{
// setup motor pins - you can customize/modify these after loop
// default sets step/dir pairs together, with first four motors at 4/5, 6/7, 8/9, 10/11
// then, for the Mega boards, it jumps to 22/23, 24/25, etc
motorStepPin[i] = (i * 2) + ( (i < 4) ? 4 : 14 );
motorDirPin[i] = motorStepPin[i] + 1;
*motorDirection[i] = 0;
*motorPos[i] = 0L;
*motorMoveDestination[i] = 0L;
setPulsesPerSecond(i, 2000);
}
// set output pins
for (int i = 0; i < MOTOR_COUNT; i++)
{
pinMode(motorStepPin[i], OUTPUT);
pinMode(motorDirPin[i], OUTPUT);
#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
// disable PWM
switch (motorStepPin[i])
{
#if defined(TCCR3A) && defined(COM3B1)
case 4:
TCCR3A &= ~COM3B1;
break;
#endif
#if defined(TCCR4A) && defined(COM4A1)
case 6:
TCCR4A &= ~COM4A1;
break;
#endif
#if defined(TCCR4A) && defined(COM4C1)
case 8:
TCCR4A &= ~COM4C1;
break;
#endif
#if defined(TCCR2A) && defined(COM2A1)
case 10:
TCCR2A &= ~COM2A1;
break;
#endif
}
#else
switch (motorStepPin[i])
{
#if defined(TCCR1A) && defined(COM1B1)
case 10:
TCCR1A &= ~COM1B1;
break;
#endif
}
#endif
}
// set initial direction
for (int i = 0; i < MOTOR_COUNT; i++)
{
digitalWrite( motorDirPin[i], (*motorDirection[i] == 1) ? HIGH : LOW );
}
// setup serial connection
#ifdef ARDUINO
Serial.begin(57600);
#endif
sendMessage(MSG_HI, 0);
// SET UP interrupt timer
#ifdef MAPLE
#else
TCCR1A = 0;
TCCR1B = _BV(WGM13);
ICR1 = (F_CPU / 4000000) * TIME_CHUNK; // goes twice as often as time chunk, but every other event turns off pins
TCCR1B &= ~(_BV(CS10) | _BV(CS11) | _BV(CS12));
TIMSK1 = _BV(TOIE1);
TCCR1B |= _BV(CS10);
#endif
}
byte sendPosition = 0;
byte toggleStep = 0;
#ifdef MAPLE
void unused()
#else
ISR(TIMER1_OVF_vect)
#endif
{
toggleStep = !toggleStep;
if (toggleStep)
{
// MOTOR 1
if (bitRead(movingMotors, 0))
{
if (!--motorNextStep0)
{
motorPos0 += motorDirection0;
if (!--motorMoveSteps0)
{
motorMoveIndex[0]++;
FIND_NEXT_POSITION(0);
if (motorMoveIndex[0] > 40)
{
motorMoveDestination0 = motorPos0;
motorDirection0 = 0;
bitClear(movingMotors, 0);
bitSet(sendPosition, 0);
}
}
motorNextStep0 = motorMoveRate0;
#ifdef MAPLE
gpio_write_bit(MOTOR0_STEP_PORT, MOTOR0_STEP_PIN, HIGH);
#else
MOTOR0_STEP_PORT |= MOTOR0_STEP_PIN;
#endif
}
}
// MOTOR 2
if (bitRead(movingMotors, 1))
{
if (!--motorNextStep1)
{
motorPos1 += motorDirection1;
if (!--motorMoveSteps1)
{
motorMoveIndex[1]++;
FIND_NEXT_POSITION(1);
if (motorMoveIndex[1] > 40)
{
motorMoveDestination1 = motorPos1;
motorDirection1 = 0;
bitClear(movingMotors, 1);
bitSet(sendPosition, 1);
}
}
motorNextStep1 = motorMoveRate1;
#ifdef MAPLE
gpio_write_bit(MOTOR1_STEP_PORT, MOTOR1_STEP_PIN, HIGH);
#else
MOTOR1_STEP_PORT |= MOTOR1_STEP_PIN;
#endif
}
}
// MOTOR 3
if (bitRead(movingMotors, 2))
{
if (!--motorNextStep2)
{
motorPos2 += motorDirection2;
if (!--motorMoveSteps2)
{
motorMoveIndex[2]++;
FIND_NEXT_POSITION(2);
if (motorMoveIndex[2] > 40)
{
motorMoveDestination2 = motorPos2;
motorDirection2 = 0;
bitClear(movingMotors, 2);
bitSet(sendPosition, 2);
}
}
motorNextStep2 = motorMoveRate2;
#ifdef MAPLE
gpio_write_bit(MOTOR2_STEP_PORT, MOTOR2_STEP_PIN, HIGH);
#else
MOTOR2_STEP_PORT |= MOTOR2_STEP_PIN;
#endif
}
}
// MOTOR 4
if (bitRead(movingMotors, 3))
{
if (!--motorNextStep3)
{
motorPos3 += motorDirection3;
if (!--motorMoveSteps3)
{
motorMoveIndex[3]++;
FIND_NEXT_POSITION(3);
if (motorMoveIndex[3] > 40)
{
motorMoveDestination3 = motorPos3;
motorDirection3 = 0;
bitClear(movingMotors, 3);
bitSet(sendPosition, 3);
}
}
motorNextStep3 = motorMoveRate3;
#ifdef MAPLE
gpio_write_bit(MOTOR3_STEP_PORT, MOTOR3_STEP_PIN, HIGH);
#else
MOTOR3_STEP_PORT |= MOTOR3_STEP_PIN;
#endif
}
}
#if MOTOR_COUNT > 4
// MOTOR 5
if (bitRead(movingMotors, 4))
{
if (!--motorNextStep4)
{
motorPos4 += motorDirection4;
if (!--motorMoveSteps4)
{
motorMoveIndex[4]++;
FIND_NEXT_POSITION(4);
if (motorMoveIndex[4] > 40)
{
motorMoveDestination4 = motorPos4;
motorDirection4 = 0;
bitClear(movingMotors, 4);
bitSet(sendPosition, 4);
}
}
motorNextStep4 = motorMoveRate4;
#ifdef MAPLE
gpio_write_bit(MOTOR4_STEP_PORT, MOTOR4_STEP_PIN, HIGH);
#else
MOTOR4_STEP_PORT |= MOTOR4_STEP_PIN;
#endif
}
}
// MOTOR 6
if (bitRead(movingMotors, 5))
{
if (!--motorNextStep5)
{
motorPos5 += motorDirection5;
if (!--motorMoveSteps5)
{
motorMoveIndex[5]++;
FIND_NEXT_POSITION(5);
if (motorMoveIndex[5] > 40)
{
motorMoveDestination5 = motorPos5;
motorDirection5 = 0;
bitClear(movingMotors, 5);
bitSet(sendPosition, 5);
}
}
motorNextStep5 = motorMoveRate5;
#ifdef MAPLE
gpio_write_bit(MOTOR5_STEP_PORT, MOTOR5_STEP_PIN, HIGH);
#else
MOTOR5_STEP_PORT |= MOTOR5_STEP_PIN;
#endif
}
}
// MOTOR 7
if (bitRead(movingMotors, 6))
{
if (!--motorNextStep6)
{
motorPos6 += motorDirection6;
if (!--motorMoveSteps6)
{
motorMoveIndex[6]++;
FIND_NEXT_POSITION(6);
if (motorMoveIndex[6] > 40)
{
motorMoveDestination6 = motorPos6;
motorDirection6 = 0;
bitClear(movingMotors, 6);
bitSet(sendPosition, 6);
}
}
motorNextStep6 = motorMoveRate6;
#ifdef MAPLE
gpio_write_bit(MOTOR6_STEP_PORT, MOTOR6_STEP_PIN, HIGH);
#else
MOTOR6_STEP_PORT |= MOTOR6_STEP_PIN;
#endif
}
}
// MOTOR 8
if (bitRead(movingMotors, 7))
{
if (!--motorNextStep7)
{
motorPos7 += motorDirection7;
if (!--motorMoveSteps7)
{
motorMoveIndex[7]++;
FIND_NEXT_POSITION(7);
if (motorMoveIndex[7] > 40)
{
motorMoveDestination7 = motorPos7;
motorDirection7 = 0;
bitClear(movingMotors, 7);
bitSet(sendPosition, 7);
}
}
motorNextStep7 = motorMoveRate7;
#ifdef MAPLE
gpio_write_bit(MOTOR7_STEP_PORT, MOTOR7_STEP_PIN, HIGH);
#else
MOTOR7_STEP_PORT |= MOTOR7_STEP_PIN;
#endif
}
}
#endif
}
else
{
#ifdef MAPLE
gpio_write_bit(MOTOR0_STEP_PORT, MOTOR0_STEP_PIN, LOW);
gpio_write_bit(MOTOR1_STEP_PORT, MOTOR1_STEP_PIN, LOW);
gpio_write_bit(MOTOR2_STEP_PORT, MOTOR2_STEP_PIN, LOW);
gpio_write_bit(MOTOR3_STEP_PORT, MOTOR3_STEP_PIN, LOW);
gpio_write_bit(MOTOR4_STEP_PORT, MOTOR4_STEP_PIN, LOW);
gpio_write_bit(MOTOR5_STEP_PORT, MOTOR5_STEP_PIN, LOW);
gpio_write_bit(MOTOR6_STEP_PORT, MOTOR6_STEP_PIN, LOW);
gpio_write_bit(MOTOR7_STEP_PORT, MOTOR7_STEP_PIN, LOW);
#else
MOTOR0_STEP_PORT &= ~MOTOR0_STEP_PIN;
MOTOR1_STEP_PORT &= ~MOTOR1_STEP_PIN;
MOTOR2_STEP_PORT &= ~MOTOR2_STEP_PIN;
MOTOR3_STEP_PORT &= ~MOTOR3_STEP_PIN;
#if MOTOR_COUNT > 4
MOTOR4_STEP_PORT &= ~MOTOR4_STEP_PIN;
MOTOR5_STEP_PORT &= ~MOTOR5_STEP_PIN;
MOTOR6_STEP_PORT &= ~MOTOR6_STEP_PIN;
MOTOR7_STEP_PORT &= ~MOTOR7_STEP_PIN;
#endif
#endif
}
}
/*
* For stepper-motor timing, every clock cycle counts.
*/
void loop()
{
unsigned long lastSendPositionTime;
unsigned long now;
byte serialAvail;
lastSendPositionTime = millis();
while (true)
{
serialAvail = SERIAL_DEVICE.available();
if (serialAvail > 3)
processSerialCommand();
#ifdef MAPLE
else if (serialAvail > 0)
{
while (serialAvail--)
SERIAL_DEVICE.read();
}
#endif
// check if we have serial output
#ifdef ARDUINO
if (*txBufPtr)
{
if ((TX_UCSRA) & (1 << TX_UDRE))
{
TX_UDR = *txBufPtr++;
// we are done with this msg, get the next one
if (!*txBufPtr)
nextMessage();
}
}
#endif
now = millis();
if (now < lastSendPositionTime || (now - lastSendPositionTime) > 1000)
{
lastSendPositionTime = now;
if (movingMotors || sendPosition)
{
byte i;
for (i = 0; i < 8; i++)
{
if (bitRead(movingMotors, i) || bitRead(sendPosition, i))
sendMessage(MSG_MP, i);
}
}
sendPosition = 0;
}
}
}
/*
* Set up the axis for pulses per second (approximate)
*/
void setPulsesPerSecond(int motorIndex, unsigned int pulsesPerSecond)
{
if (pulsesPerSecond > 100000)
pulsesPerSecond = 100000;
if (pulsesPerSecond < 100)
pulsesPerSecond = 100;
unsigned int itersPerSecond = 1000000 / TIME_CHUNK;
unsigned int minDelay = itersPerSecond / pulsesPerSecond;
motorPulsesPerSecond[motorIndex] = pulsesPerSecond;
motorMinDelay[motorIndex] = min(235, max(2, minDelay));
}
void setupMotorMove(int motorIndex, long destination)
{
#if DEBUG_MOVE == 1
unsigned long microTime = micros();
#endif
noInterrupts();
*motorMoveDestination[motorIndex] = destination;
if ( destination != *motorPos[motorIndex] )
{
boolean forward = *motorMoveDestination[motorIndex] >= *motorPos[motorIndex];
*motorDirection[motorIndex] = forward ? 1 : -1;
digitalWrite(motorDirPin[motorIndex], forward);
bitSet(movingMotors, motorIndex);
}
else
{
*motorDirection[motorIndex] = 0;
bitClear(movingMotors, motorIndex);
interrupts();
return;
}
interrupts();
unsigned long totalSteps = (unsigned long) abs(*motorMoveDestination[motorIndex] - *motorPos[motorIndex]) ;
unsigned int targetChunk = (unsigned int)(150000L / TIME_CHUNK); // .15 seconds
byte i, endIndex, delayTime;
MoveElement *move = moveHolder;
for (i = 0, endIndex = 40, delayTime = motorMinDelay[motorIndex] + 20 ; i < 20 ; i++, delayTime--, endIndex--)
{
move[endIndex].rate = delayTime;
move[i].rate = delayTime;
// I don't think we can overflow an int here
unsigned int steps = targetChunk / delayTime;
unsigned int moveSteps = steps < totalSteps ? steps : totalSteps;
move[endIndex].steps = moveSteps;
totalSteps -= moveSteps;
moveSteps = steps < totalSteps ? steps : totalSteps;
move[i].steps = moveSteps;
totalSteps -= moveSteps;
}
move[i].rate = motorMinDelay[motorIndex];
move[i].steps = totalSteps;
noInterrupts();
memcpy(motorMove[motorIndex], move, sizeof(MoveElement) * 43);
motorMoveIndex[motorIndex] = 0;
findNextPosition(motorIndex);
interrupts();
// PRINT OUT ENTIRE MOVE
#if DEBUG_MOVE == 1
unsigned long duration = micros() - microTime;
unsigned int long total = 0;
SERIAL_DEVICE.println("MOVE");
SERIAL_DEVICE.println(duration);
for (int i = 0; i < 41; i++)
{
SERIAL_DEVICE.print(i);
SERIAL_DEVICE.print(" ");
SERIAL_DEVICE.print((int) move[i].rate);
SERIAL_DEVICE.print(" ");
SERIAL_DEVICE.print(move[i].steps);
SERIAL_DEVICE.print("\r\n");
total += (move[i].steps);
}
SERIAL_DEVICE.print("TOTAL: " );
SERIAL_DEVICE.print(total);
SERIAL_DEVICE.print("\r\n");
#endif
}
void findNextPosition(byte motorIndex)
{
MoveElement *move = motorMove[motorIndex];
byte moveIndex = motorMoveIndex[motorIndex];
while (moveIndex < 41 && move[moveIndex].steps == 0)
{
moveIndex++;
}
motorMoveIndex[motorIndex] = moveIndex;
*motorMoveRate[motorIndex] = move[moveIndex].rate;
*motorMoveSteps[motorIndex] = move[moveIndex].steps;
*motorNextStep[motorIndex] = *motorMoveRate[motorIndex];
}
void hardStop(boolean disableInterrupts)
{
// set the destination to the current location, so they won't move any more
if (disableInterrupts)
noInterrupts();
for (int i = 0; i < MOTOR_COUNT; i++)
{
*motorMoveDestination[i] = *motorPos[i];
*motorDirection[i] = 0;
}
movingMotors = 0;
if (disableInterrupts)
interrupts();
}
void stopMotor(int motorIndex)
{
#if DEBUG_MOVE == 1
SERIAL_DEVICE.print("stop motor: ");
SERIAL_DEVICE.print((int)motorMoveIndex[motorIndex]);
SERIAL_DEVICE.print(" ");
#endif
noInterrupts();
if (*motorDirection[motorIndex])
{
if (motorMoveIndex[motorIndex] < 21)
{
int delta = 20 - motorMoveIndex[motorIndex];
motorMoveIndex[motorIndex] = max(21, 20 + delta);
findNextPosition(motorIndex);
}
}
interrupts();
#if DEBUG_MOVE == 1
SERIAL_DEVICE.print((int)*motorDirection[motorIndex]);
SERIAL_DEVICE.print(" ");
SERIAL_DEVICE.print((int)motorMoveIndex[motorIndex]);
SERIAL_DEVICE.println();
#endif
}
boolean isValidMotor(int motorIndex)
{
return (motorIndex >=0 && motorIndex < MOTOR_COUNT);
}
void processGoPosition(int motorIndex, long pos)
{
noInterrupts();
if (*motorPos[motorIndex] != pos)
{
setupMotorMove(motorIndex, pos);
sendMessage(MSG_MM, motorIndex);
}
else
{
sendMessage(MSG_MP, motorIndex);
}
interrupts();
}
/*
Command format
TODO:
CHANGE hello to return version, motor count
ASCII
[command two bytes]
Version
"hi"
-> "hi 1"
zero motor
"zm 1"
-> "z 1"
move motor
"mm 1 +1111111111
motor position?
mp 1
MOTOR STATUS
"ms"
-> "ms [busy motor count]"
SET PULSE PER SECOND
"pp 1 200"
STOP MOTOR
sm 1
HARD STOP
hs
*/
void processSerialCommand()
{
byte cmd1 = 0;
byte cmd2 = 0;
int motor = 0;
long pos = 0;
long startTime = millis();
cmd1 = (byte)SERIAL_DEVICE.read();
cmd2 = (byte)SERIAL_DEVICE.read();
if (cmd1 == 'h' && cmd2 == 'i' && readEndLine())
{
sendMessage(MSG_HI, 0);
return;
}
else if (cmd1 == 'z' && cmd2 == 'm') // zero motor
{
if (eatSpace() && readMotor(&motor) && readEndLine())
{
*motorPos[motor - 1] = 0;
setupMotorMove(motor - 1, 0);
processGoPosition(motor - 1, 0);
return;
}
}
else if (cmd1 == 'm' && cmd2 == 'm') // move motor
{
if (eatSpace() && readMotor(&motor) && eatSpace() && readPosition(&pos) && readEndLine())
{
processGoPosition(motor - 1, pos);
return;
}
}
else if (cmd1 == 'm' && cmd2 == 'p') // query position
{
if (eatSpace() && readMotor(&motor) && readEndLine())
{
sendMessage(MSG_MP, motor - 1);
return;
}
}
else if (cmd1 == 'm' && cmd2 == 's') // motor status
{
if (readEndLine())
{
sendMessage(MSG_MS, 0);
return;
}
}
else if (cmd1 == 's' && cmd2 == 'm') // stop motor
{
if (eatSpace() && readMotor(&motor) && readEndLine())
{
stopMotor(motor - 1);
sendMessage(MSG_SM, motor - 1);
sendMessage(MSG_MP, motor - 1);
return;
}
}
else if (cmd1 == 's' && cmd2 == 'a') // stop all
{
if (readEndLine())
{
hardStop(true);
sendMessage(MSG_SA, 0);
return;
}
}
else if (cmd1 == 'p' && cmd2 == 'r') // pulses rate (pulses per second)
{
if (eatSpace() && readMotor(&motor) && eatSpace() && readPosition(&pos) && readEndLine())
{
setPulsesPerSecond(motor - 1, (unsigned int)pos);
sendMessage(MSG_PR, motor - 1);
return;
}
}
sendError();
}
boolean readMotor(int *motor)
{
long startTime = millis();
#ifdef MAPLE
if (SERIAL_DEVICE.available() < 1)
return false;
#else
while (SERIAL_DEVICE.available() < 1)
{
if ((millis() - startTime) > 100)
return false;
}
#endif
int a = SERIAL_DEVICE.read() - '0';
if (a < 0 || a > 9)
return false;
*motor = a;
return isValidMotor(*motor - 1);
}
boolean readPosition(long *pos)
{
*pos = 0;
boolean negative = false;
long startTime = millis();
#ifdef MAPLE
if (SERIAL_DEVICE.available() < 2)
return false;
#else
while (SERIAL_DEVICE.available() < 2)
{
if ((millis() - startTime) > 100)
return false;
}
#endif
byte p = (byte)SERIAL_DEVICE.read();
if (p == '-' || p == '+')
{
negative = (p == '-');
}
else if (p >= '0' && p <= '9')
{
*pos += (p - '0');
}
while (true)
{
#ifdef MAPLE
if (SERIAL_DEVICE.available() < 1)
return false;
#else
while (SERIAL_DEVICE.available() < 1)
{
if ((millis() - startTime) > 100)
return false;
}
#endif
byte p = (byte)SERIAL_DEVICE.read();
if (p < '0' || p > '9')
break;
*pos *= 10;
*pos += (p - '0');
}
if (negative)
*pos = -(*pos);
return true;
}
boolean readEndLine()
{
#ifdef MAPLE
byte r = (byte)SERIAL_DEVICE.read();
if (r == '\n')
return true;
byte n = (byte)SERIAL_DEVICE.read();
return (r == '\r' && n == '\n') || (r == '\n' && n == '\r');
#else
long startTime = millis();
while (SERIAL_DEVICE.available() < 1)
{
if ((millis() - startTime) > 100)
return false;
}
if (SERIAL_DEVICE.available() >= 1)
{
byte r = (byte)SERIAL_DEVICE.read();
if (r == '\n')
return true;
startTime = millis();
while (SERIAL_DEVICE.available() < 1)
{
if ((millis() - startTime) > 100)
return false;
}
byte n = (byte)SERIAL_DEVICE.read();
return (r == '\r' && n == '\n') || (r == '\n' && n == '\r');
}
return false;
#endif
}
boolean eatSpace()
{
long startTime = millis();
#ifdef MAPLE
if (SERIAL_DEVICE.available() < 1)
return false;
#else
while (SERIAL_DEVICE.available() < 1)
{
delay(1);
if ((millis() - startTime) > 100)
return false;
}
#endif
byte b = (byte)SERIAL_DEVICE.read();
return b == ' ';
}
void sendError()
{
SERIAL_DEVICE.print("Parse error\r\n");
while (SERIAL_DEVICE.available() > 0)
{
byte n = (byte)SERIAL_DEVICE.read();
if (n == '\n')
break;
}
}
/*
*
* Serial transmission.
*
*/
void sendMessage(byte msg, byte motorIndex)
{
#ifdef ARDUINO
interrupts();
int i = (unsigned int)(txMsgBuffer.head + 1) % TX_MSG_BUF_SIZE;
if (i != txMsgBuffer.tail)
{
txMsgBuffer.buffer[txMsgBuffer.head].msg = msg;
txMsgBuffer.buffer[txMsgBuffer.head].motor = motorIndex;
txMsgBuffer.head = i;
if (!*txBufPtr)
nextMessage();
}
interrupts();
#else
int i;
switch (msg)
{
case MSG_HI:
SERIAL_DEVICE.print("hi ");
SERIAL_DEVICE.print(DFMOCO_VERSION);
SERIAL_DEVICE.print(" ");
SERIAL_DEVICE.print(MOTOR_COUNT);
SERIAL_DEVICE.print(" ");
SERIAL_DEVICE.print(DFMOCO_VERSION_STRING);
SERIAL_DEVICE.print("\r\n");
break;
case MSG_MM:
SERIAL_DEVICE.print("mm ");
SERIAL_DEVICE.print(motorIndex + 1);
SERIAL_DEVICE.print(" ");
noInterrupts();
SERIAL_DEVICE.print(*motorMoveDestination[motorIndex]);
interrupts();
SERIAL_DEVICE.print("\r\n");
break;
case MSG_MP:
SERIAL_DEVICE.print("mp ");
SERIAL_DEVICE.print(motorIndex + 1);
SERIAL_DEVICE.print(" ");
noInterrupts();
SERIAL_DEVICE.print(*motorPos[motorIndex]);
interrupts();
SERIAL_DEVICE.print("\r\n");
break;
case MSG_MS:
SERIAL_DEVICE.print("ms ");
noInterrupts();
for (i = 0; i < MOTOR_COUNT; i++)
SERIAL_DEVICE.print(bitRead(movingMotors, i) ? '1' : '0');
interrupts();
SERIAL_DEVICE.print("\r\n");
break;
case MSG_PR:
SERIAL_DEVICE.print("pr ");
SERIAL_DEVICE.print(motorIndex + 1);
SERIAL_DEVICE.print(" ");
noInterrupts();
SERIAL_DEVICE.print(motorPulsesPerSecond[motorIndex]);
interrupts();
SERIAL_DEVICE.print("\r\n");
break;
case MSG_SM:
SERIAL_DEVICE.print("sm ");
SERIAL_DEVICE.print(motorIndex + 1);
SERIAL_DEVICE.print("\r\n");
break;
case MSG_SA:
SERIAL_DEVICE.print("sa\r\n");
break;
}
#endif
}
#ifdef ARDUINO
void nextMessage()
{
char *bufPtr;
int i;
noInterrupts();
if ((TX_MSG_BUF_SIZE + txMsgBuffer.head - txMsgBuffer.tail) % TX_MSG_BUF_SIZE)
{
byte msg = txMsgBuffer.buffer[txMsgBuffer.tail].msg;
byte motorIndex = txMsgBuffer.buffer[txMsgBuffer.tail].motor;
txMsgBuffer.tail = (unsigned int)(txMsgBuffer.tail + 1) % TX_MSG_BUF_SIZE;
interrupts();
switch (msg)
{
case MSG_HI:
sprintf(txBuf, "hi %d %d %s\r\n", DFMOCO_VERSION, MOTOR_COUNT, DFMOCO_VERSION_STRING);
break;
case MSG_MM:
sprintf(txBuf, "mm %d %ld\r\n", motorIndex + 1, *motorMoveDestination[motorIndex]);
break;
case MSG_MP:
sprintf(txBuf, "mp %d %ld\r\n", motorIndex + 1, *motorPos[motorIndex]);
break;
case MSG_MS:
sprintf(txBuf, "ms ");
bufPtr = txBuf + 3;
for (i = 0; i < MOTOR_COUNT; i++)
*bufPtr++ = bitRead(movingMotors, i) ? '1' : '0';
*bufPtr++ = '\r';
*bufPtr++ = '\n';
*bufPtr = 0;
break;
case MSG_PR:
sprintf(txBuf, "pr %d %d\r\n", motorIndex + 1, motorPulsesPerSecond[motorIndex]);
break;
case MSG_SM:
sprintf(txBuf, "sm %d\r\n", motorIndex + 1);
break;
case MSG_SA:
sprintf(txBuf, "sa\r\n");
break;
}
txBufPtr = txBuf;
}
else
interrupts();
}
#endif