void code_qput(const char *s,unsigned int n)
{
if (!flagcd)
qmail_put(&qq,s,n);
else {
if (flagcd == 'B')
encodeB(s,n,&qline,0);
else
encodeQ(s,n,&qline);
qmail_put(&qq,qline.s,qline.len);
}
}
int code_qput(const char *s,unsigned int n)
{
if (!flagcd)
qmail_put(&qq,s,n);
else {
if (flagcd == 'B')
encodeB(s,n,&qline,0);
else
encodeQ(s,n,&qline);
qmail_put(&qq,qline.s,qline.len);
}
return 0; /* always succeeds */
}
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;
}
}
}
