File content as of revision 1:0034c2a97787:

#include "main.h"
#include "robot.h"
//#include "ui.h"

Robot robo;
//UI ui;
//Thread displaythread;
sensorgen sensor;
BufferedSerial pc(USBTX, USBRX, 115200);
//KohiMD *md[4] = {new KohiMD(PA_15), new KohiMD(PA_6), new KohiMD(PA_7), new KohiMD(PB_6)};


//void main2ui()
//{
//    char lcdname1[4] = {}, lcdname2[4] = {};
//    int  lcdvalue[2] = {};
//    double temp[2] = {};
//    while(1){
////        printf("test\r\n");
//        if(ui.displayst == 0){
//                strcpy(lcdname1, "gyr");
//                strcpy(lcdname2, "bal");
//                if(sensor.angle >= 0)lcdvalue[0] = sensor.angle;
//                else lcdvalue[0] = 360+sensor.angle;
//        } else if(ui.displayst == 1) {
//                strcpy(lcdname1, "bar");
//                strcpy(lcdname2, "bax");
//                if(sensor.ballAngle >= 0)lcdvalue[0] = sensor.ballAngle;
//                else lcdvalue[0] = 360+sensor.ballAngle;
//                lcdvalue[1] = sensor.ballRange;
//        } else if(ui.displayst == 2) {
//                strcpy(lcdname1, "ylr");
//                strcpy(lcdname2, "ylx");
//                if(sensor.yellowAngle >= 0)lcdvalue[0] = sensor.yellowAngle;
//                else lcdvalue[0] = 360+sensor.yellowAngle;
//                lcdvalue[1] = sensor.yellowRange;
//        } else if(ui.displayst == 3) {
//                strcpy(lcdname1, "blr");
//                strcpy(lcdname2, "blx");
//                if(sensor.blueAngle >= 0)lcdvalue[0] = sensor.blueAngle;
//                else lcdvalue[0] = 360+sensor.blueAngle;
//                lcdvalue[1] = sensor.yellowRange;
//        } else if(ui.displayst == 4) {
//                strcpy(lcdname1, "mt0");
//                strcpy(lcdname2, "mt1");
////                temp[0] = robo.omni.wheel[0];
////                temp[1] = robo.omni.wheel[1];
////                lcdvalue[0] = temp[0] * 100;
////                lcdvalue[1] = temp[1] * 100;
//                lcdvalue[0] = 0;
//                lcdvalue[1] = 0;
//        } else if(ui.displayst == 5) {
//                strcpy(lcdname1, "mt2");
//                strcpy(lcdname2, "mt3");
////                temp[0] = robo.omni.wheel[2];
////                temp[1] = robo.omni.wheel[3];
////                lcdvalue[0] = temp[0] * 100;
////                lcdvalue[1] = temp[1] * 100;
//                lcdvalue[0] = 0;
//                lcdvalue[1] = 0;
//        /*} else if(ui.displayst == 6) {
//                strcpy(lcdname1, "
//                ln1");
//                strcpy(lcdname2, "ln2");
//                lcdvalue[2] = sensor.;
//                lcdvalue[3] = sensor.;
//                break;
//        } else if(ui.displayst == 7) {
//                strcpy(lcdname1, "ln3");
//                strcpy(lcdname2, "ln4");
//                lcdvalue[2] = sensor.;
//                lcdvalue[3] = sensor.;
//                break;*/
//        }
//        ui.change(lcdname1, lcdname2, lcdvalue);
//        thread_sleep_for(3);
//    }
//}

int main(){
    
//    displaythread.start(main2ui);
    uint8_t motorChecknum, uibt[4];
    bool startflag;
//    robo.pidtest();
    while(1){
//        printf("%d ",ui._tgl[0]);
//        ui.updatebt();
//        main2ui();
//        printf("ball: x %5d,r %6d | yellow: x %5d,r %6d | blue: x %5d,r %6d  gyro %4d | \r\n"
//        , (int)(sensor.ballRange), (int)(sensor.ballAngle), (int)(sensor.yellowRange), (int)(sensor.yellowAngle), (int)(sensor.blueRange), (int)(sensor.blueAngle), (int)(sensor.tempAngle));
//        printf("gyro : %d\r\n",(int)sensor.angle);
        
//        ui.returnbt(uibt);
//        printf("%d  %d  %d  %d   %d\r\n", ui.flag[0], ui.flag[1], ui.flag[2], ui.flag[3], ui.start);
//        if(ui.mode == 0){
//            printf("ball: x %d,r %d | yellow: x %d,r %d | blue: x %d,r %d  gyro %d | \r\n"
//            , (int)(100*sensor.ballRange), (int)(100*sensor.ballAngle), (int)(100*sensor.yellowRange), (int)(100*sensor.yellowAngle), (int)(100*sensor.blueRange), (int)(100*sensor.blueAngle), (int)(sensor.rawAngle));
//            if(!(ui._tgl[0])) robo.moveTest();//robo.pidtest();//robo.start(ui.team, ui.algorithm);
//            else robo.motorStop(0.0);
//        } else if(ui.mode == 1){
//                motorChecknum = (8*ui.b[3])+(4*ui.b[2])+(2*ui.b[1])+(ui.b[0]);
//            if(ui.b[0]){
//                robo.motorCheck(0, 0.3);
//            }else if(ui.b[1]){
//                robo.motorCheck(1, 0.3);
//            }else if(ui.b[2]){
//                robo.motorCheck(2, 0.3);
//            }else if(ui.b[3]){
//                robo.motorCheck(3, 0.3);
//            }else{
//                robo.motorStop(0.0);
//            }
////        } else if(ui.mode == 2){
//            if(ui.start){
//                robo.kickCheck();
//            }
//            robo.motorStop(0.0);
////        } else if(ui.mode == 3){
//            if(ui.start){
//                robo.dribbleCheck(0.4);
//            } else {
//                robo.dribbleCheck(0);
//            }
//            robo.motorStop(0.0);
////        } else if(ui.mode == 4){
//            if(ui.flag[3]){
//                sensor.jy.yawcalibrate();
////            }
//        }
//        printf("e \r\n");
        if(!sensor.start) robo.moveTest();
        if(sensor.b0)
        {
            if(sensor.test) robo.dribbleCheck(0.6);
            else robo.kickCheck();
        }
        else robo.dribbleCheck(0.0);
    }
}