史也 菅 / Mbed 2 deprecated can_nodo

way

Dependencies:   mbed CANMsg

main.cpp@1:85bc939a3d22, 2021-09-08 (annotated)

Committer:
kurobikari
Date:
Wed Sep 08 00:47:38 2021 +0000
Revision:
1:85bc939a3d22
Parent:
0:f3625799a474 
ver2;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kurobikari 0:f3625799a474 1 #define LAP 1
kurobikari 0:f3625799a474 2 #define SMB 0
kurobikari 1:85bc939a3d22 3 #define EncoderMAX 2
kurobikari 0:f3625799a474 4
kurobikari 0:f3625799a474 5 #include "mbed.h"
kurobikari 0:f3625799a474 6 #include "CANMsg.h"
kurobikari 0:f3625799a474 7
kurobikari 0:f3625799a474 8 /*************クラス宣言*************/
kurobikari 0:f3625799a474 9
kurobikari 0:f3625799a474 10 /* CANのピン設定 (RD,TD,周波数) */
kurobikari 0:f3625799a474 11 CAN can(PA_11, PA_12,500000);// CAN Rx pin name, CAN Tx pin name
kurobikari 0:f3625799a474 12 /* CAN受信クラス宣言 */
kurobikari 0:f3625799a474 13 CANMsg rxMsg;
kurobikari 0:f3625799a474 14 /* CAN送信クラス宣言 */
kurobikari 0:f3625799a474 15 CANMsg txMsg;
kurobikari 0:f3625799a474 16 /* MDCのOnボードled */
kurobikari 0:f3625799a474 17 DigitalOut led[2]= {PA_8,PA_9};
kurobikari 0:f3625799a474 18 /* 割り込み用クラス */
kurobikari 0:f3625799a474 19 Ticker flipper;
kurobikari 0:f3625799a474 20 /* 割り込み用クラス */
kurobikari 0:f3625799a474 21 Ticker interrupt;
kurobikari 0:f3625799a474 22 /* I2C障害時間カウント */
kurobikari 0:f3625799a474 23 Timer LostSignal;
kurobikari 0:f3625799a474 24
kurobikari 0:f3625799a474 25 /*************関数宣言*************/
kurobikari 0:f3625799a474 26
kurobikari 0:f3625799a474 27 /* タイマ呼び出し用 */
kurobikari 0:f3625799a474 28 void IT_CallBack(void);
kurobikari 0:f3625799a474 29 /* 送信用割り込み関数 */
kurobikari 0:f3625799a474 30 void sent(void);
kurobikari 0:f3625799a474 31 /* MDの状態決める関数 */
kurobikari 0:f3625799a474 32 void Md_Status(void);
kurobikari 0:f3625799a474 33 /* CAN受信割り込み */
kurobikari 0:f3625799a474 34 void onCanReceived(void);
kurobikari 0:f3625799a474 35
kurobikari 0:f3625799a474 36 /*************ピン宣言*************/
kurobikari 0:f3625799a474 37
kurobikari 0:f3625799a474 38 /* エンコーダピン設定 */
kurobikari 0:f3625799a474 39 DigitalOut CS(PA_3);
kurobikari 0:f3625799a474 40 DigitalOut CL(PA_4);
kurobikari 0:f3625799a474 41 DigitalIn DO[] = {PA_5,PB_0};
kurobikari 0:f3625799a474 42 /* MD側出力ピン設定 */
kurobikari 0:f3625799a474 43 PwmOut PWM[2] = {PA_7,PA_6};
kurobikari 0:f3625799a474 44 /* いねーぶるピン設定 */
kurobikari 0:f3625799a474 45 DigitalOut Enable[2] = {PA_0,PB_4};
kurobikari 0:f3625799a474 46 /* INのようなOUTピン設定 */
kurobikari 0:f3625799a474 47 DigitalOut IN[4] = {PA_1,PA_2,PB_3,PA_15};
kurobikari 0:f3625799a474 48 /* アドレスピン設定 */
kurobikari 0:f3625799a474 49 DigitalIn add[3] = {PB_7,PB_6,PB_5};
kurobikari 0:f3625799a474 50
kurobikari 0:f3625799a474 51
kurobikari 0:f3625799a474 52 /*************変数宣言*************/
kurobikari 0:f3625799a474 53
kurobikari 0:f3625799a474 54 /* MDデータ保存用構造体 */
kurobikari 0:f3625799a474 55 struct m_d {
kurobikari 0:f3625799a474 56 bool mode;
kurobikari 0:f3625799a474 57 bool enable;
kurobikari 0:f3625799a474 58 int pwm;
kurobikari 0:f3625799a474 59 int code;
kurobikari 0:f3625799a474 60 };
kurobikari 0:f3625799a474 61 /* 受信データ一時保存/保存 */
kurobikari 0:f3625799a474 62 char ReadData = 0;
kurobikari 0:f3625799a474 63 /* mdデータ保存 */
kurobikari 0:f3625799a474 64 char MdData = 0;
kurobikari 0:f3625799a474 65 /* モータの状態一時保存/保存 */
kurobikari 0:f3625799a474 66 struct m_d mdc[2]= {0};
kurobikari 0:f3625799a474 67 /* エンコーダ */
kurobikari 0:f3625799a474 68 static int data[EncoderMAX] = {0};
kurobikari 1:85bc939a3d22 69 int sendData[2]={0};
kurobikari 0:f3625799a474 70 /* エンコーダのデータ入力 */
kurobikari 0:f3625799a474 71 int32_t enc_data[2]= {0};
kurobikari 0:f3625799a474 72 /* 直読みエンコーダ角度保存(degree) */
kurobikari 0:f3625799a474 73 double EncoderDeg[EncoderMAX] = {0};
kurobikari 0:f3625799a474 74 /* アドレス初期化 */
kurobikari 0:f3625799a474 75 int RX_ID = 50;
kurobikari 0:f3625799a474 76 int TX_ID = 0;
kurobikari 0:f3625799a474 77
kurobikari 0:f3625799a474 78 int main(void)
kurobikari 0:f3625799a474 79 {
kurobikari 0:f3625799a474 80 /* アドレス代入 */
kurobikari 0:f3625799a474 81 TX_ID += !add[2]<<2|!add[1]<<1|!add[0];
kurobikari 0:f3625799a474 82 RX_ID += !add[2]<<2|!add[1]<<1|!add[0];
kurobikari 0:f3625799a474 83
kurobikari 0:f3625799a474 84 /* can受信割り込み */
kurobikari 0:f3625799a474 85 can.attach(onCanReceived);
kurobikari 0:f3625799a474 86
kurobikari 0:f3625799a474 87 /* pwm周期 */
kurobikari 0:f3625799a474 88 PWM[0].period_us(50);
kurobikari 0:f3625799a474 89 PWM[1].period_us(50);
kurobikari 0:f3625799a474 90
kurobikari 0:f3625799a474 91 /* 通信キレ感知ダイマースタート */
kurobikari 0:f3625799a474 92 LostSignal.reset();
kurobikari 0:f3625799a474 93 LostSignal.start();
kurobikari 0:f3625799a474 94
kurobikari 0:f3625799a474 95 /* エンコーダタイマ割り込み */
kurobikari 0:f3625799a474 96 flipper.attach_us(&IT_CallBack, 100);
kurobikari 0:f3625799a474 97 /* 送信用タイマ割り込み */
kurobikari 0:f3625799a474 98 interrupt.attach_us(&sent, 2600);
kurobikari 0:f3625799a474 99
kurobikari 0:f3625799a474 100 while(1) {
kurobikari 0:f3625799a474 101
kurobikari 0:f3625799a474 102 for(int i=0; i<2; i++) {
kurobikari 0:f3625799a474 103 /* SMBの時 */
kurobikari 0:f3625799a474 104 if(mdc[i].mode==0) {
kurobikari 0:f3625799a474 105 PWM[i] = mdc[i].pwm /100.0;
kurobikari 0:f3625799a474 106
kurobikari 0:f3625799a474 107 if(mdc[i].code==1) {
kurobikari 0:f3625799a474 108 MdData |= (1<<!i*3+1);
kurobikari 0:f3625799a474 109 MdData &= ~(1<<!i*3+2);
kurobikari 0:f3625799a474 110 } else {
kurobikari 0:f3625799a474 111 MdData |= (1<<!i*3+2);
kurobikari 0:f3625799a474 112 MdData &= ~(1<<!i*3+1);
kurobikari 0:f3625799a474 113 }
kurobikari 0:f3625799a474 114 }
kurobikari 0:f3625799a474 115 /* LAPの時 */
kurobikari 0:f3625799a474 116 else {
kurobikari 1:85bc939a3d22 117 // if(mdc[i].pwm==0)mdc[i].pwm=5;
kurobikari 0:f3625799a474 118 PWM[i] = (mdc[i].pwm*mdc[i].code+100) /200.0;
kurobikari 1:85bc939a3d22 119 if(PWM[i]==0)PWM[i]=5;
kurobikari 0:f3625799a474 120 MdData |= 0b111<<!i*3;
kurobikari 0:f3625799a474 121 }
kurobikari 0:f3625799a474 122 }
kurobikari 0:f3625799a474 123
kurobikari 1:85bc939a3d22 124 /* MDの状態を決める */
kurobikari 0:f3625799a474 125 Md_Status();
kurobikari 0:f3625799a474 126
kurobikari 0:f3625799a474 127 /* 信号ロスト1000msで停止 */
kurobikari 0:f3625799a474 128 if (LostSignal.read_ms() > 1000) {
kurobikari 0:f3625799a474 129 Enable[0] = 0;
kurobikari 0:f3625799a474 130 Enable[1] = 0;
kurobikari 0:f3625799a474 131 led[0]=0;
kurobikari 0:f3625799a474 132 } else led[0]=1;
kurobikari 0:f3625799a474 133 }
kurobikari 0:f3625799a474 134 }
kurobikari 0:f3625799a474 135
kurobikari 0:f3625799a474 136 /*************受信割り込み関数************/
kurobikari 0:f3625799a474 137 void onCanReceived(void)
kurobikari 0:f3625799a474 138 {
kurobikari 0:f3625799a474 139 can.read(rxMsg);
kurobikari 0:f3625799a474 140
kurobikari 0:f3625799a474 141 if (rxMsg.id == RX_ID) {
kurobikari 0:f3625799a474 142
kurobikari 0:f3625799a474 143 /* LAP or SMB　を決めるデータ */
kurobikari 0:f3625799a474 144 ReadData = rxMsg.data[0];
kurobikari 0:f3625799a474 145 /* LAP or SMB 保存 */
kurobikari 0:f3625799a474 146 mdc[0].mode = ReadData&1;
kurobikari 0:f3625799a474 147 mdc[1].mode = (ReadData>>1)&1;
kurobikari 0:f3625799a474 148 /* PWM1 */
kurobikari 0:f3625799a474 149 mdc[0].code = (rxMsg.data[1]&(1<<7)) != 0 ? -1 : 1;//負のビット検知
kurobikari 0:f3625799a474 150 mdc[0].pwm = rxMsg.data[1]&~(1<<7);
kurobikari 0:f3625799a474 151 /* PWM2 */
kurobikari 0:f3625799a474 152 mdc[1].code = (rxMsg.data[2]&(1<<7)) != 0 ? -1 : 1;//負のビット検知
kurobikari 0:f3625799a474 153 mdc[1].pwm = rxMsg.data[2]&~(1<<7);
kurobikari 0:f3625799a474 154 /* 信号ロストタイマリセット */
kurobikari 0:f3625799a474 155 LostSignal.reset();
kurobikari 0:f3625799a474 156 LostSignal.start();
kurobikari 0:f3625799a474 157 }
kurobikari 0:f3625799a474 158 }
kurobikari 0:f3625799a474 159
kurobikari 0:f3625799a474 160 /*************エンコーダ取得タイマ割り込み************/
kurobikari 0:f3625799a474 161 void IT_CallBack(void)
kurobikari 0:f3625799a474 162 {
kurobikari 0:f3625799a474 163 static int cnt = 0;
kurobikari 0:f3625799a474 164
kurobikari 0:f3625799a474 165 switch(cnt) {
kurobikari 0:f3625799a474 166 /* 最初の処理 */
kurobikari 0:f3625799a474 167 case 0:
kurobikari 0:f3625799a474 168 data[0] = 0;
kurobikari 0:f3625799a474 169 data[1] = 0;
kurobikari 0:f3625799a474 170 CS = 0;
kurobikari 0:f3625799a474 171 CL = 1;
kurobikari 0:f3625799a474 172 break;
kurobikari 0:f3625799a474 173 /* 最後の処理 */
kurobikari 0:f3625799a474 174 case 25:
kurobikari 0:f3625799a474 175 CS=1;
kurobikari 1:85bc939a3d22 176 sendData[0]=data[0];
kurobikari 1:85bc939a3d22 177 sendData[1]=data[1];
kurobikari 0:f3625799a474 178 /* 通常の処理 */
kurobikari 0:f3625799a474 179 default:
kurobikari 0:f3625799a474 180 CL=!CL;
kurobikari 0:f3625799a474 181 /* 最初でも最後でもなく奇数回で最初以外の時読み取り処理 */
kurobikari 0:f3625799a474 182 if(cnt != 1 && cnt % 2) {
kurobikari 0:f3625799a474 183 data[0] |= (DO[0]==1);
kurobikari 0:f3625799a474 184 data[1] |= (DO[1]==1);
kurobikari 0:f3625799a474 185
kurobikari 0:f3625799a474 186 data[0] = data[0] << 1;
kurobikari 0:f3625799a474 187 data[1] = data[1] << 1;
kurobikari 0:f3625799a474 188 }
kurobikari 0:f3625799a474 189 break;
kurobikari 0:f3625799a474 190 }
kurobikari 0:f3625799a474 191 cnt++;
kurobikari 0:f3625799a474 192 cnt%=26;
kurobikari 0:f3625799a474 193 }
kurobikari 0:f3625799a474 194
kurobikari 0:f3625799a474 195 /*************MDのステータスを決める君***********/
kurobikari 0:f3625799a474 196 void Md_Status(void)
kurobikari 0:f3625799a474 197 {
kurobikari 0:f3625799a474 198 /* PWMが0でないまたはSMBでないとき */
kurobikari 0:f3625799a474 199 if(mdc[0].pwm!=0||mdc[0].mode) {
kurobikari 0:f3625799a474 200 IN[3] = (MdData&(1<<5)) !=0 ? 1 : 0;//1-2
kurobikari 0:f3625799a474 201 IN[2] = (MdData&(1<<4)) !=0 ? 1 : 0;//1-1
kurobikari 0:f3625799a474 202 } else {
kurobikari 0:f3625799a474 203 IN[3] = 0;//1-2
kurobikari 0:f3625799a474 204 IN[2] = 0;//1-1
kurobikari 0:f3625799a474 205 }
kurobikari 0:f3625799a474 206 /* PWMが0でないまたはSMBでないとき */
kurobikari 0:f3625799a474 207 if(mdc[1].pwm!=0||mdc[1].mode) {
kurobikari 0:f3625799a474 208 IN[1] = (MdData&(1<<2)) !=0 ? 1 : 0;//2-2
kurobikari 0:f3625799a474 209 IN[0] = (MdData&(1<<1)) !=0 ? 1 : 0;//2-1
kurobikari 0:f3625799a474 210 } else {
kurobikari 0:f3625799a474 211 IN[1] = 0;//2-2
kurobikari 0:f3625799a474 212 IN[0] = 0;//2-1
kurobikari 0:f3625799a474 213 }
kurobikari 0:f3625799a474 214
kurobikari 0:f3625799a474 215 Enable[0] = 1;
kurobikari 0:f3625799a474 216 Enable[1] = 1;
kurobikari 0:f3625799a474 217 }
kurobikari 0:f3625799a474 218
kurobikari 0:f3625799a474 219 /*************メインマイコンにデータ送信用関数************/
kurobikari 0:f3625799a474 220 void sent(void)
kurobikari 0:f3625799a474 221 {
kurobikari 0:f3625799a474 222 txMsg.len=3;
kurobikari 0:f3625799a474 223 txMsg.id = TX_ID;
kurobikari 0:f3625799a474 224 static int cnt =0;
kurobikari 0:f3625799a474 225 cnt++;
kurobikari 0:f3625799a474 226 cnt%=1000;
kurobikari 0:f3625799a474 227 //uint8_t Enc_Data[2];
kurobikari 0:f3625799a474 228
kurobikari 0:f3625799a474 229 for(int i=0; i<3; i++)
kurobikari 1:85bc939a3d22 230 txMsg.data[i]=((sendData[0]<<12|sendData[1])>>i*8)&0xFF;
kurobikari 1:85bc939a3d22 231
kurobikari 0:f3625799a474 232 can.write(txMsg);
kurobikari 0:f3625799a474 233 }