contact_printer/ino/demos/motor_encoder/motor_encoder.ino

171 lines
4.4 KiB
Arduino
Raw Normal View History

/***
*
* 100RPM 12VDC Worm Gear Motor w/ encoder
* SKU - JGY-370
* DC12V100RPM - SKU-GS00127-05
*
* Gear ratio: 40:1(?) 60:1(?)
* 11 PPR Encoder - (!)
*
* RedMotor power + (exchange can control rotating and reversing)
* BlackCoding power- negative (3.3-5V) polarity cannot be wrong
* YellowSignal feedback
* GreenSignal feedback
* BlueCoding power + positive(3.3-5V)polarity cannot be wrong
* WhiteMotor power - (exchange can control rotating and
*
***/
#include <util/atomic.h> // For the ATOMIC_BLOCK macro
#define ENCA 2 // YELLOW
#define ENCB 3 // WHITE
#define MOTORA 10
#define MOTORB 11
volatile int32_t posi = 0; // specify posi as volatile
const long maxTime = 100000;
const int32_t maxRotations = 10;
const int ppr = 11;
const float ratio = 187.0 / 3.0;
const int maxPulses = (int) round((float) ppr * ratio);
const int speed = 255;
const int framesPerRotation = 18;
const float pulsesPerFrame = (float) maxPulses / (float) framesPerRotation;
volatile long start = 0;
volatile long total;
volatile bool done = false;
volatile bool stop = false;
volatile int32_t incoming;
volatile float rpm;
volatile float fps;
volatile float fpsMax = -1.0;
volatile float fpsMin = 100000.0;
volatile int32_t rotations = 0;
volatile int32_t lastRotationPosition = 0;
volatile int32_t lastFramePosition = 0;
volatile int32_t frames = 0;
float calculateFPS (long timeLength, int frames) {
return 1000.0 / ((float) timeLength / (float) frames);
}
float calculateRPM (long rotationLength) {
return 60000.0 / (float) (rotationLength);
}
void setup() {
Serial.begin(57600);
Serial.flush();
pinMode(ENCA, INPUT);
pinMode(ENCB, INPUT);
pinMode(MOTORA, OUTPUT);
pinMode(MOTORB, OUTPUT);
attachInterrupt(digitalPinToInterrupt(ENCA), readEncoder,RISING);
Serial.println("Connected");
Serial.print("PPR: ");
Serial.println(ppr);
Serial.print("Ratio: ");
Serial.println(ratio);
Serial.print("Pulses: ");
Serial.println(maxPulses);
Serial.print("Frames per Rotation: ");
Serial.println(framesPerRotation);
Serial.print("Pulses per Frame: ");
Serial.println(pulsesPerFrame);
}
void loop() {
int32_t pos;
// Read the position in an atomic block to avoid a potential
// misread if the interrupt coincides with this code running
// see: https://www.arduino.cc/reference/en/language/variables/variable-scope-qualifiers/volatile/
ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
pos = posi;
}
rotations = (int) floor((float) pos / (float) maxPulses);
2024-01-22 16:42:21 +00:00
if (rotations != lastRotationPosition) {
lastRotationPosition = rotations;
frames = rotations * framesPerRotation;
Serial.print("Rotations: ");
Serial.print(rotations);
Serial.print(" = ");
Serial.println(millis() - start);
}
frames = (int) floor((float) pos / pulsesPerFrame);
if (frames != lastFramePosition) {
lastFramePosition = frames;
total = millis() - start;
fps = calculateFPS(total, frames);
if (fps < 10000.0 && fps > fpsMax) {
fpsMax = fps;
}
if (fps < fpsMin) {
fpsMin = fps;
}
//Serial.print("Frames: ");
//Serial.print(frames);
//Serial.print(" = ");
//Serial.println(total);
//Serial.println(fps);
//Serial.print("@");
//Serial.println(pos);
}
if (abs(pos) >= maxPulses * maxRotations) {
stop = true;
}
if (start == -1) {
delay(1000);
analogWrite(MOTORA, speed);
digitalWrite(MOTORB, LOW);
start = millis();
rotations = 0;
} else if (stop && !done) {
digitalWrite(MOTORA, LOW);
digitalWrite(MOTORB, LOW);
//Serial.print("Final: ");
//Serial.println(pos);
Serial.print("Time: ");
total = millis() - start;
Serial.println(total);
rpm = calculateRPM(total) * (float) (rotations + 1);
2024-01-22 16:40:10 +00:00
fps = calculateFPS(total, frames);
Serial.print("RPM: ");
Serial.println(rpm);
Serial.print("FPS: ");
Serial.println(fps);
Serial.print(" Min: ");
Serial.println(fpsMin);
Serial.print(" Max: ");
Serial.println(fpsMax);
Serial.print("Rotations: ");
Serial.println(rotations);
Serial.print("Frames: ");
Serial.println(frames);
done = true;
}
if (Serial.available() > 0) {
incoming = Serial.read();
start = -1;
posi = 0;
done = false;
stop = false;
}
}
void readEncoder(){
int b = digitalRead(ENCB);
if(b > 0){
posi++;
}
else{
posi--;
}
}