emulate stepper motor cog movement inertia

src/diskcontroller.cpp

@@ -87,6 +87,9 @@ unsigned char DiskController::io(const unsigned short addr, const unsigned char
  * (When the motor is on, that is.)
  */
 void DiskController::tick() {
+    this->arm1.tick();
+    this->arm2.tick();
+
     if (this->ioStepped) { // if we already ran it, above in io(), skip here
         this->ioStepped = false;
         return;

src/steppermotor.cpp

@@ -1,6 +1,7 @@
 /*
     epple2
-    Copyright (C) 2008 by Christopher A. Mosher <cmosher01@gmail.com>
+
+    Copyright © 2008–2018, Christopher Alan Mosher, Shelton, CT, USA. <cmosher01@gmail.com>
 
     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License as published by
@@ -17,11 +18,6 @@
 */
 /**
  * Emulates the arm stepper motor in the Disk ][.
- * This emulator moves the arm
- * instantaneously, whereas the Disk ][ arm would actually
- * take some time to reach its new position (this would
- * cause a difference if the state of the magnets changed
- * during this interval).
  *
  * @author Chris Mosher
  */
@@ -66,64 +62,75 @@ StepperMotor::StepperMotor():
     quarterTrack(QTRACKS >> 1), // start in the middle of the disk... just for fun
 // TODO if we want to be extremely accurate, we should save each arm's position on shutdown and restore on startup
 // (because in the real-life Apple ][, the arm stays in the same position when powered off).
-	pos(0),
-    mags(0) {
+    pos(0),
+    mags(0),
+    pendingPos(0),
+    pendingTicks(0) {
 }
 
 StepperMotor::~StepperMotor() {
 }
 
-signed char StepperMotor::mapMagPos[] = {-1,0,2,1,4,-1,3,2,6,7,-1,0,5,6,4,-1};
+std::int8_t StepperMotor::mapMagPos[] = {-1,0,2,1,4,-1,3,2,6,7,-1,0,5,6,4,-1};
 
-void StepperMotor::setMagnet(const unsigned char magnet, const bool on) {
-    const unsigned char mask = 1 << magnet;
+void StepperMotor::calculateTrack(const std::int8_t delta) {
+    std::int16_t q = this->quarterTrack;
+    q += delta;
+    if (q < 0) {
+        q = 0;
+    } else if (QTRACKS <= q) {
+        q = QTRACKS-1;
+    }
+    this->quarterTrack = static_cast<std::uint8_t>(q);
+}
+
+void StepperMotor::moveCog() {
+    if (this->pendingPos >= 0) {
+        calculateTrack(calcDeltaPos(this->pos,this->pendingPos));
+        this->pos = this->pendingPos;
+    }
+}
+
+void StepperMotor::setMagnet(const std::uint8_t magnet, const bool on) {
+    const std::uint8_t mask = static_cast<std::uint8_t>(1u << magnet);
     if (on) {
         this->mags |= mask;
     } else {
         this->mags &= ~mask;
     }
 
-    const std::uint8_t oldQT = this->quarterTrack;
+    // set magnets (above), but delay actual movement of the stepper
+    // motor cog, to emulate force of inertia while trying to move it
+    // This allows Locksmith to write on the quarter-track, for example.
 
-    const char newPos = mapMagPos[this->mags];
-    char d = 0;
-    if (newPos >= 0) {
-        d = calcDeltaPos(this->pos,newPos);
-        this->pos = newPos;
-
-        // TODO: delay moving arm by a small amount
-        // For example, Locksmith, in order to write "quarter tracks" (i.e., T+.25 or T+.75), it positions
-        // to the correct track (by turning two adjacent magnets on), then turns them both off in rapid
-        // succession. In real life the arm doesn't move in such a case. In order to emulate that, we need
-        // to delay the arm move for a bit, to see if the magnets change in the meantime.
-        /*
-         * ARM: ph2 + [..*.] T$0D.00     +0.00
-         * ARM: ph3 + [..**] T$0D.25 --> +0.25
-         * switching from tmap[34] --> [35]
-         *
-         * ARM: ph3 - [..*.] T$0D.00 <-- -0.25   <-\
-         * switching from tmap[35] --> [34]      <--\-- this needs to get delayed
-         * ARM: ph2 - [....] T$0D.00     +0.00
-         */
-        this->quarterTrack += d;
-        if (this->quarterTrack < 0)
-            this->quarterTrack = 0;
-        else if (QTRACKS <= this->quarterTrack)
-            this->quarterTrack = QTRACKS-1;
-    }
-
-    const std::uint8_t newQT = this->quarterTrack;
-    const std::int8_t deltaQT = newQT - oldQT;
-
-    printf("ARM: ph%d %s [%c%c%c%c] T$%02X.%02d %s %+0.2f\n",
-       (std::uint8_t)magnet,
-       on ? "+" : "-",
-       (mags&1)?'*':'.',
-       (mags&2)?'*':'.',
-       (mags&4)?'*':'.',
-       (mags&8)?'*':'.',
-       this->quarterTrack / 4,
-       (this->quarterTrack % 4) * 25,
-       deltaQT>0 ? "-->" : deltaQT<0 ? "<--" : "   ",
-       (deltaQT % 4) / 4.0);
+    this->pendingPos = mapMagPos[this->mags];
+    this->pendingTicks = 1000; // about 1 millisecond
 }
+
+void StepperMotor::tick() {
+    if (this->pendingTicks) {
+        --this->pendingTicks;
+        if (!this->pendingTicks) {
+            moveCog();
+        }
+    }
+}
+
+
+
+// TODO fix logging (due to new delayed movement algorithm)
+//    const std::uint8_t oldQT = this->quarterTrack;
+//...
+//    const std::uint8_t newQT = this->quarterTrack;
+//    const std::int8_t deltaQT = newQT - oldQT;
+//    printf("ARM: ph%d %s [%c%c%c%c] T$%02X.%02d %s %+0.2f\n",
+//       (std::uint8_t)magnet,
+//       on ? "+" : "-",
+//       (mags&1)?'*':'.',
+//       (mags&2)?'*':'.',
+//       (mags&4)?'*':'.',
+//       (mags&8)?'*':'.',
+//       this->quarterTrack / 4,
+//       (this->quarterTrack % 4) * 25,
+//       deltaQT>0 ? "-->" : deltaQT<0 ? "<--" : "   ",
+//       (deltaQT % 4) / 4.0);

src/steppermotor.h

@@ -25,15 +25,21 @@ private:
     enum { QTRACKS = 160 };
     // quarter track: 0=t0, 1=t0.25, 2=t0.5, 3=t0.75, 4=t1, ... 140=t35.00 ... 159=t39.75
     // (see TMAP in WOZ2 file format spec)
-    std::int16_t quarterTrack;
+    std::uint8_t quarterTrack;
 
-    signed char pos; // 0 - 7
-    unsigned char mags;
+    std::int8_t pos;
+    std::uint8_t mags;
 
-    static signed char mapMagPos[];
+    std::int8_t pendingPos;
+    std::uint32_t pendingTicks;
 
-    static signed char calcDeltaPos(const unsigned char cur, const signed char next) {
-        signed char d = next-cur; // -7 to +7
+    static std::int8_t mapMagPos[];
+
+    void moveCog();
+    void calculateTrack(const std::int8_t delta);
+
+    static std::int8_t calcDeltaPos(const std::int8_t cur, const std::int8_t next) {
+        std::int8_t d = next-cur; // -7 to +7
 
         if (d == 4 || d == -4) {
             d = 0; // <--- TODO pick random direction?
@@ -50,13 +56,14 @@ public:
     StepperMotor();
     ~StepperMotor();
 
-    void setMagnet(const unsigned char magnet, const bool on);
-    unsigned char getTrack() {
-        return ((unsigned short)(this->quarterTrack)) >> 2;
+    void setMagnet(const std::uint8_t magnet, const bool on);
+    std::uint8_t getTrack() {
+        return this->quarterTrack >> 2;
     }
     std::uint8_t getQuarterTrack() {
         return this->quarterTrack;
     }
+    void tick();
 };
 
 #endif

src/wozfile.cpp

@@ -511,7 +511,7 @@ void WozFile::rotateOneBit(std::uint8_t currentQuarterTrack) {
     // previous, based on each track's length (tracks can be of
     // different lengths in the WOZ image).
     if (currentQuarterTrack != this->lastQuarterTrack) {
-        printf("switching from tmap[%02x] --> [%02x]\n", this->lastQuarterTrack, currentQuarterTrack);
+//        printf("switching from tmap[%02x] --> [%02x]\n", this->lastQuarterTrack, currentQuarterTrack);
         const double oldLen = this->trk_bits[this->tmap[this->lastQuarterTrack]];
         const double newLen = this->trk_bits[this->tmap[currentQuarterTrack]];
         const double ratio = newLen/oldLen;
@@ -640,8 +640,8 @@ void WozFile::reduceTracks() {
             break;
         }
     }
-    dumpTmap();
-    dumpTracks();
+//    dumpTmap();
+//    dumpTracks();
 }
 
 static std::uint16_t bytesForBits(const std::uint32_t c_bits) {