From 8ff8852e8b819646222cdb075388605bcbc8c4d9 Mon Sep 17 00:00:00 2001 From: Christopher Mosher Date: Wed, 2 Jan 2019 19:15:13 -0500 Subject: [PATCH] emulate stepper motor cog movement inertia --- src/diskcontroller.cpp | 3 ++ src/steppermotor.cpp | 113 ++++++++++++++++++++++------------------- src/steppermotor.h | 25 +++++---- src/wozfile.cpp | 6 +-- 4 files changed, 82 insertions(+), 65 deletions(-) diff --git a/src/diskcontroller.cpp b/src/diskcontroller.cpp index b7fdfff..ee3c2a4 100644 --- a/src/diskcontroller.cpp +++ b/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; diff --git a/src/steppermotor.cpp b/src/steppermotor.cpp index 621a755..654e7cd 100644 --- a/src/steppermotor.cpp +++ b/src/steppermotor.cpp @@ -1,6 +1,7 @@ /* epple2 - Copyright (C) 2008 by Christopher A. Mosher + + Copyright © 2008–2018, Christopher Alan Mosher, Shelton, CT, USA. 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(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(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); diff --git a/src/steppermotor.h b/src/steppermotor.h index 11b7d51..0da0765 100644 --- a/src/steppermotor.h +++ b/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 diff --git a/src/wozfile.cpp b/src/wozfile.cpp index 7ab98b1..0e74f6f 100644 --- a/src/wozfile.cpp +++ b/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) {