[Programming] Servo control code

Stage 1: Basic function

Goal: Create a code that allows servo to rotate certain angel for certain pressure

Idea: Use “Level model” to rotate the servo
         Assume that we allow servo to rotate each 30 degree for each 1000 Pa change of pressure
         We have:

 

 

 

 

 

Level = (current pressure – initial pressure) / pressurePerLevel

                               and then we can let servo rotate: Level * AngelPerLevel degree
Solution:

int floors=(bmp.readPressure() - initialPressure)/PPerFloor;
if(floors<lastFloor)
{
myservo.write(AngelPerFloor*lastFloor);
}
else
{
myservo.write(AngelPerFloor*floors);
lastFloor=floors;
}
(codes from “main_code 5.1_debug swtich”)

P.S.

          In the code: “floors” refers to levels
          To avoid the servo rotating back to previous level, I used variabe “Lastfloors” to record the last level. Only when the current level is greater than last floor, we change the degree that servo should rotate.


Stage 2: Non- uniform function

Goal: Becasue the change of pressuren and the change angel of the servo are not linear functions,      each point in the function of “pressure vs servo” should be executed separately

Idea: To avoid the servo rotating back to previous level, we used variabe “levelflag” here to record the last level.

Solution:

long gaugepressure= bmp.readPressure()- initialPressure;
if(gaugepressure>level1 && gaugepressure<level2 && levelflag < 2)
{
myservo.write(angle1);
if(levelflag<1)
levelflag=1;
}
if(gaugepressure>level2 && gaugepressure<level3 && levelflag < 3)
{
myservo.write(angle2);
if(levelflag<2)
levelflag=2;
}
if(gaugepressure>level3 && gaugepressure<level4 && levelflag < 4)
{
myservo.write(angle3);
If(levelflag<3)
levelflag=3;
}
if(gaugepressure>level4 && gaugepressure<level5 && levelflag < 5)

{

myservo.write(angle4);
if(levelflag<4)
levelflag=4;
}
if(gaugepressure>level5)
{
myservo.write(angle5);
if(levelflag<5)
levelflag=5;
}
(codes from “main_code_5.3_servo control updated”)

Stage 3: Mason's version

Goal:

First rotate: when the rocket is launched
Second rotate: reach 3 miles or 90 seconds
Third rotate: reach 5 miles or 3 mins
Fourth rotate: when it's back to 5 miles or 6 mins
Fifth rotate: 15 mins

Trival goal:

First rotate: reach Lv1 or 30 seconds

Second rotate: reach Lv2 or 90 seconds
Third rotate: reach Lv3 or 180 seconds
Fourth rotate: reach Lv4 or 360seconds
Fifth rotate: reach Lv5 or 900 seconds

Solution:

if((gaugepressure>level1 && gaugepressure<level2 && levelflag < 2) || (millis() > 30000 && millis() < 90000))
{
myservo.write(angle1);
if(levelflag<1)
levelflag=1;
}
if((gaugepressure>level2 && gaugepressure<level3 && levelflag < 3) || (millis() > 90000 && millis() < 180000))
{
myservo.write(angle2);
if(levelflag<2)
levelflag=2;
}
if((gaugepressure>level3 && gaugepressure<level4 && levelflag < 4)|| (millis() > 180000 && millis() < 360000))
{
myservo.write(angle3);
if(levelflag<3)
levelflag=3;
}
if((gaugepressure>level4 && gaugepressure<level5 && levelflag < 5) || (millis() > 360000 && millis() < 900000))
{
myservo.write(angle4);
if(levelflag<4)
levelflag=4;
}
if((gaugepressure>level5)|| (millis() > 900000 ))
{
myservo.write(angle5);
if(levelflag<5)
levelflag=5;
}

(codes from “main_code_5.5”)

Limitation: However, there's a serious bug in this code!

When it reaches Lv1 and the time goes into the interval between 90 sec and 180 sec, the program will go into both the first and the second “if”. In other world, the servo will rotate angel1 and angel2 almost at the same time. Thus, the servo will not work!

 

Stage 4: Modified version

Goal: Solve the bug in stage 3.

Idea: check the level every time before allowing the servo rotate

Solution:

if((gaugepressure<level1 && gaugepressure>level2 && levelflag < 2) || (millis() > 30000 && millis() < 90000)) //if the new floor is lower than the last floor, keep angel the servo has moved
{
if(levelflag==1)
myservo.write(angle1);
if(levelflag<1)
levelflag=1;
}
if((gaugepressure<level2 && gaugepressure>level3 && levelflag < 3) || (millis() > 90000 && millis() < 180000)) //if the new floor is lower than the last floor, keep angel the servo has moved
{
if(levelflag==2)
myservo.write(angle2);
if(levelflag<2)
levelflag=2;
}
if((gaugepressure<level3 && gaugepressure>level4 && levelflag < 4)|| (millis() > 180000 && millis() < 360000)) //if the new floor is lower than the last floor, keep angel the servo has moved
{
if(levelflag==3)
myservo.write(angle3);
if(levelflag<3)
levelflag=3;
}
if((gaugepressure<level4 && gaugepressure>level5 && levelflag < 5) || (millis() > 360000 && millis() < 900000)) //if the new floor is lower than the last floor, keep angel the servo has moved
{
if(levelflag==4)
myservo.write(angle4);
if(levelflag<4)
levelflag=4;
}
if((gaugepressure<level5)|| (millis() > 900000 )) //if the new floor is lower than the last floor, keep angel the servo has moved
{
if(levelflag==5)
myservo.write(angle5);
if(levelflag<5)
levelflag=5;
}
(codes from “main_code_5.6_modified servo control with pressure”)

Conclusion: Hopefully, it will work well in the next test that getting a function of pressure and the real altitude.