//在中断中执行 /5ms
void Car_Control(void){
imu_sensor_read_data_from_fifo(&sensor_saw_data,&sensor_data,&sensor_euler_angle);
if(CarRunning == 1) {
motor_output_Angle = Angle_Control_PD(sensor_euler_angle.roll,Car_Angle_Center, -(sensor_data.gyro[1] - 3.3f));
motor_output_Turn = Turn_Control(trun_mode,turn_target_speed,sensor_euler_angle.yaw,turn_target_orientaion);
motor_output_Speed = Speed_Control(speed_target);
int16_t motor1_output=motor_output_Angle-motor_output_Speed+motor_output_Turn;
int16_t motor2_output=motor_output_Angle-motor_output_Speed-motor_output_Turn;
Motor_Control_1(motor1_output);
Motor_Control_2(motor2_output);
}
else {
Motor_Control_1(0);
Motor_Control_2(0);
}
}
void DoMainLoop(void)
{
//COUNT FOR TIMER
static uint32_t t_current, t_old=(uint32_t)-1, t_test,cnt=0;
//DIPSW - 1-Camera Run / 2-cam_th / 3-.! or %3d / 4-cam_pulsewidth(exposuretime)
//dipsw = DSW_GetDipAll();
//RUN CAMERA //with dip sw 1
/*CAM RUN mode 1 or 2
* main loop 돌때 마다 스위치 값 읽어서 camera run mode를 바꿔주는거
* 카메라를 돌리는건 periodic interrupt timer channel 0 기준 */
if(dipsw & 0b0001) { //sw1 //0x1
CAM_RUN_MODE_SETUP(2);//up
DLED_Set(DLED_LED1,0);
blinking_LED= DLED_LED2;
CAM_RUN2();
}
else {
CAM_RUN_MODE_SETUP(1);//down
DLED_Set(DLED_LED2,0);
blinking_LED= DLED_LED1;
}
//SERIAL COMMUNICATE
if (SDBG_IsEvents())
SDBG_ExecuteCallback();
if (MUART_IsEvents())
MUART_ExecuteCallback();
//servo test in main
sAngle = (FADC_Get(1)-870)*2*37/156-37;
MCTL_Servo(sAngle);
//DC test
//if(DSW_GetTactAll() & 0x1){
//DC_Speed = (FADC_Get(1)-800)*10;
MCTL_DC_R(DC_Speed);
MCTL_DC_L(DC_Speed);
//TIMER FOR EACH N ms
time_gap = 200;
t_current = BTMR_GetRuntime()/time_gap;
if (t_current != t_old) {
t_test = CAM_MICRO_SEC_RETURN();
SDBG_Printf("\n%u: %d", cnt++, t_test);
t_old = t_current;
DLED_Toggle(blinking_LED);
/*DIP SWitch
*1-Camera Run Mode
*2-cam_threshold
*3- .! or %3d 로 카메라 pixel 중 라인 위치, 명암값 출력
*4-cam_pulsewidth(exposure time)*/
dipsw = DSW_GetDipAll();
//Threshold
if(dipsw & 0b0010)//sw2 //0x2
cam_th = (uint16_t)(FADC_Get(1)-800)*5;
else
cam_th = CAM_AVG();
//camera exposure time
if(dipsw & 0b1000)//sw4//0x8
CAM_PulseWidth(100);//up
else//down
CAM_PulseWidth(50);
//ENCODER TEST
//*/
enc1_current = RENC_Get1();
enc2_current = RENC_Get2();
enc1_rate = enc1_current - enc1_old;
enc2_rate = enc2_current - enc2_old;
enc1_old = enc1_current;
enc2_old = enc2_current;
//SDBG_Printf("\n%u: Speed : %4d Rval : %4d %4d RATE : %4d %4d", cnt++, DC_Speed,RENC_Get1(),RENC_Get2(), enc1_rate, enc2_rate);
Speed_Control(3);//3cm/s
//DC_Speed = (FADC_Get(1)-800)*10;//(A2D_GetChResult_10bit(1)-800)*10;
MCTL_DC_R(DC_Speed);
MCTL_DC_L(DC_Speed);
SDBG_Printf("\n%u: Speed : %4d Rval : %4d RATE : %4d", cnt++, DC_Speed,RENC_Get1(), enc1_rate);
//SDBG_Printf("\n%u: US : %8u IR : %4d", cnt++, FUSS_GetMm(FUSS_CH0),FADC_Get(6));//adc6 = pd2 = IR
/**/
//Servo test
//MCTL_Servo(sAngle);
//sAngle +=5;
if(sAngle > MCTL_SWHEEL_MAX)
{
sAngle = -MCTL_SWHEEL_MAX;
}
//SDBG_Printf("\nServo Angle : %d",sAngle);
//if(dipsw & 0x8){//sw4, SDBG
if(flag==0) {
//flag = 1;
}
//}
}
if(flag) {
SDBG_Printf("\n %4d TH:%3d,%3d,%3d\t",t_current,cam_th,CAM_MAX(),CAM_MIN());
for(i_serial = 0; i_serial < NUM_OF_PIXEL; i_serial+=2) { //i_serial++){
if(dipsw & 0b0100) { //up//0x04
if(CAM_DATA1(i_serial) > cam_th) {
SDBG_Printf(".");
}
else { //if(CAM_READ(i_serial) , cam_th)
SDBG_Printf("!");
}
}
else { //down
SDBG_Printf("%3d ",CAM_DATA1(i_serial));
}
}
SDBG_Printf("||");
for(i_serial = 0; i_serial < NUM_OF_PIXEL; i_serial+=2) { //i_serial++){
if(dipsw & 0b0100) { //up//0x04
if(CAM_DATA2(i_serial) > cam_th) {
SDBG_Printf(".");
}
else { //if(CAM_READ(i_serial) , cam_th)
SDBG_Printf("!");
}
}
else { //down
SDBG_Printf("%3d ",CAM_DATA2(i_serial));
}
}
flag = 0;
}
}
