File content as of revision 5:576041795232:

#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("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.flag[3]) 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();
            }
        }
    }
}