1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
| /*
|| NYCCNC HYPOCYCLOID DRIVE
|| -version 1
|| -author Alex Pinson
|| -8/6/2019
|| -Copyright Saunders Machine Works, LLC 2019
*/
#include <SabertoothSimplified.h>
#include <PID_v1.h> //PID library by Brett Beauregard
//Source for positional readings: https://github.com/BenTommyE/BenRotaryEncoder/blob/master/BenRotaryEncoder.ino
SabertoothSimplified ST; //Names the sabertooth object ST
volatile float temp, counter = 0; //This variable will increase or decrease depending on the rotation of encoder
int PPR = 400; //Equal to the number of ticks per revolution of your encoder
float pulsemSec = 0; //the amount of ticks the encoder is reading every millisecond
float RPM = 0; //RPM of the motor
float timeToRev = 0; //time in uSec per revolution of encoder
float timeSample1 = 0;
float timeSample2 = 0;
double setPoint; //desired value
float goalRPM = 1500;
double input; //sensor (RPM in my case)
double output; //action to be taken (Altering the PWM frequency in my case)
double kP = .8, kI = 1.1, kD = .0035; //PID tuners
PID rpmPID(&input, &output, &setPoint, kP, kI, kD, DIRECT);
void setup() {
setPoint = map(goalRPM, 0, 6500, 0, 127);
Serial.begin (38400);
rpmPID.SetOutputLimits(0, 80);
rpmPID.SetTunings(kP, kI, kD); //Defines which variables above are tuning variables
rpmPID.SetSampleTime(10);
rpmPID.SetMode(AUTOMATIC); //Turns on PID loop
ST.motor(2, 10);
pinMode(2, INPUT_PULLUP); //sets pin mode for pin 2
pinMode(3, INPUT_PULLUP); //sets pin mode for pin 3
//Setting up interrupt
//A rising pulse from encoder activated ai0(). AttachInterrupt 0 is DigitalPin nr 2 on most Arduinos.
attachInterrupt(0, ai0, RISING);
//B rising pulse from encoder activated ai1(). AttachInterrupt 1 is DigitalPin nr 3 on most Arduinos.
attachInterrupt(1, ai1, RISING);
}
void loop() {
RPM = (1/timeToRev) * 1000 * 1000 * 60; //conversion for uSec/rev to RPMs
input = map(RPM, 0, 6500, 0, 127);
rpmPID.Compute();
Serial.println(output);
ST.motor(2, output);
// Send the value of counter
if ( counter != temp ) { //if change is detected in the encoder, print the positional data values
//Serial.println ("Positional Data: ");
//Serial.println (counter);
temp = counter;
}
if ( counter >= PPR or counter <= -PPR) { //This if statement resets the counter every time the encoder does a full revolution, protecting from reset after memory becomes filled
counter = 0;
timeSample2 = micros();
timeToRev = timeSample2 - timeSample1;
//Serial.println ("timeToRev: ");
//Serial.println (timeToRev);
//Serial.println ("RPM: ");
Serial.println (RPM);
//input = map(RPM, 0, 3840, 0, 80);
timeSample1 = timeSample2;
//rpmPID.Compute();
//Serial.println(output);
//ST.motor(2, output);
}
//}
}
void ai0() {
// ai0 is activated if DigitalPin nr 2 is going from LOW to HIGH
// Check pin 3 to determine the direction
if (digitalRead(3) == LOW) {
counter++;
} else {
counter--;
}
}
void ai1() {
// ai0 is activated if DigitalPin nr 3 is going from LOW to HIGH
// Check with pin 2 to determine the direction
if (digitalRead(2) == LOW) {
counter--;
} else {
counter++;
}
} |
