Rubik: Arduino sketch updated with acceleration curves

rubik_arduino.c

/*
 * Full-step = 1, Half-step = 2, Quarter = 4, Eighth = 8, Sixteenth = 16
 */
const int STEP_FACTOR = 8; 

/*
 * pins to select stepfactor
 */
const int MS1 = 52;
const int MS2 = 50;
const int MS3 = 48;

/*
 * Motors 0-5 are represented by the letters for the faces of the cube.
 * Up, Face, Right, Back, Left, Down 
 */
enum Motor { U, F, R, B, L, D }; 


enum PinOffset { ENABLE, DIRECTION, STEP, SLEEP, RESET, UNUSED };

/*
 * The number of arduino pins allocacted per motor, used to calculate the first pin for a particular motor
 */
const int PINS_PER_MOTOR = 6; //one for each possible pin + one extra for spacing

/*
 * Direction is important when doing quarter turns. 
 * TODO: verify that this is correct for each motor
 */
enum Direction{ CLOCKWISE = HIGH, COUNTERCLOCKWISE = LOW };

/*
 * Array of decreasing delays. Size is allocated dynamically based on the stepping resolution.
 * The table is used to accelerate the initial movement of the stepper motor but also slow
 * down the last steps
 */
int* accelerationTable;

/*
 * Number of delays in table, also number of steps per turn before full speed is reached
 */
const int TABLE_SIZE = 3 * STEP_FACTOR;

/*
 * Delay between high/low pulses to the motors. 
 * A 90 degree rotation with a 10 millisecond delay will take 1 second.
 */
const int STEPDELAY_MICROS_MIN = 320 / STEP_FACTOR; 
const int STEPDELAY_MICROS_MAX = 4000 / STEP_FACTOR; 

/*
 * Number of steps in full step mode required to make a 90 degree turn
 * 200 steps per revolution in full step mode
 */
const int STEPS_90 = 50 * STEP_FACTOR;

/*
 * Number of steps to make a 180 degree turn
 */
const int STEPS_180 = STEPS_90 * 2;

/*
 * Macro for calculating first pin of a particular motor
 * Usage: int pin = BASE_PIN(U) + STEP;
 */
#define BASE_PIN(x) (((int)(x)) * PINS_PER_MOTOR + 2)

void setup() {

  int numberOfPinsToConfigure = PINS_PER_MOTOR * 6;
  
  for(int i = 0; i < numberOfPinsToConfigure; i++ ) 
    pinMode(i + 2, OUTPUT);

  for(int i = 0; i < 6; i++) {
    int basePin = BASE_PIN(i);
    digitalWrite(basePin + DIRECTION, LOW);
    digitalWrite(basePin + STEP, LOW);
    digitalWrite(basePin + ENABLE, HIGH); //no power to the motors
  }
  setStepFactor();
  createAccelerationTable(STEPDELAY_MICROS_MIN, STEPDELAY_MICROS_MAX);
  Serial.begin(9600);
  Serial.println("Starting in 5 seconds"); //allow time to plug in motor power
  delay(5000);
}

void loop() {  
  smokeTest();
}

/*
 * Turn the first motor 90 degrees forward and back, then 180 degrees forward and back. Rinse. Repeat.
 */
void smokeTest(){
  Motor motor = U; //the motor to turn
  int delayBetweenTurns = 1000;
  int delayBetweenLoops = 5000;
  
  Serial.println("turn(U, CLOCKWISE, STEPS_90)");
  turn(motor, CLOCKWISE, STEPS_90);
  delay(delayBetweenTurns);
  
  Serial.println("turn(U, COUNTERCLOCKWISE, STEPS_90)");
  turn(motor, COUNTERCLOCKWISE, STEPS_90);
  delay(delayBetweenTurns);
  
  Serial.println("turn(U, CLOCKWISE, STEPS_180)");
  turn(motor, CLOCKWISE, STEPS_180);
  delay(delayBetweenTurns);
  
  Serial.println("turn(U, COUNTERCLOCKWISE, STEPS_180)");
  turn(motor, COUNTERCLOCKWISE, STEPS_180);
  delay(delayBetweenLoops);  
}


void setStepFactor() {
  if (STEP_FACTOR == 1) {
    digitalWrite(MS1, LOW);
    digitalWrite(MS2, LOW);
    digitalWrite(MS3, LOW); 
  }
  else if (STEP_FACTOR == 2) {
    digitalWrite(MS1, HIGH);
    digitalWrite(MS2, LOW);
    digitalWrite(MS3, LOW); 
  }
  else if (STEP_FACTOR == 4) {
    digitalWrite(MS1, LOW);
    digitalWrite(MS2, HIGH);
    digitalWrite(MS3, LOW); 
  }
    else if (STEP_FACTOR == 8) {
    digitalWrite(MS1, HIGH);
    digitalWrite(MS2, HIGH);
    digitalWrite(MS3, LOW); 
  }
    else if (STEP_FACTOR == 16) {
    digitalWrite(MS1, HIGH);
    digitalWrite(MS2, HIGH);
    digitalWrite(MS3, HIGH); 
  }
  delay(2);
}

/*
 * Enable or disable a single motor.
 * Note that motor is enabled when signal is LOW
 */
void setEnabled(Motor motor, boolean value) {
  int pin = BASE_PIN(motor) + ENABLE;
  digitalWrite(pin, value ? LOW : HIGH);
}


/*
 * Enable or disable all 6 motors
 */
void setAllEnabled(boolean value) {
  for(int i = 0; i < 6; i++) setEnabled((Motor)i, value);  
}

/*
 * Allocates table for delays and calculates them using a quadratic function
  */
void createAccelerationTable(int min, int max) {
  
  accelerationTable = (int*)malloc(sizeof(int) * TABLE_SIZE);
  float step = (max - min) / (float)TABLE_SIZE;
  for(int i=0;i<TABLE_SIZE;i++) {
    int val = max - int(step * i);
   *(accelerationTable + i) = val;
  }
}

/*
 * Turn a single motor, a given number of steps in a given direction
 */
void turn(Motor motor, Direction direction, int steps) {

   long startTime = micros();

   //calculate the actual step and direction pins
   int step_pin = BASE_PIN(motor) + STEP;
   int dir_pin = BASE_PIN(motor) + DIRECTION;
   
   //power up the motor
   setEnabled(U, true);

   //set the direction
   digitalWrite(dir_pin, direction);
   
   for(int step = 0; step < steps; step++) {

    // figure out the delay for the current step
    // use delay from table for the starting and finishing steps,
    // or the minimum delay for steps in between
    // which means full speed
    int delay_micros = (step < TABLE_SIZE) ? *(accelerationTable + step) : STEPDELAY_MICROS_MIN;
    if (step >= steps - TABLE_SIZE) delay_micros = *(accelerationTable + steps - 1 - step);

    //send a step pulse
    digitalWrite(step_pin, HIGH);
    delayMicroseconds(delay_micros);
    digitalWrite(step_pin, LOW);
    delayMicroseconds(delay_micros);
  }
  long duration = micros() - startTime;
  
  //release holding current to motor because power supply is to weak
  //to power 6 motors simultaneously, also keeps motor heat down.
  setEnabled(U, false);
  
  Serial.print("duration in micros: ");
  Serial.println(duration);
}