YKNCT
ctrlStation RX sample
Dependencies: mbed CtrlStationRX
Revision 2:0806f6996e8c, committed 2021-11-12
- Comitter:
- TakushimaYukimasa
- Date:
- Fri Nov 12 14:58:59 2021 +0000
- Parent:
- 1:56c272cb4522
- Commit message:
- compleate RX program
Changed in this revision
diff -r 56c272cb4522 -r 0806f6996e8c CtrlStationRX.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/CtrlStationRX.lib Fri Nov 12 14:58:59 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/TakushimaYukimasa/code/CtrlStationRX/#9fb5088cd76f
diff -r 56c272cb4522 -r 0806f6996e8c SO1602A.lib --- a/SO1602A.lib Sat Oct 02 05:14:25 2021 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -https://os.mbed.com/users/AkinoriHashimoto/code/SO1602A/#b9955a5f760c
diff -r 56c272cb4522 -r 0806f6996e8c main.cpp
--- a/main.cpp Sat Oct 02 05:14:25 2021 +0000
+++ b/main.cpp Fri Nov 12 14:58:59 2021 +0000
@@ -1,191 +1,28 @@
#include "mbed.h"
-#include "SO1602A.h"
-#define SW_ONE(__NUM__) (SWData[__NUM__][0]-SWData[__NUM__][1]==1)
-
-DigitalIn SW[7]= {PA_3,PA_12,PB_5,PA_11,PA_8,PF_1,PF_0};
+#include "conste.h"
-DigitalOut POWER[2]= {PA_1,PB_1};
-
-Serial UART(PA_9,PA_10, 9600);
-
-DigitalOut led(LED1);
-Serial telemetry(USBTX, USBRX, 115200);
-
-SO1602A oled(PB_7,PB_6);
+CONSTE conste(PA_0,PA_1, 9600);
-Timer emoTimer;
-Timer restTimer;
-Timer newsTimer[3];
-Timer mainTimer;
-int msTimer=0;
+DigitalOut led(LED2);
+Serial telemetry(USBTX, USBRX, 9600);
-int newsStatus[3] = {0};
-
-void uartRX()
-{
- char data = UART.getc();
- newsStatus[(data>>2)&0b11]=(data&0b11);
- newsTimer[(data>>2)&0b11].reset();
-}
+/* 操縦権 0…停止 1…手動 1…自動 */
+int operate = 0;
+/* 座標移動追従出来ているか(現在座標と目標座標の大きさは小さいか) */
+int isFollow = 0;
int main()
{
-
- POWER[0]=POWER[1]=1;
- for(int i=0; i<7; i++) SW[i].mode(PullDown);
-
- mainTimer.reset();
- mainTimer.start();
- restTimer.start();
- emoTimer.start();
- for(int i=0; i<3; i++) newsTimer[i].start();
-
- oled.init();
- oled.setDispFlag(1,0,0);
-// oled.locate(2,0);
-// oled.printf("Stop display");
-
- UART.attach(uartRX, Serial::RxIrq);
+ // 識別番号 0…のあ 1…ねぷちゅーん 2…とりとん 4…リモコン1 6…リモコン2
+ conste.identificationNum = 0;
while(1) {
-
-// telemetry.printf("%d\n\r", (SW[6]==0?0:1));
-
- /* SW入力のワンタイム化 */
- static bool SWData[7][2] = {0};
- for(int i=0; i<7; i++) {
- SWData[i][1]=SWData[i][0];
- SWData[i][0]=SW[i];
- }
-
- /* 入力制限モード */
- static bool restFlag = 0;
- if(SW_ONE(0)) restFlag=!restFlag;
-
- /* メインタイマー制御 0-pause 1-start */
- static bool timerMode=0;
- if(!SW[1]) timerMode=0;
- if(!restFlag) {
- if(SW_ONE(2)) msTimer+=1000*60;
- if(SW_ONE(3)) msTimer+=1000*10;
- if(SW_ONE(4)) msTimer+=1000*1;
-
- if(SW[1]&&SW_ONE(5)) {
- timerMode=0;
- msTimer=0;
- }
- if(SW[1]&&SW_ONE(6)) timerMode=!timerMode;
- }
-
- /* 時間の更新 */
- static int temp=mainTimer.read_ms();
- if(timerMode) msTimer+=mainTimer.read_ms()-temp;
- temp=mainTimer.read_ms();
- if(msTimer>120e3) {
- timerMode=0;
- msTimer=120e3;
- }
-
- /* UART送受信関連 */
- uint8_t sendData[4] = {0};
- uint8_t checkSum = 0;
-
- //チェックサム計算
- for (int i = 0; i < 7; i++)
- checkSum += msTimer / (int)pow(10.0, (double)i) % 10;
-
- sendData[3] = 0x80;
- sendData[3] |= timerMode << 6;
- sendData[3] |= checkSum & 0x3F;
-
- //1バイト区切る
- for(int i = 0; i < 3; i++)
- sendData[i] = 0x7F & msTimer >> 7 * (2-i);
-
- for(int i = 0; i < 4; i++){
- UART.putc(sendData[i]);
- telemetry.putc(sendData[i]);
- }
-// telemetry.printf("%d \n\r",msTimer);
-
- for(int i=0; i<3; i++)
- if(newsTimer[i].read_ms()>500)
- newsStatus[i]=0;
-
-
-
- /* LCD描画 */
+ // ロボットの状態 1…モード異常 2…座標追従異常 3…正常
+ conste.status = (operate == 1) << 1 | (isFollow == 1);
+
+ /* 非常停止の時，モーターを止める */
+ if(conste.isEMO) /*MovMotor[i] = 0*/;
-// oled.locate(5,0);
-// oled.printf("%1d:%02d.%d ", msTimer/60000,msTimer/1000%60,msTimer/100%10);
-//
-// oled.locate(0,0);
-// int statious=!SW[1];
-// if(restFlag&&(SW[2]+SW[3]+SW[4]+SW[5]+SW[6])) restTimer.reset();
-// if(emoTimer.read()<1&&emoTimer.read_ms()%200<100) statious=0;
-// if(statious) oled.printf("EMO ");
-// else oled.printf(" ");
-//
-// oled.locate(12,0);
-// statious=restFlag;
-// if(!SW[1]&&(SW[5]+SW[6])) emoTimer.reset();
-// if(restTimer.read()<1&&restTimer.read_ms()%200<100) statious=0;
-// if(statious) oled.printf("REST");
-// else oled.printf(" ");
-
-
-// for(int i=0; i<3; i++) {
-// oled.locate(i*6,1);
-// if(newsStatus[i]==0) oled.printf("Lost");
-// if(newsStatus[i]==1) oled.printf("None");
-// if(newsStatus[i]==2) oled.printf("Yoki");
-// }
-// for(int i=0; i<2; i++) {
-// oled.locate(i*6+4,1);
-// oled.printf(" ");
-// }
-
- char printLCD[32] = {0};
-
- int statious=!SW[1];
- if(restFlag&&(SW[2]+SW[3]+SW[4]+SW[5]+SW[6])) restTimer.reset();
- if(emoTimer.read()<1&&emoTimer.read_ms()%200<100) statious=0;
- if(statious) strcat(printLCD,"EMO ");
- else strcat(printLCD," ");
-
- char tmp[17] = {0};
- sprintf(tmp,"%1d:%02d.%d ", msTimer/60000,msTimer/1000%60,msTimer/100%10);
- strcat(printLCD,tmp);
-
- statious=restFlag;
- if(!SW[1]&&(SW[5]+SW[6])) emoTimer.reset();
- if(restTimer.read()<1&&restTimer.read_ms()%200<100) statious=0;
- if(statious) strcat(printLCD,"REST");
- else strcat(printLCD," ");
-
- const char stat[3][5] = {"Lost","None","Yoki"};
- oled.locate(0,1);
- sprintf(tmp,"%s %s %s",stat[newsStatus[0]],stat[newsStatus[1]],stat[newsStatus[2]]);
- strcat(printLCD,tmp);
-
- oled.locate(0,0);
- oled.printf(printLCD);
-
-// for(int i=0; i<3; i++) {
-// oled.locate(i*6,1);
-// if(newsStatus[i]==0) oled.printf("Lost");
-// if(newsStatus[i]==1) oled.printf("None");
-// if(newsStatus[i]==2) oled.printf("Yoki");
-// }
-// for(int i=0; i<2; i++) {
-// oled.locate(i*6+4,1);
-// oled.printf(" ");
-// }
-//static Timer tim;
-//tim.start();
-//telemetry.printf("%d \n\r",tim.read_ms());
-//tim.reset();
-
-
+ telemetry.printf("%d\n\r",conste.timeMs);
}
}