int main(void)
{
InitPeripherals();
SDBG_Printf("\nDisplacement test\n");
SDBG_Printf("Press S2!\n");
while (FADC_Get(1) < 950)
{
SDBG_Printf("Release S2!\n");
DLED_Set(DLED_LED2, DLED_ON);
delay_ms(50);
DLED_Set(DLED_LED2, DLED_OFF);
delay_ms(50);
}
while(1){
//while ((DSW_GetTactAll() & 0b0000) != 0b0000){
DoMainLoop();
//}
}
return 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;
}
}