平衡segway机器人开源项目

描述

“Segbot”是 UIUC ME 461 级制造的平衡 segway 机器人。segbot 由 Dan Block (d-block@illinois.edu) 教授设计的电路板和 F28379D 微控制器组成，该微控制器是德州仪器 C2000 系列的一部分。该项目的目标是让 segbot 播放歌曲和舞蹈以响应检测到不同的音符。

音符检测

音符检测是使用模数转换完成的，将从麦克风接收到的模拟信号转换为数字值。没有占空比输出的脉宽调制被用作定时器，以 10 kHz 的采样率触发 ADC 中断。为了识别不同的频率，使用Goertzel 算法一次对 1000 个 ADC 值进行离散傅里叶变换。如果算法的输出超过某个阈值，则检测到音符。阈值是一个稍微随意的值，调整为在适当的时间做出响应，这意味着如果由于意外事件（例如落笔）而出现频率，则不会检测到音符。

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

乒乓缓冲器

实现了一个乒乓缓冲器版本，以将 ADC 读数存储在单独的阵列中，这样一个可以收集数据，而另一个可以在 Goertzel 函数中执行计算。使用了三个这样的数组，以便 Goertzel 算法在检查对应于三种不同歌曲和舞蹈的三个单独音符之间交替。当检测到其中一个目标频率时，它会发出歌舞开始的信号。

//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 播放歌曲，蜂鸣器由脉冲宽度调制控制。三个 CPU 定时器中断中的每一个都被设置为不同的周期以对应歌曲的节奏。歌曲的音符组合成一个数组，用来改变PWM的周期，在检测到第一个音符的情况下，每次发生定时器中断时，蜂鸣器都会播放相应的音符。播放的歌曲是对披头士的“Hey Jude”、夏奇拉的“Hips Don't Lie”和海滩男孩的“Little Saint Nick”的演绎。为了增强“小圣尼克”的表现，增加了一个铃铛，并结合 RC 伺服电机使用 PWM，在歌曲的不同部分敲击表面。

让 segbot 平衡是在此项目中完成的，再次使用 PWM 触发 ADC，其中值通过 SPI 写入，以便从 MPU-9250 读取加速度和陀螺仪值。实施卡尔曼滤波器以在将值发送到平衡 segbot 的控制律之前对其进行过滤。与播放歌曲类似，为了让 segbot 跳舞，为转弯速率和向前/向后偏移创建了一个数组。当检测到相应的音符时，在 CPU 定时器中断中执行舞蹈。

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