2021 rcj
Dependencies: _sensorGen ui Robot omni_wheel PID jy901 solenoid IRsensor aqm0802 camera beep kohiMD linesSnsor lpf RCJESC
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 }
Generated on Mon Jul 25 2022 16:18:25 by
1.7.2