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

use speed var, set variable for safe angle for simplicity

Browse Source
This commit is contained in:
Thefeli73 2021-04-13 22:09:37 +02:00
parent f12389b570
commit 3ba28c950a
1 changed files with 16 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
EENX15_LQR/EENX15_LQR.ino
View File

@ -17,8 +17,10 @@ float angularSpeed = 0;
float motorAngularPosition = 0; float motorAngularPosition = 0;
/** Motor's angular speed */ /** Motor's angular speed */
float motorAngularSpeed = 0; float motorAngularSpeed = 0;
/** PWM signal applied to the motor's driver 400 is 100% cicle and -400 100% but inverse direction */
/** PWM signal applied to the motor's driver 255 is 100% */
int32_t speed; int32_t speed;
int safe_angle;
//gyro stuff //gyro stuff
@ -92,21 +94,21 @@ void setup() {
void loop() { void loop() {
gyro_loop(); gyro_loop();
safe_angle = int(round(angle_pitch_output));
if(temp_loops>100){ if(temp_loops>100){
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(int(round(angle_pitch_output)))); 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);
temp_loops = 0; temp_loops = 0;
} }
else { else {
temp_loops++; temp_loops++;
} }
if(angle_pitch_output!=0 && (abs(angle_pitch_output))<50 ){ if(safe_angle!=0 && safe_angle<50 ){
digitalWrite(MotorBrakeB, LOW); if(safe_angle<0){
digitalWrite(MotorBrakeA, LOW);
if(angle_pitch_output<0){
digitalWrite(MotorPinB, CW); digitalWrite(MotorPinB, CW);
digitalWrite(MotorPinA, CCW); digitalWrite(MotorPinA, CCW);
} }
@ -114,20 +116,15 @@ void loop() {
digitalWrite(MotorPinB, CCW); digitalWrite(MotorPinB, CCW);
digitalWrite(MotorPinA, CW); digitalWrite(MotorPinA, CW);
} }
if((abs(int(angle_pitch_output))*8 + 30)<250){ speed = 30 + 8*abs(safe_angle);
analogWrite(MotorSpeedB,30 + 8*abs(int(round(angle_pitch_output)))); //Wheel close to connections speed = constrain(speed, 0, 250);
analogWrite(MotorSpeedA,30 + 8*abs(int(round(angle_pitch_output)))); //First experiment wheel analogWrite(MotorSpeedB, speed); //Wheel close to connections
} analogWrite(MotorSpeedA, speed); //First experiment wheel
else{
analogWrite(MotorSpeedB,250); //Wheel close to connections
analogWrite(MotorSpeedA,250); //First experiment wheel
}
} }
else{ else{
digitalWrite(MotorBrakeB, HIGH); speed = 0;
digitalWrite(MotorBrakeA, HIGH); analogWrite(MotorSpeedB, speed);
analogWrite(MotorSpeedB,0); analogWrite(MotorSpeedA, speed);
analogWrite(MotorSpeedA,0);
} }
} }