Exemplo n.º 1
0
void us_ticker_init(void) {
    if (us_ticker_inited)
        return;
    us_ticker_inited = 1;
    
    SIM->SCGC6 |= SIM_SCGC6_PIT_MASK;   // Clock PIT
    PIT->MCR = 0;                       // Enable PIT
    
    clk_mhz = bus_frequency() / 1000000;

    timer_init();
    ticker_init();
}
Exemplo n.º 2
0
int main() {
    pc.baud(115200);
    
    /*  Turn off the      BOD, ADC & WDT */
    LPC_SYSCON->PDRUNCFG |= (1<<3)|(1<<4)|(1<<6);

    pwm_init();

    
    if(config_power_mode(PARAM_CPU_EXEC)){
        //pc.printf("48MHz maximum CPU performance Config Fail\n");
        pc.puts("48MHz maximum CPU performance Config Fail\n");
    }else{
        //pc.printf("48MHz maximum CPU performance Config OK~\n");
        pc.puts("48MHz maximum CPU performance Config OK~\n");
    }

    if(mpu.testConnection()){
        //pc.printf("MPU6050 OK~\n");
        pc.puts("MPU6050 OK~\n");
        wait_ms(100);
        mpu.setBW(MPU6050_BW_256); // acc low pass 260 Hz
        mpu.setFullScaleAccelRange(MPU6050_ACCELERO_RANGE_8G);
        mpu.setFullScaleGyroRange(MPU6050_GYRO_RANGE_1000);
        mpu.initialize();
    }else{
        //pc.printf("MPU6050 failed!\n");
        pc.puts("MPU6050 failed!\n");
        NVIC_SystemReset();
    }


    pc.attach(&BT_RX_ISR, pc.RxIrq);
    NVIC_EnableIRQ(UART_IRQn);
    
    ticker_init();
    //status |= 0x02;
    
    //pc.printf("initialize OK!\n");
    pc.puts("initialize OK!\n");

    while(1) {
        status_upate();
        switch(status){
            case 0x00:
            
            break;

            case 0x01:
                led1 = 1;
                status &= ~(0x01);
                //pc.printf("%f %f %f\n", degree[0], degree[1], degree[2]);
                
                //pc.printf("%f\t%f\t%f\t%f\n", q0, q1, q2, q3);
                encodeQ(q0, q1, q2, q3);
                pc.printf("%s\n", buf);
                //pc.puts(buf);
                //pc.printf("%d\t%d\n", LPC_TMR32B1->TC, sys_tick);
                
                led1 = 0;
            break;
            
            case 0x03:
            case 0x02:
                led2 = 1;
                status &= ~(0x02);
                sensfusion6_thread();
                calPID(input, degree, gyr_raw, pidout);
                calpwm(pidout, pwm);
                pwm_pulsewidth(pwm);
                led2 = 0;
            break;
            
            default:
            case 0x04:
                status &= ~(0x04);
                BT_thread();
                calPID(input, degree, gyr_raw, pidout);
                calpwm(pidout, pwm);
                pwm_pulsewidth(pwm);
            break;

        }
    }
}