Объединение непрерывной петли в функции переключателя

Question

У меня есть робот, исполняющий танец, и я стараюсь сделать так, чтобы во время танца, если в какой-то момент робот врезался в стену (в то время, он отобразит что-то на экране lcd, резервную копию, а затем вернется к своей оригинальной песне и танцу. У меня есть код для танца, однако я не уверен, как включить функцию, когда бамперы попадают в нее, она поворачивается, а затем возвращается к задаче перемещения в случае переключения, используемой для песни и танца. .какие-либо предложения?? вот моя песня и танец код до сих пор:

//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

Вы можете изменить метод Drive(), чтобы проверить датчик бампера. Если ударит бампер, тогда вы должны вернуться в течение некоторого времени. Поэтому вместо wait1Msec(msec) перейдите на wait1Msec(1) и используйте петлю с опросом бампера . Я бы предположил, что метод Rotate() не может вызвать столкновение, но это зависит от геометрии вашего робота.

Например,

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; 
} 

Если двигатель не поддерживает обратное движения, то можно повернуть на 180 °, проходит вперед и поворот на 180 ° снова, чтобы сохранить ту же ориентацию. Я надеюсь тебе весело.

Answer 2

Имеют две управляющие переменные, состояние и время. Государство держит то, что делает робот, и таймер, как долго он это делает. Имейте внешний цикл while, и в этом, на каждой итерации, проверьте состояние робота.

Вот пример того, как это будет работать.

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; 
}