2021 rcj

Dependencies:   _sensorGen ui Robot omni_wheel PID jy901 solenoid IRsensor aqm0802 camera beep kohiMD linesSnsor lpf RCJESC

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 #include "main.h"
00002 #include "robot.h"
00003 #include "ui.h"
00004 
00005 Robot robo;
00006 UI ui;
00007 Thread displaythread;
00008 sensorgen sensor;
00009 BufferedSerial pc(USBTX, USBRX, 115200);
00010 KohiMD *md[4] = {new KohiMD(PA_15), new KohiMD(PA_6), new KohiMD(PA_7), new KohiMD(PB_6)};
00011 
00012 
00013 void main2ui()
00014 {
00015     char lcdname1[4] = {}, lcdname2[4] = {};
00016     int  lcdvalue[2] = {};
00017     double temp[2] = {};
00018     while(1){
00019 //        printf("test\r\n");
00020         if(ui.displayst == 0){
00021                 strcpy(lcdname1, "gyr");
00022                 strcpy(lcdname2, "bal");
00023                 if(sensor.angle >= 0)lcdvalue[0] = sensor.angle;
00024                 else lcdvalue[0] = 360+sensor.angle;
00025         } else if(ui.displayst == 1) {
00026                 strcpy(lcdname1, "bar");
00027                 strcpy(lcdname2, "bax");
00028                 if(sensor.ballAngle >= 0)lcdvalue[0] = sensor.ballAngle;
00029                 else lcdvalue[0] = 360+sensor.ballAngle;
00030                 lcdvalue[1] = sensor.ballRange;
00031         } else if(ui.displayst == 2) {
00032                 strcpy(lcdname1, "ylr");
00033                 strcpy(lcdname2, "ylx");
00034                 if(sensor.yellowAngle >= 0)lcdvalue[0] = sensor.yellowAngle;
00035                 else lcdvalue[0] = 360+sensor.yellowAngle;
00036                 lcdvalue[1] = sensor.yellowRange;
00037         } else if(ui.displayst == 3) {
00038                 strcpy(lcdname1, "blr");
00039                 strcpy(lcdname2, "blx");
00040                 if(sensor.blueAngle >= 0)lcdvalue[0] = sensor.blueAngle;
00041                 else lcdvalue[0] = 360+sensor.blueAngle;
00042                 lcdvalue[1] = sensor.yellowRange;
00043         } else if(ui.displayst == 4) {
00044                 strcpy(lcdname1, "mt0");
00045                 strcpy(lcdname2, "mt1");
00046 //                temp[0] = robo.omni.wheel[0];
00047 //                temp[1] = robo.omni.wheel[1];
00048 //                lcdvalue[0] = temp[0] * 100;
00049 //                lcdvalue[1] = temp[1] * 100;
00050                 lcdvalue[0] = 0;
00051                 lcdvalue[1] = 0;
00052         } else if(ui.displayst == 5) {
00053                 strcpy(lcdname1, "mt2");
00054                 strcpy(lcdname2, "mt3");
00055 //                temp[0] = robo.omni.wheel[2];
00056 //                temp[1] = robo.omni.wheel[3];
00057 //                lcdvalue[0] = temp[0] * 100;
00058 //                lcdvalue[1] = temp[1] * 100;
00059                 lcdvalue[0] = 0;
00060                 lcdvalue[1] = 0;
00061         /*} else if(ui.displayst == 6) {
00062                 strcpy(lcdname1, "
00063                 ln1");
00064                 strcpy(lcdname2, "ln2");
00065                 lcdvalue[2] = sensor.;
00066                 lcdvalue[3] = sensor.;
00067                 break;
00068         } else if(ui.displayst == 7) {
00069                 strcpy(lcdname1, "ln3");
00070                 strcpy(lcdname2, "ln4");
00071                 lcdvalue[2] = sensor.;
00072                 lcdvalue[3] = sensor.;
00073                 break;*/
00074         }
00075         ui.change(lcdname1, lcdname2, lcdvalue);
00076         thread_sleep_for(3);
00077     }
00078 }
00079 
00080 int main(){
00081     
00082     displaythread.start(main2ui);
00083     uint8_t motorChecknum, uibt[4];
00084     bool startflag;
00085 //    robo.pidtest();
00086     while(1){
00087 //        printf("ball: x %5d,r %6d | yellow: x %5d,r %6d | blue: x %5d,r %6d  gyro %4d | \r\n"
00088 //        , (int)(sensor.ballRange), (int)(sensor.ballAngle), (int)(sensor.yellowRange), (int)(sensor.yellowAngle), (int)(sensor.blueRange), (int)(sensor.blueAngle), (int)(sensor.tempAngle));
00089 //        printf("gyro : %d\r\n",(int)sensor.angle);
00090         
00091 //        ui.returnbt(uibt);
00092 //        printf("%d  %d  %d  %d   %d\r\n", ui.flag[0], ui.flag[1], ui.flag[2], ui.flag[3], ui.start);
00093         if(ui.mode == 0){
00094 //            printf("ball: x %d,r %d | yellow: x %d,r %d | blue: x %d,r %d  gyro %d | \r\n"
00095 //            , (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));
00096             if(ui.flag[3]) robo.moveTest();//robo.pidtest();//robo.start(ui.team, ui.algorithm);
00097             else robo.motorStop(0.0);
00098         } else if(ui.mode == 1){
00099 //                motorChecknum = (8*ui.b[3])+(4*ui.b[2])+(2*ui.b[1])+(ui.b[0]);
00100             if(ui.b[0]){
00101                 robo.motorCheck(0, 0.3);
00102             }else if(ui.b[1]){
00103                 robo.motorCheck(1, 0.3);
00104             }else if(ui.b[2]){
00105                 robo.motorCheck(2, 0.3);
00106             }else if(ui.b[3]){
00107                 robo.motorCheck(3, 0.3);
00108             }else{
00109                 robo.motorStop(0.0);
00110             }
00111         } else if(ui.mode == 2){
00112             if(ui.start){
00113                 robo.kickCheck();
00114             }
00115             robo.motorStop(0.0);
00116         } else if(ui.mode == 3){
00117             if(ui.start){
00118                 robo.dribbleCheck(0.4);
00119             } else {
00120                 robo.dribbleCheck(0);
00121             }
00122             robo.motorStop(0.0);
00123         } else if(ui.mode == 4){
00124             if(ui.flag[3]){
00125                 sensor.jy.yawcalibrate();
00126             }
00127         }
00128     }
00129 }