Need help combining code


#1

Hey guys, I've written some pseudo code for my robot but I cannot figure out how to incorporate the coding of 2 separate dual ultrasonic sensor modules, that will be located in different areas on the robot. They're used for measuring the distance between the robot and the ceiling (or piece of furniture if <2 meters) and the distance measured will define the sequence of events to follow. Which would include either standing up by releasing a solenoid(if distance >2m), or rolling to a point where it can release the solenoid to stand up(constant checking of distance until >2m). The code also incorporate the running of 2 separate motors running at the same time without an axel. I've pasted the pseudo code below for a better understanding, but all advice and guidance is greatly appreciated :slight_smile:

B_label: when button is pushed;
A_label: sonar2 measures distance;
if(distance <2m
)
{
run wheel motors forward;
Sonar2 constantly measures distance; when serial monitor recognises >2m for sonar2;
(stop rolling);
//to stand guitar up
release solenoid;
standby until micro switch is hit;
//human has pushed guitar down
when switch is hit turn solenoid on;
loop A_label;

continue;
}

if (check microswitch is on)[on]
{
run wheel motors backwards;
Sonar1 constantly measures distance; when serial monitor recognises <2m for sonar1;
(stop rolling);}
else [not on]
standby;
loop B_label

in simpler terms:
// when button is pushed, sonar2 measures distance
// [distance check] If distance <2m run wheel motors forward
// Sonar2 constantly measures distance
// when serial monitor recognises >2m for sonar2, stop rolling
// release solenoid
[guitar stands itself up]
// standby until micro switch is hit (when human pushes guitar but down)
// When micro switch is hit, switch solenoid on
// loop from distance check
// If distance >2m, check solenoid is on.
// If solenoid is on, run wheel motors backwards
// Sonar1 constantly measures distance
// when serial monitor recognises >2m for sonar1, stop rolling
// standby and loop to button push [step one]