schulze/EENX15
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

function for getting RPM, checkpoint. sorta working

Browse Source
This commit is contained in:
Thefeli73 2021-04-14 01:55:49 +02:00
parent 2712a72b75
commit f9ebdc8ecd
1 changed files with 12 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
EENX15_LQR/EENX15_LQR.ino
View File

@ -55,6 +55,7 @@ int temp;
#define encoderA2 4 #define encoderA2 4
#define encoderB2 7 #define encoderB2 7
volatile int countA = 0; volatile int countA = 0;
volatile int countAold = 0;
volatile int countB = 0; volatile int countB = 0;
const int MotorPinA = 12; const int MotorPinA = 12;
@ -74,6 +75,8 @@ unsigned int rpmA;
unsigned int rpmB; unsigned int rpmB;
unsigned long timeoldA; unsigned long timeoldA;
unsigned long timeoldB; unsigned long timeoldB;
float rpm = 0;
float rps = 0;
void setup() { void setup() {
Wire.begin(); Wire.begin();
@ -116,6 +119,15 @@ void loop() {
printInfo(); printInfo();
} }
} }
void getSpeed() {
float alpha = 2.0 / (20.0 + 1.0); //alfa aka EMA length. 10 length = 20*20ms=0.2s
int ticks = countA-countAold;
float rpm_new = ticks*50*60/56.0; //ticks * 50 hz * 60s / ticks per rotation
float rps_new = ticks*50/8.91; //ticks * 50hz/ ticks per rad
rpm = rpm*(1-alpha)+rpm_new*alpha;
rps = rps*(1-alpha)+rps_new*alpha;
countAold = countA;
}
void printInfo(){ void printInfo(){
Serial.println(""); Serial.println("");
Serial.print("pitch Angle = "); Serial.println(angle_pitch_output); Serial.print("pitch Angle = "); Serial.println(angle_pitch_output);