mcopy/ino/mcopy_projector_firmware/McopyProjector.cpp

322 lines
7.3 KiB
C++

/// Mcopy Projector Class
#include "McopyProjector.h"
McopyProjector::McopyProjector (AccelStepper takeup, AccelStepper feed,
uint8_t takeupSettingA, uint8_t takeupSettingB,
uint8_t feedSettingA, uint8_t feedSettingB,
uint8_t takeupEmitter, uint8_t takeupReceiver,
uint8_t feedEmitter, uint8_t feedReceiver,
uint8_t servoPin) {
_takeup = takeup;
_feed = feed;
_takeupSettingA = takeupSettingA;
_takeupSettingB = takeupSettingB;
_feedSettingA = feedSettingA;
_feedSettingB = feedSettingB;
_takeupEmitter = takeupEmitter;
_takeupReceiver = takeupReceiver;
_feedEmitter = feedEmitter;
_feedReceiver = feedReceiver;
_servoPin = servoPin;
}
void McopyProjector::begin () {
_takeup.setMaxSpeed(_speed);
_takeup.setSpeed(_speed);
_takeup.setAcceleration(1000.0);
_feed.setMaxSpeed(_speed);
_feed.setSpeed(_speed);
_feed.setAcceleration(1000.0);
pinMode(_takeupSettingA, OUTPUT);
pinMode(_takeupSettingB, OUTPUT);
pinMode(_feedSettingA, OUTPUT);
pinMode(_feedSettingB, OUTPUT);
pinMode(_takeupEmitter, OUTPUT);
pinMode(_feedEmitter, OUTPUT);
pinMode(_takeupReceiver, INPUT);
pinMode(_feedReceiver, INPUT);
//keep at 1 for now
setStepperMode(1);
_servo.attach(_servoPin);
_servo.write(_servoHome);
}
void McopyProjector::setDirection (bool dir) {
_dir = dir;
}
void McopyProjector::frame (bool dir) {
int16_t spf = _stepsPerFrame * _mode; //scaled
bool running = true;
if (dir != _dir) {
setDirection(dir);
}
int64_t takeupGoal = _takeup.currentPosition();
int64_t feedGoal = _feed.currentPosition();
takeupGoal += _dir ? spf : -spf;
feedGoal += _dir ? spf : -spf;
_takeup.moveTo(takeupGoal);
_feed.moveTo(feedGoal);
_running = true;
_servo.write(_servoAway);
delay(20);
while (running) {
if (_takeup.distanceToGo() == 0 && _feed.distanceToGo() == 0) {
running = false;
_posTakeup = takeupGoal;
_posFeed += feedGoal;
} else {
_takeup.run();
_feed.run();
}
}
delay(20);
_servo.write(_servoHome);
_running = false;
}
void McopyProjector::adjust(uint8_t motor, int64_t steps) {
uint64_t s = abs(steps);
//moveTo
if (motor == 0) {
_posTakeup += steps;
} else if (motor == 1) {
_posFeed += steps;
}
}
void McopyProjector::adjustBoth (int64_t steps) {
uint64_t s = abs(steps);
//steppers.stepBoth(s);
_posTakeup += steps;
_posFeed += steps;
}
void McopyProjector::loop () {
/*
if (_running) {
if (_takeup.distanceToGo() == 0 && _feed.distanceToGo() == 0) {
//frame done
_running = false;
_posTakeup += _dir ? _stepsPerFrame : -_stepsPerFrame;
_posFeed += _dir ? _stepsPerFrame : -_stepsPerFrame;
Serial.println(_count);
} else {
_takeup.run();
_feed.run();
_count++;
}
} else if (_adjusting) {
if (_takeup.distanceToGo() == 0 && _feed.distanceToGo() == 0) {
//adjustment done
_adjusting = false;
} else {
_takeup.run();
_feed.run();
}
}
*/
}
//https://wiki.iteadstudio.com/Arduino_Dual_Step_Motor_Driver_Shield
void McopyProjector::setStepperMode (uint8_t mode) {
_mode = mode;
switch (mode) {
case 1 :
digitalWrite(_takeupSettingA, LOW);
digitalWrite(_takeupSettingB, LOW);
digitalWrite(_feedSettingA, LOW);
digitalWrite(_feedSettingB, LOW);
break;
case 2 :
digitalWrite(_takeupSettingA, HIGH);
digitalWrite(_takeupSettingB, LOW);
digitalWrite(_feedSettingA, HIGH);
digitalWrite(_feedSettingB, LOW);
break;
case 4 :
digitalWrite(_takeupSettingA, LOW);
digitalWrite(_takeupSettingB, HIGH);
digitalWrite(_feedSettingA, LOW);
digitalWrite(_feedSettingB, HIGH);
break;
case 8 :
digitalWrite(_takeupSettingA, HIGH);
digitalWrite(_takeupSettingB, HIGH);
digitalWrite(_feedSettingA, HIGH);
digitalWrite(_feedSettingB, HIGH);
break;
}
}
void McopyProjector::home () {
uint16_t steps = _motorSteps * _mode;
uint16_t quarter = steps / 4;
uint16_t eighth = quarter / 2;
uint16_t takeupPeak = 0;
uint16_t feedPeak = 0;
uint16_t takeupOffset = 0;
uint16_t feedOffset = 0;
long takeupReading = 0.0;
long feedReading = 0.0;
emitters(true);
delay(10);
for (uint16_t i = 0; i < steps; i++) {
takeupReading = analogReadAccurateAverage(_takeupReceiver);
feedReading = analogReadAccurateAverage(_feedReceiver);
_takeupSamples[i] = takeupReading;
_feedSamples[i] = feedReading;
if (i < steps - 1) {
_takeup.move(1);
_feed.move(1);
_takeup.runToPosition();
_feed.runToPosition();
}
}
for (uint16_t i = 0; i < steps; i++) {
Serial.print(i);
Serial.print(", ");
Serial.print(_takeupSamples[i]);
Serial.print(", ");
Serial.println(_feedSamples[i]);
}
takeupPeak = findPeak(_takeupSamples, steps);
feedPeak = findPeak(_feedSamples, steps);
Serial.print(" takeup peak: ");
Serial.println(takeupPeak);
Serial.print(" feed peak: ");
Serial.println(feedPeak);
takeupOffset = abs(steps - takeupPeak);
feedOffset = abs(steps - feedPeak);
Serial.print("takeup offset: ");
Serial.println(takeupOffset);
Serial.print(" feed offset: ");
Serial.println(feedOffset);
if (takeupOffset > 0) {
for (uint16_t i = 0; i < takeupOffset; i++) {
_takeup.move(-1);
_takeup.runToPosition();
}
}
if (feedOffset > 0) {
for (uint16_t i = 0; i < feedOffset; i++) {
_feed.move(-1);
_feed.runToPosition();
}
}
for (uint16_t i = 0; i < eighth; i++) {
_takeup.move(-1);
_feed.move(-1);
_takeup.runToPosition();
_feed.runToPosition();
}
for (uint16_t i = 0; i < quarter; i++) {
takeupReading = analogReadAccurateAverage(_takeupReceiver);
feedReading = analogReadAccurateAverage(_feedReceiver);
_takeupSamples[i] = takeupReading;
_feedSamples[i] = feedReading;
if (i < steps - 1) {
_takeup.move(1);
_feed.move(1);
_takeup.runToPosition();
_feed.runToPosition();
}
}
emitters(false);
takeupPeak = findPeak(_takeupSamples, quarter);
feedPeak = findPeak(_feedSamples, quarter);
takeupOffset = abs(quarter - takeupPeak);
feedOffset = abs(quarter - feedPeak);
if (takeupOffset > 0) {
for (uint16_t i = 0; i < takeupOffset; i++) {
_takeup.move(-1);
_takeup.runToPosition();
}
}
if (feedOffset > 0) {
for (uint16_t i = 0; i < feedOffset; i++) {
_feed.move(-1);
_feed.runToPosition();
}
}
_posTakeup = 0;
_posFeed = 0;
}
long McopyProjector::readVcc() {
long result;
// Read 1.1V reference against AVcc
ADMUX = _BV(REFS0) | _BV(MUX3) | _BV(MUX2) | _BV(MUX1);
delay(2); // Wait for Vref to settle
ADCSRA |= _BV(ADSC); // Convert
while (bit_is_set(ADCSRA,ADSC));
result = ADCL;
result |= ADCH<<8;
result = 1125300L / result; // Back-calculate AVcc in mV
return result;
}
long McopyProjector::analogReadAccurate (uint8_t &pin) {
double Vcc = readVcc() / 1000.0;
double ADCValue = analogRead(pin);
return (ADCValue / 1024.0) * Vcc;
}
long McopyProjector::analogReadAccurateAverage (uint8_t &pin) {
uint8_t count = 3;
double sum = 0.0;
for (uint8_t i = 0; i < count; i++) {
sum += analogReadAccurate(pin);
delay(1);
}
return sum / (double) count;
}
uint16_t McopyProjector::findPeak(long (&arr)[200], uint16_t &steps) {
uint16_t maxI = 0;
long max = 0;
for (uint16_t i = 0; i < steps; i++) {
if (arr[i] > max) {
maxI = i;
max = arr[i];
}
}
return maxI;
}
void McopyProjector::emitters (bool enabled) {
digitalWrite(_takeupEmitter, enabled ? HIGH : LOW);
digitalWrite(_feedEmitter, enabled ? HIGH : LOW);
}