1

我有一个机器人在跳舞，我试图让它在跳舞期间，如果机器人在任何时候撞到墙上（我假设是一个while循环），它会在液晶屏幕上显示一些东西，备份和然后回到原来的歌舞。我有一个舞蹈代码，但是我不确定如何在保险杠撞到它时将其合并，转身，然后返回到用于歌曲和舞蹈的开关盒运动任务。 。有什么建议么？？到目前为止，这是我的歌舞代码：

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

}
4

2 回答 2

1

有两个控制变量，状态和时间。状态保存了机器人正在做什么，并记录了它已经做了多长时间。有一个外部 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;
}
于 2013-03-23T22:50:37.707 回答
0

您可以修改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°，以保持相同的方向。希望你玩的开心。

于 2013-03-23T22:48:14.033 回答