比赛前一天晚上.c

#include "gpio.h"
#include "common.h"
#include "uart.h"
#include "dma.h"

#include "i2c.h"
#include "ov7725.h"

#include "oled_spi.h"

#include "ftm.h"
#include "lptmr.h"
#include "pit.h"
#include "wdog.h"


int ENABLE_DRIVE = 1;
int ENABLE_SERVO = 1;

#define offset 77
void turn(float angel){
    angel += offset;
    int pwm = (int)((angel/90.0 + 0.5) * 500);  //90度是1.5ms
    //printf("turn:%d\r\n", pwm);
    FTM_PWM_ChangeDuty(HW_FTM1, HW_FTM_CH0, pwm);
}

#define DRIVER_PWM_WIDTH 1000
void setSpeed(int spd);
void initDriver(){
    printf("initPWM\r\n");

    //for(int i=0;i<0;i++)
    GPIO_QuickInit(HW_GPIOC, 0, kGPIO_Mode_OPP);
    PCout(0)=1;
    //使能INH

    FTM_PWM_QuickInit(FTM1_CH0_PA12, kPWM_EdgeAligned, 50);     //设置FTM，边沿对齐模式
    turn(0);
    //初始化舵机

    FTM_PWM_QuickInit(FTM0_CH0_PC01, kPWM_EdgeAligned, DRIVER_PWM_WIDTH);
    FTM_PWM_QuickInit(FTM0_CH1_PC02, kPWM_EdgeAligned, DRIVER_PWM_WIDTH);
    FTM_PWM_QuickInit(FTM0_CH2_PC03, kPWM_EdgeAligned, DRIVER_PWM_WIDTH);
    FTM_PWM_QuickInit(FTM0_CH3_PC04, kPWM_EdgeAligned, DRIVER_PWM_WIDTH);
    //初始化电机PWM输出
    setSpeed(0);
}

void setLeftSpeed(int spd){
    if(spd>0){
        FTM_PWM_ChangeDuty(HW_FTM0, HW_FTM_CH1, spd);
        FTM_PWM_ChangeDuty(HW_FTM0, HW_FTM_CH0, 0);
    }else
    {
        FTM_PWM_ChangeDuty(HW_FTM0, HW_FTM_CH1, 0);
        FTM_PWM_ChangeDuty(HW_FTM0, HW_FTM_CH0, -spd);
    }
}

void setRightSpeed(int spd){
    if(spd>0){
        FTM_PWM_ChangeDuty(HW_FTM0, HW_FTM_CH2, spd);
        FTM_PWM_ChangeDuty(HW_FTM0, HW_FTM_CH3, 0);
    }else
    {
        FTM_PWM_ChangeDuty(HW_FTM0, HW_FTM_CH2, 0);
        FTM_PWM_ChangeDuty(HW_FTM0, HW_FTM_CH3, -spd);
    }
}

/* 请将I2C.H中的 I2C_GPIO_SIM 改为 1 */

// 改变图像大小
//0: 80x60
//1: 160x120
//2: 240x180
#define IMAGE_SIZE  1

#if (IMAGE_SIZE  ==  0)
#define OV7620_W    (80)
#define OV7620_H    (60)

#elif (IMAGE_SIZE == 1)
#define OV7620_W    (160)
#define OV7620_H    (120)

#elif (IMAGE_SIZE == 2)
#define OV7620_W    (240)
#define OV7620_H    (180)

#else
#error "Image Size Not Support!"
#endif

#define WIDTH OV7620_W
#define HEIGHT OV7620_H-1

// 图像内存池
uint8_t gCCD_RAM[(OV7620_H)*((OV7620_W/8)+1)];   //使用内部RAM

/* 行指针 */
uint8_t * gpHREF[OV7620_H+1];

/* 引脚定义 PCLK VSYNC HREF 接到同一个PORT上 */
#define BOARD_OV7620_PCLK_PORT      HW_GPIOE
#define BOARD_OV7620_PCLK_PIN       (8)
#define BOARD_OV7620_VSYNC_PORT     HW_GPIOE
#define BOARD_OV7620_VSYNC_PIN      (10)
#define BOARD_OV7620_HREF_PORT      HW_GPIOE
#define BOARD_OV7620_HREF_PIN       (9)
/*
摄像头数据引脚PTA8-PTA15 只能填入 0 8 16三个值
0 :PTA0-PTA7
8 :PTA8-PTA15
16:PTA16-PTA24
*/
#define BOARD_OV7620_DATA_OFFSET    (0)

static void UserApp(uint32_t vcount);
//定义一帧结束后的用户函数

/* 状态机定义 */
typedef enum
{
    TRANSFER_IN_PROCESS, //数据在处理
    NEXT_FRAME,          //下一帧数据
}OV7620_Status;

int SCCB_Init(uint32_t I2C_MAP)
{
    int r;
    uint32_t instance;
    instance = I2C_QuickInit(I2C_MAP, 50*1000);
    r = ov7725_probe(instance);
    if(r)
    {
        return 1;
    }
    r = ov7725_set_image_size((ov7725_size)IMAGE_SIZE);
    if(r)
    {
        printf("OV7725 set image error\r\n");
        return 1;
    }
    return 0;
}

//行中断和场中断都使用PTE中断
void OV_ISR(uint32_t index)
{
    static uint8_t status = TRANSFER_IN_PROCESS;
    static uint32_t h_counter, v_counter;
   // uint32_t i;

    /* 行中断 */
    if(index & (1 << BOARD_OV7620_HREF_PIN))
    {
        DMA_SetDestAddress(HW_DMA_CH2, (uint32_t)gpHREF[h_counter++]);
        //i = DMA_GetMajorLoopCount(HW_DMA_CH2);
        DMA_SetMajorLoopCounter(HW_DMA_CH2, (OV7620_W/8)+1);
        DMA_EnableRequest(HW_DMA_CH2);

        return;
    }
    /* 场中断 */
    if(index & (1 << BOARD_OV7620_VSYNC_PIN))
    {
        GPIO_ITDMAConfig(BOARD_OV7620_VSYNC_PORT, BOARD_OV7620_VSYNC_PIN, kGPIO_IT_FallingEdge, false);
        GPIO_ITDMAConfig(BOARD_OV7620_HREF_PORT, BOARD_OV7620_HREF_PIN, kGPIO_IT_FallingEdge, false);
        switch(status)
        {
            case TRANSFER_IN_PROCESS: //接受到一帧数据调用用户处理
                    UserApp(v_counter++);
                    //printf("i:%d %d\r\n", h_counter, i);
                    status = NEXT_FRAME;
                    h_counter = 0;

                break;
            case NEXT_FRAME: //等待下次传输
                status =  TRANSFER_IN_PROCESS;
                break;
            default:
                break;
        }
        GPIO_ITDMAConfig(BOARD_OV7620_VSYNC_PORT, BOARD_OV7620_VSYNC_PIN, kGPIO_IT_FallingEdge, true);
        GPIO_ITDMAConfig(BOARD_OV7620_HREF_PORT, BOARD_OV7620_HREF_PIN, kGPIO_IT_FallingEdge, true);
        PORTE->ISFR = 0xFFFFFFFF;   //这里可以改PORTE
        h_counter = 0;
        return;
    }
}

void initCamera(){
    uint32_t i;

    printf("OV7725 test\r\n");

    //检测摄像头
    if(SCCB_Init(I2C0_SCL_PB00_SDA_PB01))
    {
        printf("no ov7725device found!\r\n");
        while(1);
    }
    printf("OV7620 setup complete\r\n");

    //每行数据指针
    for(i=0; i<OV7620_H+1; i++)
    {
        gpHREF[i] = (uint8_t*)&gCCD_RAM[i*OV7620_W/8];
    }

    DMA_InitTypeDef DMA_InitStruct1 = {0};

    /* 场中断  行中断 像素中断 */
    GPIO_QuickInit(BOARD_OV7620_PCLK_PORT, BOARD_OV7620_PCLK_PIN, kGPIO_Mode_IPD);
    GPIO_QuickInit(BOARD_OV7620_VSYNC_PORT, BOARD_OV7620_VSYNC_PIN, kGPIO_Mode_IPD);
    GPIO_QuickInit(BOARD_OV7620_HREF_PORT, BOARD_OV7620_HREF_PIN, kGPIO_Mode_IPD);

    /* install callback */
    GPIO_CallbackInstall(BOARD_OV7620_VSYNC_PORT, OV_ISR);

    GPIO_ITDMAConfig(BOARD_OV7620_HREF_PORT, BOARD_OV7620_HREF_PIN, kGPIO_IT_FallingEdge, true);
    GPIO_ITDMAConfig(BOARD_OV7620_VSYNC_PORT, BOARD_OV7620_VSYNC_PIN, kGPIO_IT_FallingEdge, true);
    GPIO_ITDMAConfig(BOARD_OV7620_PCLK_PORT, BOARD_OV7620_PCLK_PIN, kGPIO_DMA_RisingEdge, true);

    /* 初始化数据端口 */
    for(i=0;i<8;i++)
    {
        GPIO_QuickInit(HW_GPIOE, BOARD_OV7620_DATA_OFFSET+i, kGPIO_Mode_IFT);
    }

    //DMA配置
    DMA_InitStruct1.chl = HW_DMA_CH2;
    DMA_InitStruct1.chlTriggerSource = PORTE_DMAREQ;    //这里可以改PORTE
    DMA_InitStruct1.triggerSourceMode = kDMA_TriggerSource_Normal;
    DMA_InitStruct1.minorLoopByteCnt = 1;
    DMA_InitStruct1.majorLoopCnt = ((OV7620_W/8) +1);

    DMA_InitStruct1.sAddr = (uint32_t)&PTE->PDIR + BOARD_OV7620_DATA_OFFSET/8;  //这里可以改PTE
    DMA_InitStruct1.sLastAddrAdj = 0;
    DMA_InitStruct1.sAddrOffset = 0;
    DMA_InitStruct1.sDataWidth = kDMA_DataWidthBit_8;
    DMA_InitStruct1.sMod = kDMA_ModuloDisable;

    DMA_InitStruct1.dAddr = (uint32_t)gpHREF[0];
    DMA_InitStruct1.dLastAddrAdj = 0;
    DMA_InitStruct1.dAddrOffset = 1;
    DMA_InitStruct1.dDataWidth = kDMA_DataWidthBit_8;
    DMA_InitStruct1.dMod = kDMA_ModuloDisable;

    /* initialize DMA moudle */
    DMA_Init(&DMA_InitStruct1);
}


bool printflag = false;
//串口接收中断
void UART_RX_ISR(uint16_t byteRec){
    //打印整个图像
    if(byteRec == ' ')printflag = true;

}
// IMG
uint8_t IMG[OV7620_W][OV7620_H];   //使用内部RAM


int white[HEIGHT];
bool crossflag = false;
bool rectflag = false;
int rectcounter = 0;
void fixLine(int black, int y, bool isblack){
    if(isblack==0)rectcounter=40;
    //宽度大于5的黑条或白条
    if(white[y]>WIDTH-5)crossflag=true;else rectflag=false;

    int crossstart = y+black+15;
    int crossend = y-10;
    if(crossend<0)crossend=0;
    if(crossstart>HEIGHT)crossstart=WIDTH-1;
    if(white[crossstart]>10&&white[crossend]>10){

        //起始位置从中心向两边搜索边界
        int left1 = WIDTH/2;
        int right1 = WIDTH/2;
        while(left1){
            if(IMG[left1][crossstart] == 1 && (IMG[left1+1][crossstart] == 0))break;
            left1--;
        }
        while(right1 < WIDTH-1){
            if(IMG[right1][crossstart] == 0 && (IMG[right1+1][crossstart] == 1))break;
            right1++;
        }
        //printf("left1=%d,right1=%d\n", left1, right1);

        //终止位置从中心向两边搜索边界
        int left2;
        int right2;
        float k1 = 0;
        float k2 = 0;
        
        left2 = WIDTH/2;
        right2 = WIDTH/2;
        while(left2){
            if(IMG[left2][crossend] == 1 && (IMG[left2+1][crossend] == 0))break;
            left2--;
        }
        while(right2 < WIDTH-1){
            if(IMG[right2][crossend] == 0 && (IMG[right2+1][crossend] == 1))break;
            right2++;
        }
        IMG[left2][crossend] = 2;
        IMG[right2][crossend] = 2;

        k1 = (float)(left2-left1)/(crossend-crossstart);
        k2 = (float)(right2-right1)/(crossend-crossstart);
        //f(k1<0 && k2>0)break;

        if(!isblack){
//            for(int i=0;i<(crossstart-crossend) && crossend+i < HEIGHT-1;i++){
//                //补线
//                for(int j= ;j<right2+k2*i;j++)
//                    IMG[j][crossend+i] = 0;
//                }
            for(int i=0;i<(crossstart-crossend) && crossend+i < HEIGHT-1;i++){
                IMG[(int)(left2+k1*i)][crossend+i] = 0;
                IMG[(int)(right2+k2*i)][crossend+i] = 0;
                IMG[(int)(left2+k1*i)+1][crossend+i] = 0;
                IMG[(int)(right2+k2*i)-1][crossend+i] = 0;
                IMG[(int)(left2+k1*i)+2][crossend+i] = 0;
                IMG[(int)(right2+k2*i)-2][crossend+i] = 0;
            }
        }
        else 
        for(int i=0;i<(crossstart-crossend) && crossend+i < HEIGHT-1;i++){
            //两边补线
            IMG[(int)(left2+k1*i)][crossend+i] = 2;
            IMG[(int)(right2+k2*i)][crossend+i] = 2;
        }
    }
}

void findType(){
    int x,y;
    for(y=0;y<HEIGHT;y++){
        int whitedots=0;
        for(x=0;x<WIDTH;x++){
            if(IMG[x][y]==0)whitedots++;
        }
        white[y] = whitedots;
    }
    //统计白点个数

    int black=0;bool lastblack=false;
    int whitesum=0;bool lastwhite=false;
    
    for(y=HEIGHT;y>0;y--){

        if(white[y]<5){
            //一条黑条
            if(lastblack)black++;   //上一次也是黑,则黑++
            else black=1;   //第一次黑
            lastblack=true;
        }else{
            lastblack=false;
            if((black > 7) && (black < 18) && (y > 60)){    //黑大于5次
                rectflag=true;  //直角标志
                fixLine(black, y, false);

            }else{
                rectflag = false;
            }
            black=0;
        }

        if(white[y]>WIDTH-3){
            //一条白
            if(lastwhite)whitesum++;
            else whitesum=1;
            lastwhite=true;
        }else{
            lastwhite=true;
            if(whitesum>10){
                
                char buf[20]={0};
                sprintf(buf, "s=%d", white[y+2]);
                LED_P8x16Str(80, 1, buf);
                
                if(abs(white[y+2] - 0.82*y+50) < 10){
                    //避免直角弯补线
                    crossflag=true;
                    fixLine(whitesum, y, true);
                }
            }else{
                crossflag = false;
            }
            whitesum=0;
        }

    }

}

void findType2();

void findCenter();

#define DELTA_MAX 3
int average=0;
int centers[HEIGHT] = {0};

/* 接收完成一场后 用户处理函数 */
static void UserApp(uint32_t vcount)
{
    for(int y=0;y<OV7620_H-1;y++)
        for(int x=0;x<OV7620_W/8;x++)
            for(int i=0; i<8; i++)
                IMG[x*8+i][y] = (gpHREF[y][x+1]>>(7-i))%2;
    //将图片从OV7620_H*OV7620_W/8映射到OV7620_H*OV7620_W
    
    findType();
    findCenter();
    //findType2();
    
//    if(printflag){
//        printflag = false;
//        //打印出图像
//        printf("start\r\n");
//        for(int y=0;y<OV7620_H-1;y++){
//            for(int x=0;x<OV7620_W;x++){
//                printf("%d", IMG[x][y]);
//            }
//            printf("\r\n");
//        }
//        
//    }
    
    if(printflag){
        printflag = false;
        printf("ave\twhite\r\n");
        for(int y=0;y<HEIGHT;y++){
            printf("%d\t%d\r\n", centers[y], white[y]);
        }
    }
    
    
    
    //打印到屏幕上
    for(int y=0;y<8;y++){
        LED_WrCmd(0xb0 + y); //0xb0+0~7表示页0~7
        LED_WrCmd(0x00); //0x00+0~16表示将128列分成16组其地址在某组中的第几列
        LED_WrCmd(0x10); //0x10+0~16表示将128列分成16组其地址所在第几组
        for(int x=0;x<80;x++){
            uint8_t data = 0;
            for(int i=0;i<8 && (y*8+i)*2<HEIGHT ;i++){
                data += (IMG[x*2][(y*8+i)*2] > 0 | IMG[x*2+1][(y*8+i)*2] > 0)<<(i);
            }
            LED_WrDat(data);
        }
    }
//    
//    if(crossflag)LED_P8x16Str(80, 0, "cross ");
//    else LED_P8x16Str(80, 0, "normal");

    
}

int zhijiao = 0;
int err2 = 0;
void findCenter(){
    int y=HEIGHT;

    int center = WIDTH/2;       //赛道中心
    int s = 0;          //中心累积求和
    int sum = 0;        //中心数
    
    int left;
    int right;
    
    if(rectcounter>0){
        //检测到直角
        rectcounter--;
        PBout(11)=1;
        //findType2();
//        if(abs(average)>
    }else PBout(11)=0;
    
    
    int err = 0;
    
    while(white[y] - 0.82*y+50 > 50){
            y--;
    }
    //跟直道差太大,丢弃
    
    left = WIDTH;
    right = 0;
    
    while(y){
        
        int zhongxin = 0;
        int zhongxinnum = 0;
        
#define SCAN_WIDTH 25
        int i=-SCAN_WIDTH;
        if(center+i<0)i=-center+1;
        for(;i<SCAN_WIDTH & (center+i < WIDTH);i++){
            if(IMG[center+i][y]){
                zhongxin += center+i;
                zhongxinnum ++;
            }
        }
        
        if(zhongxinnum && (zhongxinnum < 20) ){
            center = zhongxin / zhongxinnum;
        }else{
            while(left){
                if((IMG[left][y]==1)&&(IMG[left+1][y]==0))break;
                left--;
            }
            while(right < WIDTH){
                if((IMG[right-1][y]==0)&&(IMG[right][y]==1))break;
                right++;
            }
            
            center = (left+right)/2;
        }
        

        
        centers[y] = center;
        if((right-left<10) && (zhongxin == 0)){
            break;
        }
        //赛道太窄而且不是中心引导线
        //if(zhongxin && white[y]>20)break;
        //中心线过宽(黑条)
        
        left = center-1;
        right = center+1;
        
        s += center;
        sum ++;
        
        //画出中心线
        
        y--;
    }
    
    s=0;
    if(y<70)y=70;
    int sum2 = 80-y;
    for(;y<80;y++)s+=centers[y];
    
    
    if(sum>10){
        for(y=HEIGHT;y>0;y--)IMG[centers[y]][y] = 1;
        average = s/sum2;
        average -= 80;
        if(ENABLE_SERVO)turn(average/1.5);
        if(ENABLE_DRIVE)setSpeed(800+(sum-60)*20+(25-abs(average))*20-(rectcounter>0)*900);
        err2=0;
    }else{
        err2++;
        if(err2>200){
            setSpeed(0);
            //turn(0);
            //while(1);
        }
    }

    char buf[20]={0};
    sprintf(buf, "a=%d ", average);
    LED_P8x16Str(80, 0, buf);
    
}

float differ = 0;

void setSpeed(int spd){
#define kchasu 60
    if(average>0){
        setLeftSpeed(spd+spd*average/kchasu);
        setRightSpeed(spd);
    }else{
        setLeftSpeed(spd);
        setRightSpeed(spd-spd*average/kchasu);
    }
}

//int speed = 0;
//int lastspeed = 0;
//float Pspeed = -1;
//float Dspeed = 0.1;
//int lastpwm = 0;
//int valueright = 0;
//int valueleft = 0;
//void PIT_ISR(void)
//{
//     /* 记录正交脉冲个数 */
//    uint8_t dir; /* 记录编码器旋转方向1 */
//    
//    /* 获取正交解码数据 */
//    FTM_QD_GetData(HW_FTM2, &valueright, &dir);
//    valueleft = LPTMR_PC_ReadCounter();
//    FTM_QD_ClearCount(HW_FTM2); /* 如测量频率则需要定时清除Count值  */
//    LPTMR_ClearCounter();       //清空Counter
//    
//    if(valueright>32767)valueright=65535-valueright;
//    valueright*=4.2279;
//}

//void speedPID(){
//    
//
//    //printf("\r\n%d\t%d", valueleft, valueright);
//    speed = valueleft;
//    
//    int pwm = Pspeed * (speed - 1000) + Dspeed * (speed - lastspeed);
//    lastspeed = speed;
//    setSpeed(pwm+lastpwm);
//    lastpwm = pwm+lastpwm;
//    char buf[20]={0};
//    sprintf(buf, "p=%d ", pwm+lastpwm);
//    LED_P8x16Str(80, 0, buf);
//    sprintf(buf, "v=%d ", speed);
//    LED_P8x16Str(80, 3, buf);
//}

void findType2(){
    //判断直角
    int y;
    int zhijiao = 0;
    int err = 0;
    y=HEIGHT;
    while( (abs(white[y] - 0.82*y+50) < 10) & (y > 0) ){
        y--;
    }
    while(y>0){
        y--;
        int zhidao = 0.82*y+50;
        if(white[y] - zhidao > 10){
            if(IMG[0][y] == 0)zhijiao--;
            if(IMG[WIDTH][y] == 0)zhijiao++;
            if(white[y] - zhidao > 50) break;
        }else break;
    }
    
    if(y>10)y-=5;
    if(white[y]>5 | white[y-1]>5 | white[y-2]>5)zhijiao=0;
    
    if((abs(zhijiao)>10) & (y > 25)){
        if(zhijiao>0)average=-40;
        turn(average/1.2);
        setSpeed(1500);
        DelayMs(300);
    }
    
//    char buf[20] = {0};
//    sprintf(buf, "Z=%d ", zhijiao);
//    LED_P8x16Str(80, 0, buf);
//    sprintf(buf, "y=%d ", y);
//    LED_P8x16Str(80, 1, buf);
}


int main(void)
{

    DelayInit();
    /* 打印串口及小灯 */
    
    GPIO_QuickInit(HW_GPIOB, 11, kGPIO_Mode_OPP);       //蜂鸣器
    GPIO_QuickInit(HW_GPIOD, 10, kGPIO_Mode_OPP);
    UART_QuickInit(UART0_RX_PB16_TX_PB17, 115200);
    /* 注册中断回调函数 */
    UART_CallbackRxInstall(HW_UART0, UART_RX_ISR);

    /* 开启UART Rx中断 */
    UART_ITDMAConfig(HW_UART0, kUART_IT_Rx, true);

    initOLED();
    initDriver();
    initCamera();
    
    //编码器初始化
    //FTM_QD_QuickInit(FTM2_QD_PHA_PA10_PHB_PA11, kFTM_QD_NormalPolarity, kQD_PHABEncoding);
    //LPTMR_PC_QuickInit(LPTMR_ALT2_PC05); /* 脉冲计数 */
    
    /* 开启PIT中断 */
    //PIT_QuickInit(HW_PIT_CH0, 1000*100);
    //PIT_CallbackInstall(HW_PIT_CH0, PIT_ISR);
    //PIT_ITDMAConfig(HW_PIT_CH0, kPIT_IT_TOF, true);
    
    //用户控制部分
    GPIO_QuickInit(HW_GPIOC, 8, kGPIO_Mode_IPU);
    GPIO_QuickInit(HW_GPIOC, 10, kGPIO_Mode_IPU);
    GPIO_QuickInit(HW_GPIOC, 12, kGPIO_Mode_IPU);
    GPIO_QuickInit(HW_GPIOC, 14, kGPIO_Mode_IPU);
    GPIO_QuickInit(HW_GPIOC, 16, kGPIO_Mode_IPU);
    GPIO_QuickInit(HW_GPIOC, 18, kGPIO_Mode_IPU);
#define differadd 0.02
    if(PCin(8)){
        differ += differadd;
//        for(int i=0;i<5000;i+=100){
//            setSpeed(i);
//            DelayMs(500);
//            printf("\t%d", i);
//            DelayMs(100);
//        }
        
        //setRightSpeed(3000); 
        //turn(-20);
        
    }
    if(PCin(10))differ += differadd;
    if(PCin(12))differ += differadd;
    if(PCin(14))differ += differadd;
    if(PCin(16))differ += differadd;
    if(PCin(18))differ += differadd;
    
    printf("differ=%f\r\n", differ);

    
    GPIO_QuickInit(HW_GPIOB, 3, kGPIO_Mode_IPU);
    
    int test = 0;
    int last = 0;
    
    ENABLE_SERVO = 1;
    ENABLE_DRIVE = 1;
    
    while(1)
    {
        DelayUs(500);
//        
//        if(PBin(3)){
//            if(test>0)test--;
//        }
//        else {
//            test = 50;
//        }
//        
//        PDout(10) = test>0;
//        
//        if((test>0)!=last){
//            last = test>0;
//            if(last){
//                ENABLE_SERVO = 0;
//                ENABLE_DRIVE = 0;
//                setSpeed(0);
//                turn(0);
//                DelayMs(1000);
//            }else{
//                ENABLE_SERVO = 1;
//                ENABLE_DRIVE = 1;
//                DelayMs(1000);
//            }
//        }
        
    }
}