Diff: main.cpp

Revision:

0:f3625799a474

Child:

1:85bc939a3d22

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Sep 01 02:47:08 2021 +0000
@@ -0,0 +1,227 @@
+#define LAP                     1
+#define SMB                     0
+#define EncoderMAX  2
+
+#include "mbed.h"
+#include "CANMsg.h"
+
+/*************クラス宣言*************/
+
+/* CANのピン設定  (RD,TD,周波数) */
+CAN   can(PA_11, PA_12,500000);// CAN Rx pin name, CAN Tx pin name
+/* CAN受信クラス宣言 */
+CANMsg  rxMsg;
+/* CAN送信クラス宣言 */
+CANMsg  txMsg;
+/* MDCのOnボードled */
+DigitalOut   led[2]= {PA_8,PA_9};
+/* 割り込み用クラス */
+Ticker flipper;
+/* 割り込み用クラス */
+Ticker interrupt;
+/* I2C障害時間カウント */
+Timer LostSignal;
+
+/*************関数宣言*************/
+
+/* タイマ呼び出し用 */
+void IT_CallBack(void);
+/* 送信用割り込み関数 */
+void sent(void);
+/* MDの状態決める関数 */
+void Md_Status(void);
+/* CAN受信割り込み */
+void onCanReceived(void);
+
+/*************ピン宣言*************/
+
+/* エンコーダピン設定 */
+DigitalOut CS(PA_3);
+DigitalOut CL(PA_4);
+DigitalIn  DO[] = {PA_5,PB_0};
+/* MD側出力ピン設定 */
+PwmOut PWM[2] = {PA_7,PA_6};
+/* いねーぶるピン設定 */
+DigitalOut Enable[2] = {PA_0,PB_4};
+/* INのようなOUTピン設定 */
+DigitalOut IN[4]     = {PA_1,PA_2,PB_3,PA_15};
+/* アドレスピン設定 */
+DigitalIn  add[3]    = {PB_7,PB_6,PB_5};
+
+
+/*************変数宣言*************/
+
+/* MDデータ保存用構造体 */
+struct m_d {
+    bool mode;
+    bool enable;
+    int pwm;
+    int code;
+};
+/* 受信データ一時保存/保存 */
+char ReadData = 0;
+/* mdデータ保存 */
+char MdData = 0;
+/* モータの状態一時保存/保存 */
+struct m_d mdc[2]= {0};
+/* エンコーダ */
+static int data[EncoderMAX] = {0};
+/*  エンコーダのデータ入力  */
+int32_t  enc_data[2]= {0};
+/* 直読みエンコーダ角度保存(degree) */
+double EncoderDeg[EncoderMAX] = {0};
+/* アドレス初期化 */
+int  RX_ID = 50;
+int  TX_ID = 0;
+
+int main(void)
+{
+    /* アドレス代入 */
+    TX_ID += !add[2]<<2|!add[1]<<1|!add[0];
+    RX_ID += !add[2]<<2|!add[1]<<1|!add[0];
+
+    /* can受信割り込み */
+    can.attach(onCanReceived);
+
+    /* pwm周期 */
+    PWM[0].period_us(50);
+    PWM[1].period_us(50);
+
+    /* 通信キレ感知ダイマースタート */
+    LostSignal.reset();
+    LostSignal.start();
+
+    /* エンコーダタイマ割り込み */
+    flipper.attach_us(&IT_CallBack, 100);
+    /* 送信用タイマ割り込み */
+    interrupt.attach_us(&sent, 2600);
+
+    while(1) {
+
+        for(int i=0; i<2; i++) {
+            /* SMBの時 */
+            if(mdc[i].mode==0) {
+                PWM[i] = mdc[i].pwm /100.0;
+
+                if(mdc[i].code==1) {
+                    MdData |= (1<<!i*3+1);
+                    MdData &= ~(1<<!i*3+2);
+                } else {
+                    MdData |= (1<<!i*3+2);
+                    MdData &= ~(1<<!i*3+1);
+                }
+            }
+            /* LAPの時 */
+            else {
+                PWM[i] = (mdc[i].pwm*mdc[i].code+100) /200.0;
+                MdData |= 0b111<<!i*3;
+            }
+        }
+
+        /* INの状態を決める */
+        Md_Status();
+
+        /* 信号ロスト1000msで停止 */
+        if (LostSignal.read_ms() > 1000) {
+            Enable[0] = 0;
+            Enable[1] = 0;
+            led[0]=0;
+        } else  led[0]=1;
+    }
+}
+
+/*************受信割り込み関数************/
+void onCanReceived(void)
+{
+    can.read(rxMsg);
+
+    if (rxMsg.id == RX_ID) {
+
+        /* LAP or SMB　を決めるデータ */
+        ReadData = rxMsg.data[0];
+        /* LAP or SMB 保存 */
+        mdc[0].mode = ReadData&1;
+        mdc[1].mode = (ReadData>>1)&1;
+        /* PWM1 */
+        mdc[0].code = (rxMsg.data[1]&(1<<7)) != 0 ? -1 : 1;//負のビット検知
+        mdc[0].pwm  = rxMsg.data[1]&~(1<<7);
+        /* PWM2 */
+        mdc[1].code = (rxMsg.data[2]&(1<<7)) != 0 ? -1 : 1;//負のビット検知
+        mdc[1].pwm  = rxMsg.data[2]&~(1<<7);
+        /* 信号ロストタイマリセット */
+        LostSignal.reset();
+        LostSignal.start();
+    }
+}
+
+/*************エンコーダ取得タイマ割り込み************/
+void IT_CallBack(void)
+{
+    static int cnt = 0;
+
+    switch(cnt) {
+        /* 最初の処理 */
+        case 0:
+            data[0] = 0;
+            data[1] = 0;
+            CS = 0;
+            CL = 1;
+            break;
+        /* 最後の処理 */
+        case 25:
+            CS=1;
+        /* 通常の処理 */
+        default:
+            CL=!CL;
+            /* 最初でも最後でもなく奇数回で最初以外の時読み取り処理 */
+            if(cnt != 1 && cnt % 2) {
+                data[0] |= (DO[0]==1);
+                data[1] |= (DO[1]==1);
+
+                data[0] = data[0] << 1;
+                data[1] = data[1] << 1;
+            }
+            break;
+    }
+    cnt++;
+    cnt%=26;
+}
+
+/*************MDのステータスを決める君***********/
+void  Md_Status(void)
+{
+    /* PWMが0でないまたはSMBでないとき */
+    if(mdc[0].pwm!=0||mdc[0].mode) {
+        IN[3] = (MdData&(1<<5)) !=0 ? 1 : 0;//1-2
+        IN[2] = (MdData&(1<<4)) !=0 ? 1 : 0;//1-1
+    } else  {
+        IN[3] =  0;//1-2
+        IN[2] =  0;//1-1
+    }
+    /* PWMが0でないまたはSMBでないとき */
+    if(mdc[1].pwm!=0||mdc[1].mode) {
+        IN[1] = (MdData&(1<<2)) !=0 ? 1 : 0;//2-2
+        IN[0] = (MdData&(1<<1)) !=0 ? 1 : 0;//2-1
+    } else  {
+        IN[1] =  0;//2-2
+        IN[0] =  0;//2-1
+    }
+
+    Enable[0] = 1;
+    Enable[1] = 1;
+}
+
+/*************メインマイコンにデータ送信用関数************/
+void sent(void)
+{
+    txMsg.len=3;
+    txMsg.id = TX_ID;
+    static int cnt =0;
+    cnt++;
+    cnt%=1000;
+    //uint8_t Enc_Data[2];
+
+    for(int i=0; i<3; i++)
+        txMsg.data[i]=((data[0]<<12|data[1])>>i*8)&0xFF;
+    can.write(txMsg);
+}