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

encoder HAL interrupts for position (Net rotations)

Browse Source
This commit is contained in:
Thefeli73
2021-04-14 00:20:35 +02:00
parent 8585e34c73
commit 851cc8c46d
1 changed files with 44 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

53
EENX15_LQR/EENX15_LQR.ino
View File

@@ -45,6 +45,14 @@ long loop_timer;
int temp; int temp;
//motor stuff //motor stuff
#define encoderA1 2
#define encoderB1 3
#define encoderA2 4
#define encoderB2 7
volatile int countA = 0;
volatile int countB = 0;
const int MotorPinA = 12; const int MotorPinA = 12;
const int MotorSpeedA = 3; const int MotorSpeedA = 3;
const int MotorBrakeA = 9; const int MotorBrakeA = 9;
@@ -72,6 +80,13 @@ void setup() {
gyro_setup(); gyro_setup();
//motor //motor
pinMode(encoderA1, INPUT_PULLUP);
pinMode(encoderB1, INPUT_PULLUP);
pinMode(encoderA2, INPUT_PULLUP);
pinMode(encoderB2, INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(encoderA1), pulseA, RISING);
//attachInterrupt(digitalPinToInterrupt(encoderB1), pulseB, RISING);
pinMode(MotorPinA, OUTPUT); pinMode(MotorPinA, OUTPUT);
pinMode(MotorSpeedA, OUTPUT); pinMode(MotorSpeedA, OUTPUT);
pinMode(MotorBrakeA, OUTPUT); pinMode(MotorBrakeA, OUTPUT);
@@ -80,8 +95,6 @@ void setup() {
pinMode(MotorSpeedB, OUTPUT); pinMode(MotorSpeedB, OUTPUT);
pinMode(MotorBrakeB, OUTPUT); pinMode(MotorBrakeB, OUTPUT);
attachInterrupt(0, magnet_detectA, RISING);
//attachInterrupt(1, magnet_detectB, RISING);
half_revolutionsA = 0; half_revolutionsA = 0;
rpmA = 0; rpmA = 0;
timeoldA = 0; timeoldA = 0;
@@ -96,11 +109,15 @@ void loop() {
safe_angle = int(round(angle_pitch_output)); safe_angle = int(round(angle_pitch_output));
if(temp_loops>100){ if(temp_loops>250){
Serial.println(""); Serial.println("");
Serial.print(" pitch Angle = "); Serial.println(angle_pitch_output); Serial.print(" pitch Angle = "); Serial.println(angle_pitch_output);
Serial.print(" pitch Angle abs = "); Serial.println(abs(safe_angle)); Serial.print(" pitch Angle abs = "); Serial.println(abs(safe_angle));
Serial.print(" pitch Angle measured = "); Serial.println(angle_pitch); Serial.print(" pitch Angle measured = "); Serial.println(angle_pitch);
Serial.print("Position: "); Serial.println(countA);
//Serial.print("A: "); Serial.println(countA);
//Serial.print("B: "); Serial.println(countB);
temp_loops = 0; temp_loops = 0;
} }
else { else {
@@ -133,14 +150,28 @@ void loop() {
analogWrite(MotorSpeedA, speed); analogWrite(MotorSpeedA, speed);
} }
} }
int directionA(){
if(digitalRead(encoderA2) == HIGH){
return 1;
}
else{
return -1;
}
}
void magnet_detectA()//This function is called whenever a magnet/interrupt is detected by the arduino int directionB(){
{ if(digitalRead(encoderB2) == HIGH){
half_revolutionsA++; return 1;
Serial.println("detect");
} }
void magnet_detectB()//This function is called whenever a magnet/interrupt is detected by the arduino else{
{ return -1;
half_revolutionsB++; }
//Serial.println("detect"); }
void pulseA(){
countA += directionA();
}
void pulseB(){
countB += directionB();
} }