平衡segway機(jī)器人開源項(xiàng)目

描述

“Segbot”是 UIUC ME 461 級制造的平衡 segway 機(jī)器人。segbot 由 Dan Block (d-block@illinois.edu) 教授設(shè)計的電路板和 F28379D 微控制器組成，該微控制器是德州儀器 C2000 系列的一部分。該項(xiàng)目的目標(biāo)是讓 segbot 播放歌曲和舞蹈以響應(yīng)檢測到不同的音符。

音符檢測

音符檢測是使用模數(shù)轉(zhuǎn)換完成的，將從麥克風(fēng)接收到的模擬信號轉(zhuǎn)換為數(shù)字值。沒有占空比輸出的脈寬調(diào)制被用作定時器，以 10 kHz 的采樣率觸發(fā) ADC 中斷。為了識別不同的頻率，使用Goertzel 算法一次對 1000 個 ADC 值進(jìn)行離散傅里葉變換。如果算法的輸出超過某個閾值，則檢測到音符。閾值是一個稍微隨意的值，調(diào)整為在適當(dāng)?shù)臅r間做出響應(yīng)，這意味著如果由于意外事件（例如落筆）而出現(xiàn)頻率，則不會檢測到音符。

// DFT with Goertzel Algorithm
float goertzel_mag(int numSamples,int TARGET_FREQUENCY,int SAMPLING_RATE, float* data)
{
    int     k,i;
    float   floatnumSamples;
    float   omega,sine,cosine,coeff,q0,q1,q2,magnitude,real,imag;

    float   scalingFactor = numSamples / 2.0;

    floatnumSamples = (float) numSamples;
    k = (int) (0.5 + ((floatnumSamples * TARGET_FREQUENCY) / SAMPLING_RATE));
    omega = (2.0 * PI * k) / floatnumSamples;
    sine = sin(omega);
    cosine = cos(omega);
    coeff = 2.0 * cosine;
    q0=0;
    q1=0;
    q2=0;

    for(i=0; i
    {
        q0 = coeff * q1 - q2 + data[i];
        q2 = q1;
        q1 = q0;
    }

    // calculate the real and imaginary results
    // scaling appropriately
    real = (q1 - q2 * cosine) / scalingFactor;
    imag = (q2 * sine) / scalingFactor;

    magnitude = sqrtf(real*real + imag*imag);
    return magnitude;
}

乒乓緩沖器

實(shí)現(xiàn)了一個乒乓緩沖器版本，以將 ADC 讀數(shù)存儲在單獨(dú)的陣列中，這樣一個可以收集數(shù)據(jù)，而另一個可以在 Goertzel 函數(shù)中執(zhí)行計算。使用了三個這樣的數(shù)組，以便 Goertzel 算法在檢查對應(yīng)于三種不同歌曲和舞蹈的三個單獨(dú)音符之間交替。當(dāng)檢測到其中一個目標(biāo)頻率時，它會發(fā)出歌舞開始的信號。

//Use three-part "Ping-Pong" Buffer in ADC interrupt
//Ping
    if(PingPong == 0){
        adcb_arrayPing[adcbcount] = adcb0result; // add ADC reading to array
        if(adcbcount == (n_samples-1)){
            adcbcount = -1; //incremented to 0 at end of interrupt
            RunPing = 1; //check in while loop
            PingPong = 1; //switch to Pong buffer
        }
    }
    //Pong
    if(PingPong == 1){
        adcb_arrayPong[adcbcount] = adcb0result;
        if(adcbcount == (n_samples-1)){
            adcbcount = -1; //incremented to 0 at end of interrupt
            RunPong = 1;
            PingPong = 2; //switch to Dong buffer
        }
    }
    //Dong
    if(PingPong == 2){
        adcb_arrayDong[adcbcount] = adcb0result;
        if(adcbcount == (n_samples-1)){
            adcbcount = -1; //incremented to 0 at end of interrupt
            RunDong = 1;
            PingPong = 0; //switch to Ping buffer
        }
    }

// IDLE loop. Use to pass arrays through Goertzel fxn and check for note detection
    while(1)
    {
        //after n samples, pass data through Goertzel fxn
        // use Ping Pong buffer

        if(RunPing == 1){
            goer_result = goertzel_mag(n_samples,NOTE,sampling_rate, adcb_arrayPing);
            RunPing = 0;
            if(goer_result > thresh){
                note_detected = 1;
            }
        }
        if(RunPong == 1){
            goer_result2 = goertzel_mag(n_samples,NOTE2,sampling_rate, adcb_arrayPong);
            RunPong = 0;
            if(goer_result2 > thresh){
                note2_detected = 1;
            }
        }
        if(RunDong == 1){
            goer_result3 = goertzel_mag(n_samples,NOTE3,sampling_rate, adcb_arrayDong);
            RunDong = 0;
            if(goer_result3 > thresh){
                note3_detected = 1;
            }
        }

歌舞

為了讓 segbot 播放歌曲，蜂鳴器由脈沖寬度調(diào)制控制。三個 CPU 定時器中斷中的每一個都被設(shè)置為不同的周期以對應(yīng)歌曲的節(jié)奏。歌曲的音符組合成一個數(shù)組，用來改變PWM的周期，在檢測到第一個音符的情況下，每次發(fā)生定時器中斷時，蜂鳴器都會播放相應(yīng)的音符。播放的歌曲是對披頭士的“Hey Jude”、夏奇拉的“Hips Don't Lie”和海灘男孩的“Little Saint Nick”的演繹。為了增強(qiáng)“小圣尼克”的表現(xiàn)，增加了一個鈴鐺，并結(jié)合 RC 伺服電機(jī)使用 PWM，在歌曲的不同部分敲擊表面。

讓 segbot 平衡是在此項(xiàng)目中完成的，再次使用 PWM 觸發(fā) ADC，其中值通過 SPI 寫入，以便從 MPU-9250 讀取加速度和陀螺儀值。實(shí)施卡爾曼濾波器以在將值發(fā)送到平衡 segbot 的控制律之前對其進(jìn)行過濾。與播放歌曲類似，為了讓 segbot 跳舞，為轉(zhuǎn)彎速率和向前/向后偏移創(chuàng)建了一個數(shù)組。當(dāng)檢測到相應(yīng)的音符時，在 CPU 定時器中斷中執(zhí)行舞蹈。

//Sing and dance when the Goertzel value exceeds threshold
if(note_detected == 1){

        GPIO_SetupPinMux(16, GPIO_MUX_CPU1, 5);// set up buzzer

        if (numtimer1calls < songsize){
            //play the song
            if(song[numtimer1calls]==0){
                GpioDataRegs.GPACLEAR.bit.GPIO16 = 1; // ground the buzzer
            }else{
                EPwm9Regs.TBPRD = (int)(3125000/song[numtimer1calls]/2);
            }
            //dance
            FwdBkOffset = fwddance[numtimer1calls];
            turnrate = turndance[numtimer1calls];
        }   else{
            GPIO_SetupPinMux(16, GPIO_MUX_CPU1, 0); // set GPIO16 back to GPIO
            GpioDataRegs.GPACLEAR.bit.GPIO16 = 1; // ground the buzzer
            note_detected = 0;
            numtimer1calls = 0;
            FwdBkOffset = 0;
            turnrate = 0;
        }

        numtimer1calls++; // only increment after note is detected
    }

?