Motor Shield and Dragon Frame

Adafruit Ethernet, Motor, Proto, Wave, Datalogger, GPS Shields - etc!

Moderators: adafruit_support_bill, adafruit

Please be positive and constructive with your questions and comments.
Locked
juliflash
 
Posts: 4
Joined: Thu Jun 21, 2012 2:24 pm

Motor Shield and Dragon Frame

Post by juliflash »

Hello Im new over here so thanks in advance!

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


User avatar
adafruit_support_bill
 
Posts: 88096
Joined: Sat Feb 07, 2009 10:11 am

Re: Motor Shield and Dragon Frame

Post by adafruit_support_bill »

The AF_Motor library exposes a "onestep(direction, style)" function. This has been used successfully to adapt other stepper libraries such as AccelStepper to the MotorShield.

juliflash
 
Posts: 4
Joined: Thu Jun 21, 2012 2:24 pm

Re: Motor Shield and Dragon Frame

Post by juliflash »

adafruit_support wrote:The AF_Motor library exposes a "onestep(direction, style)" function. This has been used successfully to adapt other stepper libraries such as AccelStepper to the MotorShield.
So... could you help me? ani ideas, link... way to aproach the change??

thanks

User avatar
adafruit_support_bill
 
Posts: 88096
Joined: Sat Feb 07, 2009 10:11 am

Re: Motor Shield and Dragon Frame

Post by adafruit_support_bill »

From the DragonFrame site:
An Arduino running the DFMoco sketch will generate step and direction signals for stepper motors.
I think the easiest approach would be to modify the DFMoco sketch to call AFMotor's onestep() instead of the digitalWrite() calls it is making now.

juliflash
 
Posts: 4
Joined: Thu Jun 21, 2012 2:24 pm

Re: Motor Shield and Dragon Frame

Post by juliflash »

I´m sorry to ask maybe for basic questions, but I´m not really a guru of the code... I bought the motor shield to make things a bit easier, but Im happy to try whatever to make it work :)

I would need some help to achieve that, is there any website I could refer to see how to adapt the library?

Would it be enough with, adding the AFmotor/AFstepper library to the sketch, plus modifying this line...

Code: Select all

// set initial direction
  for (int i = 0; i < MOTOR_COUNT; i++)
  {
    digitalWrite( motorDirPin[i], (*motorDirection[i] == 1) ? HIGH : LOW );
  }

With this?

Code: Select all

void forwardstep1() {  
  motor1.onestep(FORWARD, SINGLE);
}
void backwardstep1() {  
  motor1.onestep(BACKWARD, SINGLE);
}
Thanks!

User avatar
adafruit_support_bill
 
Posts: 88096
Joined: Sat Feb 07, 2009 10:11 am

Re: Motor Shield and Dragon Frame

Post by adafruit_support_bill »

It looks like you are on the right track with the forwardstep() backwardstep(). Are these functions that are called in the DragonFrame sketch?

User avatar
pburgess
 
Posts: 4161
Joined: Sun Oct 26, 2008 2:29 am

Re: Motor Shield and Dragon Frame

Post by pburgess »

I've had a look through the DFMoco sketch, and unfortunately this isn't going to be a simple swap of a few lines. The sketch is pretty deeply designed for a combination of both digitalWrites and direct PORT accesses for direction and singular step pulses. The Timer1 interrupt would need to change also. I'm confident this could be adapted for the AF_Motor library, but it's going to involve ripping all that out and taking a different approach altogether...it's a pretty big task. Depending on your timeframe and budget, adding the EasyStepper boards might be a quicker solution.

It's funny, my roommate was asking about this exact combination of hardware and software yesterday.

juliflash
 
Posts: 4
Joined: Thu Jun 21, 2012 2:24 pm

Re: Motor Shield and Dragon Frame

Post by juliflash »

Thanks pburgess and adafruit support...

I was thinking aswell that it was going to be easier to just buy the easydriver shields... at the end of the day is written for that, and I dont have enough knowledge to change all that code... is a pity to not know enough :)

So thanks a lot to both of you!
All the best,
Julian

Locked
Please be positive and constructive with your questions and comments.

Return to “Arduino Shields from Adafruit”