Combining Continuous Loop in Switch-Case Function

Question

I have a robot performing a dance, and I am trying to make it so that during the dance, if at any point the robot runs into a wall (a while loop im assuming), it will display something on the lcd screen, backup and then go back to its original song and dance. I have a code for the dance, however I'm not sure how to incorporate the function for whenever the bumpers hit it backs up, turns around, and then returns to the switch-case motion task as used for the song and dance...any suggestions?? here is my song and dance code so far:

//FOURTH OCTAVE NOTE FREQUENCIES
#define C4  261 //  C   note frequency
#define Cs4 277 //  C   sharp note frequency
#define D4  294 //  D   note frequency
#define Ds4 311 //  D   sharp note frequency
#define E4  329 //  E   note frequency
#define F4  349 //  F   note frequency
#define Fs4 370 //  F   sharp note frequency
#define G4  392 //  G   note frequency
#define Gs4 415 //  G   sharp note frequency
 #define    A4  440 //  A   note frequency
#define As4 466 //  A   sharp note frequency
#define B4  494 //  B   note frequency
//FIFTH OCTAVE NOTE FREQUENCIES
#define C5  523 //  C   note frequency
#define Cs5 554 //  C   sharp note frequency
#define D5  587 //  D   note frequency
#define Ds5 622 //  D   sharp note frequency
#define E5  659 //  E   note frequency
#define F5  698 //  F   note frequency
#define Fs5 740 //  F   sharp note frequency
#define G5  784 //  G   note frequency
#define Gs5 831 //  G   sharp note frequency
#define A5  880 //  A   note frequency
#define As5 932 //  A   sharp note frequency
#define B5  988 //  B   note frequency
//SIXTH OCTAVE NOTE FREQUENCIES
#define C6  1046    //  note frequency
#define Cs6 1109    //  sharp note frequency
#define D6  1175    //  note frequency
#define Ds6 1245    //  sharp note frequency
#define E6  1319    //  note frequency
#define F6  1397    //  note frequency
#define Fs6 1480    //  sharp note frequency
#define G6  1568    //  note frequency
#define Gs6 1661    //  sharp note frequency
#define A6  1760    //  note frequency
#define As6 1865    //  sharp note frequency
#define B6  1976    //  note frequency
//SEVENTH OCTAVE NOTE FREQUENCIES
#define C7  2093    //  note frequency
#define Cs7 2217    //  sharp note frequency
#define D7  2349    //  note frequency
#define Ds7 2489    //  sharp note frequency
#define E7  2637    //  note frequency
#define F7  2794    //  note frequency
#define Fs7 2960    //  sharp note frequency
#define G7  3136    //  note frequency
#define Gs7 3322    //  sharp note frequency
#define A7  3520    //  note frequency
#define As7 3729    //  sharp note frequency
#define B7  3951    //  note frequency
//EIGHTH OCTAVE NOTE FREQUENCIES
#define C8  4186    //  note frequency
#define Cs8 4434    //  sharp note frequency
#define D8  4699    //  note frequency
#define Ds8 4978    //  sharp note frequency
#define E8  5274    //  note frequency
#define F8  5588    //  note frequency
#define Fs8 5920    //  sharp note frequency
#define G8  6272    //  note frequency
#define Gs8 6645    //  sharp note frequency
#define A8  7040    //  note frequency
#define As8 7459    //  sharp note frequency
#define B8  7902    //  note frequency

#define P       0 //note frequency (a rest note)


//NOTE DURATION FACTORS (THEY MULTIPLY THE WHOLE NOTE DURATION)
#define N1   1.0     //whole note
#define N2   0.5     //half note
#define N2h  0.75    //dotted half note
#define N4   0.25    //quarter note
#define N4h  0.375   //dotted quarter note
#define N8   0.125   //eighth note
#define N8h  0.1875  //dotted eighth note
#define N16  0.0625  //sixteenth note
#define N16h     0.09375 //dotted   sixteenth note
#define N32  0.03125     //thirty-secondth note
#define N32h     0.046875 //dotted thirty-secondth note

//declare and initialize arrays of note frequencies and note durations
int NotesF[] = {    G6,P,G6,P,As6,P,C7,P,
        G6,P,G6,P,F6,P,Fs6,P,
        G6,P,G6,P,As6,P,C7,P,
                G6,P,G6,P,F6,P,Fs6,P,
                As6,G6,D6,P,As6,G6,Cs6,P,
                As6,G6,C6,P,As5,C6,0};
float NotesD[] = {  N8,N4,N8,N4,N8,N8,N8,N8,
                N8,N4,N8,N4,N8,N8,N8,N8,
                N8,N4,N8,N4,N8,N8,N8,N8,
                N8,N4,N8,N4,N8,N8,N8,N8,
                N8,N8,N1,N16,N8,N8,N1,N16,
                N8,N8,N1,N8,N8,N8,0};
int index;
task motion();
void Rotate(int speed, int msecs);
void Drive(int speed, int msecs);

task main()
{
int freq;
float dur;
int beats; //beats per minutes (one beat = one quater-note)
int wholenote, quarternote;//duration of whole and quarter note in milliseconds
int tics; //duration in 10-millisecond tics needed by PlayTone( ) function
beats = 200;//tempo in beats per minute
quarternote = 1000*(60/(float)beats); //how many milliseconds
wholenote = 4*quarternote; //how many milliseconds

bPlaySounds = true;
nVolume = 2;

StartTask(motion);

//turn off voltage regulation for motor[0] (headlight is not a motor!)
nMotorPIDSpeedCtrl[0]=   mtrNoReg;

index=0;
do
{
    freq = NotesF[index];
    dur = NotesD[index];
    tics = (int)((wholenote*dur)/10);//how many 10 msec "tics"
    PlayTone(freq, tics);
    if(freq==0)
        motor[0] = 0;//headlight OFF for rest notes
    else
        motor[0] = 100;//headlight ON for all other notes
    wait10Msec(tics);
    index = index+1;
} 
while (NotesD[index] != 0); //duration-0 note marks end of song

StopTask(motion);
}

task motion()
{
int oldindex=-1; //initialize so first one won't match

//turn on speed regulation for accurate tracking
//between left and right wheel rotations
nMotorPIDSpeedCtrl[1] = mtrSpeedReg;
nMotorPIDSpeedCtrl[2] = mtrSpeedReg;

while(true)
{
    if(index!=oldindex)//has note changed in main()?
    {
        oldindex = index;
        switch(index)
        {
            case 0:
                Drive(500,500);
                break;
            case 8:
                Rotate(-75,500);
                break;
            case 16:
                Rotate(-75,500);
                break;
            case 24:
                Rotate(75,500);
                break;
            case 32:
                Rotate(75,500);
                break;
            case 36:
                Drive(500,500);
                break;
            case 40:
                Rotate(75,500);
                break;
            case 48:
                Rotate(75,500);
                break;
            case 56:
                Rotate(-75,500);
                break;
            case 64:
                Rotate(-75,500);
                break;
            case 72:
                Rotate(-500,200);
                break;
            case 80:
                Rotate(-500,200);
        }
    }
    else
        wait10Msec(5);
}

}

void Rotate(int speed, int msecs)
{
//set speeds
motor[1] = +speed;
motor[2] = -speed;
//wait for needed rotation time
wait1Msec(msecs);
//set motors back to 0 speed
motor[1] = 0;
motor[2] = 0;

}

void Drive(int speed, int msecs)
{
//set speeds
motor[1] = +speed;
motor[2] = +speed;
//wait for needed rotation time
wait1Msec(msecs);
//set motors back to 0 speed
motor[1] = 0;
motor[2] = 0;

}

Answer 1

Have two control variables, state, and time. The state holds what the robot is doing, and timer how long it has been doing it for. Have an outer while loop, and within that, every iteration, check the state of the robot.

Here is an example of how it would work.

int state=1;
int timer=0;
while (true) {
    if (state==0) {
        //Walk away from wall
        motor[0]=-30;
        if (timer>2000) {
            //State change
            state=1;
            timer=0;
        }
    }
    if (state==1) {
        //Dance
        switch(timer) {
            case 0:
            motor[1]=40;
            break;
            case 800:
            motor[0]=8;
            break;
            //rest of dance code.
        }
        if (has_hit_wall()) {
            //State change
            state=0;
            timer=0;
        }
    }
    wait1Msec(10);
    timer+=1;
}

Answer 2

You can modify the Drive() method to check the bumper sensor. If a bumper hits, then you have to reverse for some time. So instead of wait1Msec(msec), change to wait1Msec(1) and use a loop with polling of the bumper. I would assume that the Rotate() method can not cause a collision, but this depends on the geometry of your robot.

For example,

int bumper_hit = 0; /* "flag" to monitor in main() to stop song. */
void Drive(int speed, int msecs)
{
  int i;
  //set speeds
  motor[1] = +speed;
  motor[2] = +speed;
  //wait for needed drive time, while checking bumper.
  for(i = 0; i < msecs; i++) {
           wait1Msec(1);
           if(bumper()) {
             bumper_hit = 1;
             motor[1] = -speed;
             motor[2] = -speed;
             wait1Msec(msec); /* Maybe longer? */
             bumper_hit = 0;
             motor[1] = +speed; /* if you want to continue with this portion */
             motor[2] = +speed;
             /* break; */       /* if you want to stop the forward motion */
           }
  }
  //set motors back to 0 speed
  motor[1] = 0;
  motor[2] = 0;
}

If the motor doesn't support reverse motion, then you can rotate 180°, drive forward and the rotate 180° again to keep the same orientation. I hope you have fun.