Arduino - Making Music Using Step Motors

#include <math.h>
#include <Phpoc.h>
#include <PhpocExpansion.h>


ExpansionStepper step1(13);
ExpansionStepper step2(14);

ExpansionStepper steps[2] = {step1, step2};

void stepPlaySetup(int play_id, int vref = 8, int mode = 2, int accel = 10000);

void setup() {
    Serial.begin(9600);
    while(!Serial)
        ;

    Phpoc.begin(PF_LOG_SPI | PF_LOG_NET);
    Expansion.begin();

    stepPlaySetup(0);
    stepPlaySetup(1);
    stepPlayTempo(38);

    play_marble_machine_song();
}

void loop() {

}

const char melody1a[] PROGMEM = "5E2.67 4B2 4A8 4G8 4A4 4B4 4G8 4A8 5D8 R2.67 4B2 4A8 4G8 4A4 4F#4 4G8 4A8 5D8";
const char melody1b[] PROGMEM = "R2.67 4B2 5D8 5C8 4B4 4A4 4G8 4A8 4E8 R8 4C8 4E8 4B8 3B8 4C8 4D8 5D8 5C8 4B4 4A4 4G8 4A8 5E8";
const char melody1c[] PROGMEM = "R2.67 4B2 4A8 4G8 4A4 4B4 4G8 4A8 5D8 R2.67 4B2 5D8 5C8 4B4 4A4 4G8 4A8 5D8";
const char melody1d[] PROGMEM = "R2.67 4B2.67 4A8 5E8 R8 4B4 4A4 4G8 4F#8 4E8 R8 3B8 4C8 4F#8 4C8 4E8 4G8 4D8 4F#8 4A8 3B8 4B8 4D8 4G8 4A8 5E8";

const char chord1a[] PROGMEM = "3E4 4E2.67 3E8 4E4 3E4 4E2.67 3E8 4E4 3G4 4G2.67 3G8 4G4 3G4 4G2.67 3G8 4G4";
const char chord1b[] PROGMEM = "3D4 4D2.67 3D8 4D4 3D4 4D2.67 3D8 4D4 3C4 4C2.67 3C8 4C4 3D4 4D2.67 3D8 4D4";
const char chord1c[] PROGMEM = "3E4 4E2.67 3E8 4E4 3E4 4E2.67 3E8 4E4 3G4 4G2.67 3G8 4G4 3G4 4G2.67 3G8 4G4";
const char chord1d[] PROGMEM = "3D4 4D2.67 3D8 4D4 3D4 4D2.67 3D8 4D8 3C4 4C2.67 3C8 4C4 3D4 4D2.67 3D8 4D4";

const char* const melody_table[] PROGMEM = { melody1a, melody1b, melody1c, melody1d };
const char* const chord_table[] PROGMEM = { chord1a, chord1b, chord1c, chord1d };

char buffer[110];

int melody[55];
int chord[55];
byte melody_len, chord_len;

void play_marble_machine_song() {
    for (byte i = 0; i < 4; i++) {
        strcpy_P(buffer, (char*)pgm_read_word(&(melody_table[i])));
        melody_len = stepPlayEncode(buffer, melody);
        strcpy_P(buffer, (char*)pgm_read_word(&(chord_table[i])));
        chord_len = stepPlayEncode(buffer, chord);

        stepPlayChangeDir();
        stepPlayHarmony(melody, chord, melody_len, chord_len);
        //stepPlayMelody(melody, melody_len);
    }

    Serial.println("End!");
}

/* Library */
int stepPlaySpeed = 1000; // ms per bar
int stepPlayDirection = +1;

void stepPlaySetup(int play_id, int vref = 8, int mode = 2, int accel = 10000) {
    steps[play_id].setVrefStop(2);
    steps[play_id].setVrefDrive(vref);
    steps[play_id].setMode(mode);
    steps[play_id].setAccel(accel);
}

void stepPlayTempo(int tempo) {
    stepPlaySpeed = (int)(1000.0 / (tempo / 60.0));
}

void stepPlayChangeDir() {
        stepPlayDirection *= -1;
}

int stepPlayEncode(char *score, int *score_encoded) {
    char note[10];
    int score_len = 0;
    int score_encoded_id = 0;
    int blankPos = -1;
    int blankPosNext = 0;

    while(blankPosNext != -1) {
        float dur_fp;
        long dur_ms;
        long freq;

        blankPosNext = blankPos + 1;

        while(score[blankPosNext] != ' ') {
            blankPosNext++;
            if(score[blankPosNext]  == 0) {
                score_len = blankPosNext;
                blankPosNext = -1;
                break;
            }
        }

        if(blankPosNext >= 0) {
            int i;
            for(i = blankPos + 1; i < blankPosNext; i++)
                note[i - (blankPos + 1)] = score[i];

            note[i - (blankPos + 1)] = 0;
            blankPos = blankPosNext;

            if(note[0] == ' ')
                continue;
        } else {
            int i;
            for(i = blankPos + 1; i < score_len; i++)
                note[i - (blankPos + 1)] = score[i];

            note[i - (blankPos + 1)] = 0;
        }

        if(note[0] == 'R') { /* rest */
            note[0] = '0';
            dur_fp =  atof(note);
            dur_ms = (long)(stepPlaySpeed / dur_fp);

            freq = 0;
        } else { /* tone */
            int tone = (note[0] - 48)  * 12; // octave * 12

            // C  C# D  D# E  F  F# G  G# A A# B
            // 0  1  2  3  4  5  6  7  8  9 10 11

            switch(note[1]) {
                case 'C':
                    tone += 0;
                    break;
                case 'D':
                    tone += 2;
                    break;
                case 'E':
                    tone += 4;
                    break;
                case 'F':
                    tone += 5;
                    break;
                case 'G':
                    tone += 7;
                    break;
                case 'A':
                    tone += 9;
                    break;
                case 'B':
                    tone += 11;
                    break;
                default:
                    Serial.print("encode_score: unknown tone: ");
                    Serial.println(note);
                    return 0;
            }

            if(note[2] == '#') {
                tone++;
                note[0] = '0';
                note[1] = '0';
                note[2] = '0';
            } else if(note[2] == 'b') {
                tone--;
                note[0] = '0';
                note[1] = '0';
                note[2] = '0';
            } else {
                note[0] = '0';
                note[1] = '0';
            }

            dur_fp = atof(note);
            dur_ms = (long)(stepPlaySpeed / dur_fp);

            int octave = tone / 12;
            tone = tone % 12;

            long freqA = 440.0 * pow(2, (octave - 4));
            freq = (int)(freqA * pow(2, (tone - 9) / 12.0));
        }

        score_encoded[score_encoded_id++] = freq;
        score_encoded[score_encoded_id++] = dur_ms;
    }

    return score_encoded_id;
}

void stepPlayMelody(int *melody, int melody_len) {
    int melody_count = 0;

    while(melody_count < melody_len) {
        int freq = melody[melody_count++];
        int dur_ms = melody[melody_count++];

        if(freq) {
            steps[0].setSpeed(freq);
            steps[0].stepGoto(stepPlayDirection * 1000000);

            delay(dur_ms);
            steps[0].stop(0);
        } else // zero frequency is 'rest'
            delay(dur_ms);
    }
}

void stepPlayHarmony(int *melody1, int *melody2, int melody1_len, int melody2_len) {
    int *melody[2] = {melody1, melody2};
    unsigned long melody_start_ms = millis();
    unsigned long melody_next_ms[2] = { 0, 0 };
    int melody_count[2] = { 0, 0 };
    int melody_len[2] = { melody1_len, melody2_len };

    while((melody_count[0] < melody_len[0] ) || (melody_count[1] < melody_len[1] ) || melody_next_ms[0] || melody_next_ms[1]) {
        for(int i = 0; i < 2; i++) {
            if(melody_next_ms[i]) {
                if(melody_next_ms[i] <= millis()) {
                    steps[i].stop(0);
                    melody_next_ms[i] = 0;
                }
            } else {
                if(melody_count[i] < melody_len[i] ) {
                    int freq = melody[i][melody_count[i]];
                    int dur_ms = melody[i][melody_count[i] + 1];

                    melody_count[i] += 2;

                    if(freq) {
                        steps[i].stop(0);
                        steps[i].setSpeed(freq);
                        steps[i].stepGoto(stepPlayDirection * 1000000);

                        Serial.print("freq: ");
                        Serial.print(freq);
                        Serial.print(", dur_ms: ");
                        Serial.println(dur_ms);
                    }

                    melody_next_ms[i] = millis() + dur_ms;
                }
            }
        }
    }
}