Kanta Takasu
/
YUKA_tks
yuka_tks
main.cpp@5:f21e1a75f98b, 2021-12-20 (annotated)
- Committer:
- Tomo1213
- Date:
- Mon Dec 20 14:13:29 2021 +0000
- Revision:
- 5:f21e1a75f98b
- Parent:
- 4:1bd08c9d92a9
- Child:
- 6:dd35cc3b5ca0
itibanumakuitteru
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Tomo1213 | 4:1bd08c9d92a9 | 1 | //2021/12/20更新 |
ngokystk | 0:b4b94eb28093 | 2 | //YUKA本番機用プログラム |
ngokystk | 0:b4b94eb28093 | 3 | //入力切替確認済み |
ngokystk | 0:b4b94eb28093 | 4 | //一定時間の入力なしで動作切替 |
ngokystk | 0:b4b94eb28093 | 5 | //ジグザグ動作実装前 |
ngokystk | 0:b4b94eb28093 | 6 | //エンコーダ読み取りによる入力 |
ngokystk | 0:b4b94eb28093 | 7 | //加減速度調整パラメータ実装 |
ngokystk | 0:b4b94eb28093 | 8 | //PID制御による機体角度補正_ |
Tomo1213 | 3:c39c14cfc811 | 9 | //aoki |
Tomo1213 | 1:b2bd1511307e | 10 | |
ngokystk | 0:b4b94eb28093 | 11 | #include "mbed.h" |
ngokystk | 0:b4b94eb28093 | 12 | #include "hcsr04.h" |
ngokystk | 0:b4b94eb28093 | 13 | |
Tomo1213 | 4:1bd08c9d92a9 | 14 | #define RPM_RIDE 400 |
Tomo1213 | 4:1bd08c9d92a9 | 15 | #define RPM_CLEAN 400 |
Tomo1213 | 4:1bd08c9d92a9 | 16 | #define ACC_RIDE 1000 |
Tomo1213 | 4:1bd08c9d92a9 | 17 | #define DEC_RIDE 700 |
Tomo1213 | 4:1bd08c9d92a9 | 18 | #define ACC_CLEAN 1000 |
Tomo1213 | 4:1bd08c9d92a9 | 19 | #define DEC_CLEAN 1000 |
ngokystk | 0:b4b94eb28093 | 20 | |
Tomo1213 | 1:b2bd1511307e | 21 | #define KICK 2000 |
Tomo1213 | 4:1bd08c9d92a9 | 22 | #define CLEAN_OFFSET 6000 |
Tomo1213 | 1:b2bd1511307e | 23 | |
Tomo1213 | 4:1bd08c9d92a9 | 24 | #define WALL 45 |
Tomo1213 | 4:1bd08c9d92a9 | 25 | #define WALL_MIN 25 |
Tomo1213 | 4:1bd08c9d92a9 | 26 | #define Standby_Time 10 |
Tomo1213 | 4:1bd08c9d92a9 | 27 | #define GETOFF_TIME 3 |
Tomo1213 | 3:c39c14cfc811 | 28 | |
ngokystk | 0:b4b94eb28093 | 29 | DigitalOut led1(LED1); |
ngokystk | 0:b4b94eb28093 | 30 | DigitalOut led2(LED2); |
ngokystk | 0:b4b94eb28093 | 31 | DigitalOut led3(LED3); |
ngokystk | 0:b4b94eb28093 | 32 | DigitalOut led4(LED4); |
Tomo1213 | 1:b2bd1511307e | 33 | |
ngokystk | 0:b4b94eb28093 | 34 | Timer t; |
ngokystk | 0:b4b94eb28093 | 35 | double Time = 0; |
Tomo1213 | 4:1bd08c9d92a9 | 36 | |
ngokystk | 0:b4b94eb28093 | 37 | Serial pc(USBTX, USBRX); |
Tomo1213 | 4:1bd08c9d92a9 | 38 | Serial LED(PB_6,PB_7); |
ngokystk | 0:b4b94eb28093 | 39 | char Serialdata; |
ngokystk | 0:b4b94eb28093 | 40 | BusOut myled(LED1, LED2, LED3, LED4); |
Tomo1213 | 1:b2bd1511307e | 41 | |
ngokystk | 0:b4b94eb28093 | 42 | //HCSR04 u1(p13, p14), u2(p11, p12), u3(p23, p24), u4(p25, p26); // Trigger(DO), Echo(PWMIN); LPC1768 |
ngokystk | 0:b4b94eb28093 | 43 | HCSR04 u1(PA_0, PA_1),u2(PA_5, PA_6),u3(PB_0, PA_10); // Trigger(DO), Echo(PWMIN); f303k8 |
Tomo1213 | 1:b2bd1511307e | 44 | |
ngokystk | 0:b4b94eb28093 | 45 | CANMessage canmsgTx; |
ngokystk | 0:b4b94eb28093 | 46 | CANMessage canmsgRx; |
ngokystk | 0:b4b94eb28093 | 47 | //CAN canPort(p30, p29); //CAN name(PinName rd, PinName td) LPC1768 |
ngokystk | 0:b4b94eb28093 | 48 | CAN canPort(PA_11, PA_12); //CAN name(PinName rd, PinName td) F303k8 |
Tomo1213 | 1:b2bd1511307e | 49 | |
ngokystk | 0:b4b94eb28093 | 50 | //プロトタイプ宣言 |
ngokystk | 0:b4b94eb28093 | 51 | //------------------send関数------------------- |
ngokystk | 0:b4b94eb28093 | 52 | //mode Setting |
ngokystk | 0:b4b94eb28093 | 53 | void sendOPModeT(int); //Operating Mode |
ngokystk | 0:b4b94eb28093 | 54 | void sendOPModeV(int); //Operating Mode |
ngokystk | 0:b4b94eb28093 | 55 | //Control Word |
ngokystk | 0:b4b94eb28093 | 56 | void sendCtrlRS(int); //Reset |
ngokystk | 0:b4b94eb28093 | 57 | void sendCtrlSD(int); //Shutdown |
ngokystk | 0:b4b94eb28093 | 58 | void sendCtrlEN(int); //Switch on & Enable |
ngokystk | 0:b4b94eb28093 | 59 | void sendCtrlQS(int); //Quick Stop |
ngokystk | 0:b4b94eb28093 | 60 | void sendCtrlHL(int); //Halt |
ngokystk | 0:b4b94eb28093 | 61 | //Velocity Setting |
ngokystk | 0:b4b94eb28093 | 62 | void sendTgtVel(int,int); //Target Velocity |
ngokystk | 0:b4b94eb28093 | 63 | //Torque Setting |
ngokystk | 0:b4b94eb28093 | 64 | void sendTgtTrq(int,int); //Target Torque |
ngokystk | 0:b4b94eb28093 | 65 | //Acceleration Setting |
ngokystk | 0:b4b94eb28093 | 66 | void sendProAcc(int,int); //Plof Acceleration |
ngokystk | 0:b4b94eb28093 | 67 | void sendProDec(int,int); //Plof Deceleration |
ngokystk | 0:b4b94eb28093 | 68 | //------------------read関数------------------- |
ngokystk | 0:b4b94eb28093 | 69 | void readActVel(int); //Actual Velocity |
ngokystk | 0:b4b94eb28093 | 70 | void readActPos(int); //Actual Position |
ngokystk | 0:b4b94eb28093 | 71 | //-------------------その他-------------------- |
ngokystk | 0:b4b94eb28093 | 72 | void printCANTX(void); //CAN送信データをPCに表示 |
ngokystk | 0:b4b94eb28093 | 73 | void printCANRX(void); //CAN受信データをPCに表示 |
ngokystk | 0:b4b94eb28093 | 74 | void CANdataRX(void); //CAN受信処理 |
ngokystk | 0:b4b94eb28093 | 75 | void SerialRX(void); //Serial受信処理 |
Tomo1213 | 1:b2bd1511307e | 76 | //--------------------------------------------- |
Tomo1213 | 1:b2bd1511307e | 77 | void set_ACC(int); |
Tomo1213 | 1:b2bd1511307e | 78 | void set_DEC(int); |
Tomo1213 | 1:b2bd1511307e | 79 | void set_MODE_V(void); |
Tomo1213 | 1:b2bd1511307e | 80 | void set_MODE_T(void); |
ngokystk | 0:b4b94eb28093 | 81 | |
Tomo1213 | 4:1bd08c9d92a9 | 82 | void vel_stop(void); |
Tomo1213 | 4:1bd08c9d92a9 | 83 | |
Tomo1213 | 4:1bd08c9d92a9 | 84 | void vel_forward(int); |
Tomo1213 | 4:1bd08c9d92a9 | 85 | void vel_forward_con(int); |
Tomo1213 | 4:1bd08c9d92a9 | 86 | void vel_backward(int); |
Tomo1213 | 4:1bd08c9d92a9 | 87 | void vel_backward_con(int); |
Tomo1213 | 4:1bd08c9d92a9 | 88 | void vel_right(int); |
Tomo1213 | 4:1bd08c9d92a9 | 89 | void vel_right_con(int); |
Tomo1213 | 4:1bd08c9d92a9 | 90 | void vel_left(int); |
Tomo1213 | 4:1bd08c9d92a9 | 91 | |
Tomo1213 | 4:1bd08c9d92a9 | 92 | //unsigned int get_cm_n(HCSR04, unsigned int); |
Tomo1213 | 4:1bd08c9d92a9 | 93 | //USE -> unsigned int dist_UnitA = get_cm_n(u2, 5); |
Tomo1213 | 4:1bd08c9d92a9 | 94 | unsigned int get_cm_n(HCSR04 &echo_unit,int echo_n){ |
Tomo1213 | 4:1bd08c9d92a9 | 95 | unsigned int sampled_dist=0; |
Tomo1213 | 4:1bd08c9d92a9 | 96 | for (int iter_n = 0; iter_n <echo_n; iter_n++){ |
Tomo1213 | 4:1bd08c9d92a9 | 97 | echo_unit.start(); |
Tomo1213 | 4:1bd08c9d92a9 | 98 | sampled_dist += echo_unit.get_dist_cm(); |
Tomo1213 | 4:1bd08c9d92a9 | 99 | } |
Tomo1213 | 4:1bd08c9d92a9 | 100 | return (sampled_dist / echo_n); |
Tomo1213 | 4:1bd08c9d92a9 | 101 | } |
Tomo1213 | 4:1bd08c9d92a9 | 102 | |
ngokystk | 0:b4b94eb28093 | 103 | int nodeall=4; |
Tomo1213 | 1:b2bd1511307e | 104 | |
ngokystk | 0:b4b94eb28093 | 105 | int main(){ |
ngokystk | 0:b4b94eb28093 | 106 | //Serial |
ngokystk | 0:b4b94eb28093 | 107 | pc.attach(SerialRX); |
ngokystk | 0:b4b94eb28093 | 108 | //pc.baud(115200); |
ngokystk | 0:b4b94eb28093 | 109 | |
ngokystk | 0:b4b94eb28093 | 110 | //CAN |
ngokystk | 0:b4b94eb28093 | 111 | canPort.frequency(1000000); //Bit Rate:1MHz |
ngokystk | 0:b4b94eb28093 | 112 | canPort.attach(CANdataRX,CAN::RxIrq); |
ngokystk | 0:b4b94eb28093 | 113 | int node1 = 1; //CAN node Setting |
ngokystk | 0:b4b94eb28093 | 114 | int node2 = 2; |
ngokystk | 0:b4b94eb28093 | 115 | int node3 = 3; |
ngokystk | 0:b4b94eb28093 | 116 | int node4 = 4; |
ngokystk | 0:b4b94eb28093 | 117 | |
ngokystk | 0:b4b94eb28093 | 118 | //エンコーダ関係 |
ngokystk | 0:b4b94eb28093 | 119 | int ActPos = 0; |
ngokystk | 0:b4b94eb28093 | 120 | int Init_Pos = 0; |
ngokystk | 0:b4b94eb28093 | 121 | |
ngokystk | 0:b4b94eb28093 | 122 | //超音波センサ関係パラメータ |
Tomo1213 | 4:1bd08c9d92a9 | 123 | int dist1,dist2,dist3,dist4; |
ngokystk | 0:b4b94eb28093 | 124 | |
ngokystk | 0:b4b94eb28093 | 125 | //PID制御関係 |
ngokystk | 0:b4b94eb28093 | 126 | //角度調整パラメータ |
Tomo1213 | 4:1bd08c9d92a9 | 127 | |
Tomo1213 | 4:1bd08c9d92a9 | 128 | |
ngokystk | 0:b4b94eb28093 | 129 | #define DELTA_T1 0.1 |
ngokystk | 0:b4b94eb28093 | 130 | #define target_val1 0 |
ngokystk | 0:b4b94eb28093 | 131 | #define Kp1 3 |
ngokystk | 0:b4b94eb28093 | 132 | #define Ki1 0 |
ngokystk | 0:b4b94eb28093 | 133 | #define Kd1 0 |
Tomo1213 | 1:b2bd1511307e | 134 | |
ngokystk | 0:b4b94eb28093 | 135 | pc.printf("YUKA PROGRAM START\r\n"); |
ngokystk | 0:b4b94eb28093 | 136 | wait(0.1); |
ngokystk | 0:b4b94eb28093 | 137 | //-------------起動時に必ず送信--------------- |
ngokystk | 0:b4b94eb28093 | 138 | //オペレーティングモードを送信 |
Tomo1213 | 3:c39c14cfc811 | 139 | sendOPModeT(node1); |
Tomo1213 | 3:c39c14cfc811 | 140 | sendOPModeT(node2); |
Tomo1213 | 3:c39c14cfc811 | 141 | sendOPModeT(node3); |
Tomo1213 | 3:c39c14cfc811 | 142 | sendOPModeT(node4); |
Tomo1213 | 3:c39c14cfc811 | 143 | |
ngokystk | 0:b4b94eb28093 | 144 | //Shutdown,Enableコマンド送信|リセット |
ngokystk | 0:b4b94eb28093 | 145 | sendCtrlSD(node1); |
ngokystk | 0:b4b94eb28093 | 146 | sendCtrlSD(node2); |
ngokystk | 0:b4b94eb28093 | 147 | sendCtrlSD(node3); |
ngokystk | 0:b4b94eb28093 | 148 | sendCtrlSD(node4); |
Tomo1213 | 1:b2bd1511307e | 149 | |
ngokystk | 0:b4b94eb28093 | 150 | sendCtrlEN(node1); |
ngokystk | 0:b4b94eb28093 | 151 | sendCtrlEN(node2); |
ngokystk | 0:b4b94eb28093 | 152 | sendCtrlEN(node3); |
ngokystk | 0:b4b94eb28093 | 153 | sendCtrlEN(node4); |
ngokystk | 0:b4b94eb28093 | 154 | |
ngokystk | 0:b4b94eb28093 | 155 | //初期加減速度 |
Tomo1213 | 3:c39c14cfc811 | 156 | int Acc = 2000; |
Tomo1213 | 3:c39c14cfc811 | 157 | int Dec = 2000; |
Tomo1213 | 3:c39c14cfc811 | 158 | |
Tomo1213 | 3:c39c14cfc811 | 159 | sendProAcc(1,Acc); |
Tomo1213 | 3:c39c14cfc811 | 160 | sendProAcc(2,Acc); |
Tomo1213 | 3:c39c14cfc811 | 161 | sendProAcc(3,Acc); |
Tomo1213 | 3:c39c14cfc811 | 162 | sendProAcc(4,Acc); |
Tomo1213 | 3:c39c14cfc811 | 163 | |
Tomo1213 | 3:c39c14cfc811 | 164 | sendProDec(1,Dec); |
Tomo1213 | 3:c39c14cfc811 | 165 | sendProDec(2,Dec); |
Tomo1213 | 3:c39c14cfc811 | 166 | sendProDec(3,Dec); |
Tomo1213 | 3:c39c14cfc811 | 167 | sendProDec(4,Dec); |
Tomo1213 | 3:c39c14cfc811 | 168 | |
ngokystk | 0:b4b94eb28093 | 169 | //トルク設定 |
ngokystk | 0:b4b94eb28093 | 170 | int trq = 100; //torque Setting[mA] |
ngokystk | 0:b4b94eb28093 | 171 | |
ngokystk | 0:b4b94eb28093 | 172 | sendTgtTrq(1,trq); |
ngokystk | 0:b4b94eb28093 | 173 | sendTgtTrq(2,trq); |
ngokystk | 0:b4b94eb28093 | 174 | sendTgtTrq(3,trq); |
ngokystk | 0:b4b94eb28093 | 175 | sendTgtTrq(4,trq); |
Tomo1213 | 1:b2bd1511307e | 176 | |
Tomo1213 | 4:1bd08c9d92a9 | 177 | int state_1 = 0; |
Tomo1213 | 4:1bd08c9d92a9 | 178 | int state_2 = 0; |
Tomo1213 | 4:1bd08c9d92a9 | 179 | int ride_count = 0; |
Tomo1213 | 4:1bd08c9d92a9 | 180 | |
Tomo1213 | 4:1bd08c9d92a9 | 181 | int X_POS_TMP = 0; |
Tomo1213 | 4:1bd08c9d92a9 | 182 | int dist1_ori = 0; |
Tomo1213 | 4:1bd08c9d92a9 | 183 | int dist2_ori = 0; |
Tomo1213 | 4:1bd08c9d92a9 | 184 | |
Tomo1213 | 4:1bd08c9d92a9 | 185 | dist1 = 0; |
Tomo1213 | 4:1bd08c9d92a9 | 186 | readActPos(1); |
Tomo1213 | 4:1bd08c9d92a9 | 187 | ActPos=canmsgRx.data[4]+canmsgRx.data[5]*256+canmsgRx.data[6]*65536; |
Tomo1213 | 4:1bd08c9d92a9 | 188 | if(ActPos > 8388608){ |
Tomo1213 | 4:1bd08c9d92a9 | 189 | ActPos -= 0x1000000;//16進数でのマイナス処理 16^7 = 16,777,216 = 0x1000000 |
Tomo1213 | 4:1bd08c9d92a9 | 190 | } |
Tomo1213 | 4:1bd08c9d92a9 | 191 | Init_Pos = ActPos;//起動時の角度を保存 |
Tomo1213 | 4:1bd08c9d92a9 | 192 | t.reset(); |
Tomo1213 | 4:1bd08c9d92a9 | 193 | t.start(); |
Tomo1213 | 4:1bd08c9d92a9 | 194 | |
Tomo1213 | 4:1bd08c9d92a9 | 195 | //set_MODE_T(); |
Tomo1213 | 4:1bd08c9d92a9 | 196 | |
Tomo1213 | 4:1bd08c9d92a9 | 197 | printf("\nstart\r\n"); |
Tomo1213 | 4:1bd08c9d92a9 | 198 | |
Tomo1213 | 4:1bd08c9d92a9 | 199 | while(1){ |
Tomo1213 | 4:1bd08c9d92a9 | 200 | |
Tomo1213 | 4:1bd08c9d92a9 | 201 | Time = t.read(); |
Tomo1213 | 4:1bd08c9d92a9 | 202 | pc.printf("state_1:%d state_2:%d\r\n",state_1,state_2); |
Tomo1213 | 4:1bd08c9d92a9 | 203 | LED.printf("%d",state_1); |
Tomo1213 | 1:b2bd1511307e | 204 | readActPos(1); |
Tomo1213 | 1:b2bd1511307e | 205 | ActPos=canmsgRx.data[4]+canmsgRx.data[5]*256+canmsgRx.data[6]*65536; |
Tomo1213 | 1:b2bd1511307e | 206 | if(ActPos > 8388608){ |
Tomo1213 | 1:b2bd1511307e | 207 | ActPos -= 0x1000000;//16進数でのマイナス処理 16^7 = 16,777,216 = 0x1000000 |
Tomo1213 | 4:1bd08c9d92a9 | 208 | //printf("check\r\n"); |
ngokystk | 0:b4b94eb28093 | 209 | } |
Tomo1213 | 4:1bd08c9d92a9 | 210 | dist1 = get_cm_n(u1, 5); |
Tomo1213 | 4:1bd08c9d92a9 | 211 | dist2 = get_cm_n(u2, 5); |
Tomo1213 | 4:1bd08c9d92a9 | 212 | dist3 = get_cm_n(u3, 5); |
Tomo1213 | 4:1bd08c9d92a9 | 213 | |
Tomo1213 | 4:1bd08c9d92a9 | 214 | /*--------------------------*/ |
Tomo1213 | 4:1bd08c9d92a9 | 215 | // |
Tomo1213 | 4:1bd08c9d92a9 | 216 | if(state_1 == 0){//入力判断フェーズ |
Tomo1213 | 4:1bd08c9d92a9 | 217 | state_2 = 0; |
Tomo1213 | 4:1bd08c9d92a9 | 218 | if(ride_count >= 2 && Time > Standby_Time){ |
Tomo1213 | 4:1bd08c9d92a9 | 219 | state_1 = 20; |
Tomo1213 | 4:1bd08c9d92a9 | 220 | }else{ |
Tomo1213 | 4:1bd08c9d92a9 | 221 | if(ActPos < (Init_Pos - KICK)){ //前入力検出 |
Tomo1213 | 4:1bd08c9d92a9 | 222 | ride_count++; |
Tomo1213 | 4:1bd08c9d92a9 | 223 | state_1 = 1; |
Tomo1213 | 4:1bd08c9d92a9 | 224 | }else if(ActPos > (Init_Pos + KICK)){ //右入力検出 |
Tomo1213 | 4:1bd08c9d92a9 | 225 | ride_count++; |
Tomo1213 | 4:1bd08c9d92a9 | 226 | state_1 = 11; |
Tomo1213 | 4:1bd08c9d92a9 | 227 | }else{ |
Tomo1213 | 4:1bd08c9d92a9 | 228 | set_MODE_T(); |
Tomo1213 | 1:b2bd1511307e | 229 | } |
Tomo1213 | 4:1bd08c9d92a9 | 230 | } |
Tomo1213 | 4:1bd08c9d92a9 | 231 | |
Tomo1213 | 4:1bd08c9d92a9 | 232 | }else if(state_1 == 1){//前進→壁検出フェーズ |
Tomo1213 | 4:1bd08c9d92a9 | 233 | if(dist1 < WALL && dist1 >= WALL_MIN){ |
Tomo1213 | 4:1bd08c9d92a9 | 234 | vel_stop(); |
Tomo1213 | 4:1bd08c9d92a9 | 235 | wait(GETOFF_TIME); |
Tomo1213 | 4:1bd08c9d92a9 | 236 | state_1 = 2; |
Tomo1213 | 4:1bd08c9d92a9 | 237 | }else{ |
Tomo1213 | 4:1bd08c9d92a9 | 238 | set_ACC(ACC_RIDE);//加速度設定 |
Tomo1213 | 4:1bd08c9d92a9 | 239 | set_DEC(DEC_CLEAN);//減速度設定 |
Tomo1213 | 4:1bd08c9d92a9 | 240 | set_MODE_V();//速度制御モード送信 |
Tomo1213 | 4:1bd08c9d92a9 | 241 | vel_forward_con(RPM_RIDE);//前進速度指令 |
Tomo1213 | 4:1bd08c9d92a9 | 242 | } |
Tomo1213 | 4:1bd08c9d92a9 | 243 | }else if(state_1 == 2){//前進からの帰還フェーズ |
Tomo1213 | 4:1bd08c9d92a9 | 244 | if(ActPos > -3000){ |
Tomo1213 | 4:1bd08c9d92a9 | 245 | vel_stop(); |
Tomo1213 | 4:1bd08c9d92a9 | 246 | t.reset(); |
Tomo1213 | 4:1bd08c9d92a9 | 247 | t.start(); |
Tomo1213 | 4:1bd08c9d92a9 | 248 | state_1 = 0; |
Tomo1213 | 4:1bd08c9d92a9 | 249 | wait(1.0); |
Tomo1213 | 4:1bd08c9d92a9 | 250 | }else{ |
Tomo1213 | 4:1bd08c9d92a9 | 251 | vel_backward_con(RPM_RIDE); |
Tomo1213 | 4:1bd08c9d92a9 | 252 | } |
Tomo1213 | 4:1bd08c9d92a9 | 253 | }else if(state_1 == 11){//右進→壁検出フェーズ |
Tomo1213 | 4:1bd08c9d92a9 | 254 | if(dist3 < WALL && dist3 >= WALL_MIN){ |
Tomo1213 | 4:1bd08c9d92a9 | 255 | vel_stop(); |
Tomo1213 | 4:1bd08c9d92a9 | 256 | wait(GETOFF_TIME); |
Tomo1213 | 4:1bd08c9d92a9 | 257 | state_1 = 12; |
Tomo1213 | 4:1bd08c9d92a9 | 258 | }else{ |
Tomo1213 | 4:1bd08c9d92a9 | 259 | set_ACC(ACC_RIDE);//加速度設定 |
Tomo1213 | 4:1bd08c9d92a9 | 260 | set_DEC(DEC_RIDE);//減速度設定 |
Tomo1213 | 4:1bd08c9d92a9 | 261 | set_MODE_V();//速度制御モード送信 |
Tomo1213 | 4:1bd08c9d92a9 | 262 | vel_right(RPM_RIDE);//R進速度指令 |
Tomo1213 | 4:1bd08c9d92a9 | 263 | } |
Tomo1213 | 4:1bd08c9d92a9 | 264 | }else if(state_1 == 12){//右進からの帰還フェーズ |
Tomo1213 | 4:1bd08c9d92a9 | 265 | if(ActPos < 3000){ |
Tomo1213 | 4:1bd08c9d92a9 | 266 | vel_stop(); |
Tomo1213 | 4:1bd08c9d92a9 | 267 | t.reset(); |
Tomo1213 | 4:1bd08c9d92a9 | 268 | t.start(); |
Tomo1213 | 4:1bd08c9d92a9 | 269 | state_1 = 0; |
Tomo1213 | 4:1bd08c9d92a9 | 270 | wait(1.0); |
Tomo1213 | 4:1bd08c9d92a9 | 271 | }else{ |
Tomo1213 | 4:1bd08c9d92a9 | 272 | vel_left(RPM_RIDE); |
Tomo1213 | 4:1bd08c9d92a9 | 273 | } |
Tomo1213 | 4:1bd08c9d92a9 | 274 | }else if(state_1 == 20){//消毒モード |
Tomo1213 | 4:1bd08c9d92a9 | 275 | if(state_2 == 0){ |
Tomo1213 | 4:1bd08c9d92a9 | 276 | if(dist1 < WALL && dist1 >= WALL_MIN){ |
Tomo1213 | 4:1bd08c9d92a9 | 277 | X_POS_TMP = ActPos; |
Tomo1213 | 4:1bd08c9d92a9 | 278 | state_2 = 1; |
Tomo1213 | 4:1bd08c9d92a9 | 279 | }else{ |
Tomo1213 | 4:1bd08c9d92a9 | 280 | set_ACC(ACC_CLEAN);//加速度設定 |
Tomo1213 | 4:1bd08c9d92a9 | 281 | set_DEC(DEC_CLEAN);//減速度設定 |
Tomo1213 | 4:1bd08c9d92a9 | 282 | set_MODE_V();//速度制御モード送信 |
Tomo1213 | 4:1bd08c9d92a9 | 283 | vel_forward_con(RPM_CLEAN);//前進速度指令 |
Tomo1213 | 4:1bd08c9d92a9 | 284 | } |
Tomo1213 | 4:1bd08c9d92a9 | 285 | }else if(state_2 == 1){ |
Tomo1213 | 4:1bd08c9d92a9 | 286 | if(abs(ActPos - X_POS_TMP) > CLEAN_OFFSET){ |
Tomo1213 | 4:1bd08c9d92a9 | 287 | state_2 == 2; |
Tomo1213 | 4:1bd08c9d92a9 | 288 | }else{ |
Tomo1213 | 4:1bd08c9d92a9 | 289 | if(dist3 < WALL && dist3 >= WALL_MIN){ |
Tomo1213 | 4:1bd08c9d92a9 | 290 | state_2 = 4; |
Tomo1213 | 4:1bd08c9d92a9 | 291 | }else{ |
Tomo1213 | 4:1bd08c9d92a9 | 292 | set_ACC(ACC_CLEAN);//加速度設定 |
Tomo1213 | 4:1bd08c9d92a9 | 293 | set_DEC(DEC_CLEAN);//減速度設定 |
Tomo1213 | 4:1bd08c9d92a9 | 294 | set_MODE_V();//速度制御モード送信 |
Tomo1213 | 4:1bd08c9d92a9 | 295 | vel_right(RPM_CLEAN);//右進速度指令 |
Tomo1213 | 1:b2bd1511307e | 296 | } |
Tomo1213 | 4:1bd08c9d92a9 | 297 | } |
Tomo1213 | 4:1bd08c9d92a9 | 298 | }else if(state_2 == 2){ |
Tomo1213 | 4:1bd08c9d92a9 | 299 | if(ActPos > -3000){ |
Tomo1213 | 4:1bd08c9d92a9 | 300 | state_2 = 3; |
Tomo1213 | 4:1bd08c9d92a9 | 301 | }else{ |
Tomo1213 | 4:1bd08c9d92a9 | 302 | set_ACC(ACC_CLEAN);//加速度設定 |
Tomo1213 | 4:1bd08c9d92a9 | 303 | set_DEC(DEC_CLEAN);//減速度設定 |
Tomo1213 | 4:1bd08c9d92a9 | 304 | set_MODE_V();//速度制御モード送信 |
Tomo1213 | 4:1bd08c9d92a9 | 305 | vel_backward_con(RPM_CLEAN);//後進速度指令 |
Tomo1213 | 4:1bd08c9d92a9 | 306 | } |
Tomo1213 | 4:1bd08c9d92a9 | 307 | }else if(state_2 == 3){ |
Tomo1213 | 4:1bd08c9d92a9 | 308 | if(abs(ActPos - X_POS_TMP ) > CLEAN_OFFSET){ |
Tomo1213 | 4:1bd08c9d92a9 | 309 | state_2 == 0; |
Tomo1213 | 4:1bd08c9d92a9 | 310 | }else{ |
Tomo1213 | 4:1bd08c9d92a9 | 311 | if(dist3 < WALL && dist3 >= WALL_MIN){ |
Tomo1213 | 4:1bd08c9d92a9 | 312 | state_2 = 4; |
Tomo1213 | 4:1bd08c9d92a9 | 313 | }else{ |
Tomo1213 | 4:1bd08c9d92a9 | 314 | set_ACC(ACC_CLEAN);//加速度設定 |
Tomo1213 | 4:1bd08c9d92a9 | 315 | set_DEC(DEC_CLEAN);//減速度設定 |
Tomo1213 | 4:1bd08c9d92a9 | 316 | set_MODE_V();//速度制御モード送信 |
Tomo1213 | 4:1bd08c9d92a9 | 317 | vel_right(RPM_CLEAN);//右進速度指令 |
Tomo1213 | 1:b2bd1511307e | 318 | } |
Tomo1213 | 4:1bd08c9d92a9 | 319 | } |
Tomo1213 | 4:1bd08c9d92a9 | 320 | }else if(state_2 == 4){ |
Tomo1213 | 4:1bd08c9d92a9 | 321 | if(ActPos < 3000){ |
Tomo1213 | 4:1bd08c9d92a9 | 322 | state_2 = 5; |
Tomo1213 | 4:1bd08c9d92a9 | 323 | }else{ |
Tomo1213 | 4:1bd08c9d92a9 | 324 | vel_left(RPM_CLEAN); |
Tomo1213 | 4:1bd08c9d92a9 | 325 | } |
Tomo1213 | 4:1bd08c9d92a9 | 326 | }else if(state_2 == 5){ |
Tomo1213 | 4:1bd08c9d92a9 | 327 | if(ActPos > -3000){ |
Tomo1213 | 4:1bd08c9d92a9 | 328 | t.reset(); |
Tomo1213 | 4:1bd08c9d92a9 | 329 | t.start(); |
Tomo1213 | 4:1bd08c9d92a9 | 330 | state_1 = 0; |
Tomo1213 | 4:1bd08c9d92a9 | 331 | state_2 = 0; |
Tomo1213 | 4:1bd08c9d92a9 | 332 | }else{ |
Tomo1213 | 4:1bd08c9d92a9 | 333 | vel_backward_con(RPM_CLEAN); |
Tomo1213 | 1:b2bd1511307e | 334 | } |
Tomo1213 | 4:1bd08c9d92a9 | 335 | } |
Tomo1213 | 4:1bd08c9d92a9 | 336 | } |
Tomo1213 | 4:1bd08c9d92a9 | 337 | } |
Tomo1213 | 4:1bd08c9d92a9 | 338 | } |
ngokystk | 0:b4b94eb28093 | 339 | |
Tomo1213 | 4:1bd08c9d92a9 | 340 | void vel_right(int rpm){ |
Tomo1213 | 4:1bd08c9d92a9 | 341 | sendTgtVel(1,rpm); |
Tomo1213 | 4:1bd08c9d92a9 | 342 | sendTgtVel(2,rpm*(-1)); |
Tomo1213 | 4:1bd08c9d92a9 | 343 | sendTgtVel(3,rpm*(-1)); |
Tomo1213 | 4:1bd08c9d92a9 | 344 | sendTgtVel(4,rpm); |
Tomo1213 | 4:1bd08c9d92a9 | 345 | for(int i=1;i<= 4;i++){ |
Tomo1213 | 4:1bd08c9d92a9 | 346 | sendCtrlEN(i); |
Tomo1213 | 4:1bd08c9d92a9 | 347 | } |
Tomo1213 | 4:1bd08c9d92a9 | 348 | } |
Tomo1213 | 4:1bd08c9d92a9 | 349 | void vel_right_con(int rpmA){ |
Tomo1213 | 5:f21e1a75f98b | 350 | int dis1 = get_cm_n(u1,5); |
Tomo1213 | 5:f21e1a75f98b | 351 | int dis2 = get_cm_n(u2,5); |
Tomo1213 | 4:1bd08c9d92a9 | 352 | |
Tomo1213 | 4:1bd08c9d92a9 | 353 | //速度を指定 |
Tomo1213 | 4:1bd08c9d92a9 | 354 | int robot_angle = ((dis1 - dis2)*5); |
Tomo1213 | 4:1bd08c9d92a9 | 355 | sendTgtVel(1,rpmA+robot_angle); |
Tomo1213 | 4:1bd08c9d92a9 | 356 | sendTgtVel(2,rpmA*(-1)+robot_angle); |
Tomo1213 | 4:1bd08c9d92a9 | 357 | sendTgtVel(3,rpmA*(-1)+robot_angle); |
Tomo1213 | 4:1bd08c9d92a9 | 358 | sendTgtVel(4,rpmA+robot_angle); |
Tomo1213 | 4:1bd08c9d92a9 | 359 | //指令値を送信 |
Tomo1213 | 4:1bd08c9d92a9 | 360 | for(int i=1;i<= 4;i++){ |
Tomo1213 | 4:1bd08c9d92a9 | 361 | sendCtrlEN(i); |
Tomo1213 | 4:1bd08c9d92a9 | 362 | } |
Tomo1213 | 4:1bd08c9d92a9 | 363 | } |
Tomo1213 | 1:b2bd1511307e | 364 | |
Tomo1213 | 4:1bd08c9d92a9 | 365 | |
Tomo1213 | 4:1bd08c9d92a9 | 366 | void vel_left(int rpm){ |
Tomo1213 | 4:1bd08c9d92a9 | 367 | sendTgtVel(1,rpm*(-1)); |
Tomo1213 | 4:1bd08c9d92a9 | 368 | sendTgtVel(2,rpm); |
Tomo1213 | 4:1bd08c9d92a9 | 369 | sendTgtVel(3,rpm); |
Tomo1213 | 4:1bd08c9d92a9 | 370 | sendTgtVel(4,rpm*(-1)); |
Tomo1213 | 4:1bd08c9d92a9 | 371 | for(int i=1;i<= 4;i++){ |
Tomo1213 | 4:1bd08c9d92a9 | 372 | sendCtrlEN(i); |
Tomo1213 | 4:1bd08c9d92a9 | 373 | } |
Tomo1213 | 4:1bd08c9d92a9 | 374 | } |
Tomo1213 | 4:1bd08c9d92a9 | 375 | |
Tomo1213 | 4:1bd08c9d92a9 | 376 | |
Tomo1213 | 4:1bd08c9d92a9 | 377 | void vel_left_con(int rpmA){ |
Tomo1213 | 5:f21e1a75f98b | 378 | int dis1 = get_cm_n(u1,5); |
Tomo1213 | 5:f21e1a75f98b | 379 | int dis2 = get_cm_n(u2,5); |
Tomo1213 | 4:1bd08c9d92a9 | 380 | |
Tomo1213 | 4:1bd08c9d92a9 | 381 | //速度を指定 |
Tomo1213 | 4:1bd08c9d92a9 | 382 | int robot_angle = ((dis1 - dis2)*5); |
Tomo1213 | 4:1bd08c9d92a9 | 383 | sendTgtVel(1,rpmA*(-1)+robot_angle); |
Tomo1213 | 4:1bd08c9d92a9 | 384 | sendTgtVel(2,rpmA+robot_angle); |
Tomo1213 | 4:1bd08c9d92a9 | 385 | sendTgtVel(3,rpmA+robot_angle); |
Tomo1213 | 4:1bd08c9d92a9 | 386 | sendTgtVel(4,rpmA*(-1)+robot_angle); |
Tomo1213 | 4:1bd08c9d92a9 | 387 | //指令値を送信 |
Tomo1213 | 4:1bd08c9d92a9 | 388 | for(int i=1;i<= 4;i++){ |
Tomo1213 | 4:1bd08c9d92a9 | 389 | sendCtrlEN(i); |
Tomo1213 | 4:1bd08c9d92a9 | 390 | } |
Tomo1213 | 4:1bd08c9d92a9 | 391 | } |
Tomo1213 | 4:1bd08c9d92a9 | 392 | |
Tomo1213 | 4:1bd08c9d92a9 | 393 | void vel_stop(){ |
Tomo1213 | 4:1bd08c9d92a9 | 394 | //速度を指定 |
Tomo1213 | 4:1bd08c9d92a9 | 395 | sendTgtVel(1,0); |
Tomo1213 | 4:1bd08c9d92a9 | 396 | sendTgtVel(2,0); |
Tomo1213 | 4:1bd08c9d92a9 | 397 | sendTgtVel(3,0); |
Tomo1213 | 4:1bd08c9d92a9 | 398 | sendTgtVel(4,0); |
Tomo1213 | 4:1bd08c9d92a9 | 399 | //指令値を送信 |
Tomo1213 | 4:1bd08c9d92a9 | 400 | for(int i=1;i<= 4;i++){ |
Tomo1213 | 4:1bd08c9d92a9 | 401 | sendCtrlEN(i); |
Tomo1213 | 4:1bd08c9d92a9 | 402 | } |
Tomo1213 | 4:1bd08c9d92a9 | 403 | } |
Tomo1213 | 1:b2bd1511307e | 404 | |
Tomo1213 | 4:1bd08c9d92a9 | 405 | void vel_backward(int rpm){ |
Tomo1213 | 4:1bd08c9d92a9 | 406 | //速度を指定 |
Tomo1213 | 4:1bd08c9d92a9 | 407 | sendTgtVel(1,rpm); |
Tomo1213 | 4:1bd08c9d92a9 | 408 | sendTgtVel(2,rpm); |
Tomo1213 | 4:1bd08c9d92a9 | 409 | sendTgtVel(3,rpm*(-1)); |
Tomo1213 | 4:1bd08c9d92a9 | 410 | sendTgtVel(4,rpm*(-1)); |
Tomo1213 | 4:1bd08c9d92a9 | 411 | //指令値を送信 |
Tomo1213 | 4:1bd08c9d92a9 | 412 | for(int i=1;i<= 4;i++){ |
Tomo1213 | 4:1bd08c9d92a9 | 413 | sendCtrlEN(i); |
Tomo1213 | 4:1bd08c9d92a9 | 414 | } |
Tomo1213 | 4:1bd08c9d92a9 | 415 | } |
Tomo1213 | 4:1bd08c9d92a9 | 416 | |
Tomo1213 | 4:1bd08c9d92a9 | 417 | void vel_backward_con(int rpmA){ |
Tomo1213 | 5:f21e1a75f98b | 418 | int dis1 = get_cm_n(u1,5); |
Tomo1213 | 5:f21e1a75f98b | 419 | int dis2 = get_cm_n(u2,5); |
ngokystk | 0:b4b94eb28093 | 420 | |
Tomo1213 | 4:1bd08c9d92a9 | 421 | //速度を指定 |
Tomo1213 | 4:1bd08c9d92a9 | 422 | int robot_angle = ((dis1 - dis2)*10); |
Tomo1213 | 4:1bd08c9d92a9 | 423 | sendTgtVel(1,rpmA+robot_angle); |
Tomo1213 | 4:1bd08c9d92a9 | 424 | sendTgtVel(2,rpmA+robot_angle); |
Tomo1213 | 4:1bd08c9d92a9 | 425 | sendTgtVel(3,rpmA*(-1)+robot_angle); |
Tomo1213 | 4:1bd08c9d92a9 | 426 | sendTgtVel(4,rpmA*(-1)+robot_angle); |
Tomo1213 | 4:1bd08c9d92a9 | 427 | //指令値を送信 |
Tomo1213 | 4:1bd08c9d92a9 | 428 | for(int i=1;i<= 4;i++){ |
Tomo1213 | 4:1bd08c9d92a9 | 429 | sendCtrlEN(i); |
Tomo1213 | 4:1bd08c9d92a9 | 430 | } |
Tomo1213 | 4:1bd08c9d92a9 | 431 | } |
Tomo1213 | 4:1bd08c9d92a9 | 432 | |
Tomo1213 | 4:1bd08c9d92a9 | 433 | |
Tomo1213 | 4:1bd08c9d92a9 | 434 | void vel_forward_con(int rpmA){ |
Tomo1213 | 5:f21e1a75f98b | 435 | int dis1 = get_cm_n(u1,5); |
Tomo1213 | 5:f21e1a75f98b | 436 | int dis2 = get_cm_n(u2,5); |
Tomo1213 | 1:b2bd1511307e | 437 | |
Tomo1213 | 4:1bd08c9d92a9 | 438 | //速度を指定 |
Tomo1213 | 4:1bd08c9d92a9 | 439 | int robot_angle = ((dis1 - dis2)*10); |
Tomo1213 | 4:1bd08c9d92a9 | 440 | sendTgtVel(1,rpmA*(-1)+robot_angle); |
Tomo1213 | 4:1bd08c9d92a9 | 441 | sendTgtVel(2,rpmA*(-1)+robot_angle); |
Tomo1213 | 4:1bd08c9d92a9 | 442 | sendTgtVel(3,rpmA+robot_angle); |
Tomo1213 | 4:1bd08c9d92a9 | 443 | sendTgtVel(4,rpmA+robot_angle); |
Tomo1213 | 4:1bd08c9d92a9 | 444 | //指令値を送信 |
Tomo1213 | 4:1bd08c9d92a9 | 445 | for(int i=1;i<= 4;i++){ |
Tomo1213 | 4:1bd08c9d92a9 | 446 | sendCtrlEN(i); |
Tomo1213 | 4:1bd08c9d92a9 | 447 | } |
Tomo1213 | 1:b2bd1511307e | 448 | } |
Tomo1213 | 1:b2bd1511307e | 449 | |
Tomo1213 | 1:b2bd1511307e | 450 | void set_ACC(int setACC_val){ |
Tomo1213 | 1:b2bd1511307e | 451 | sendProAcc(1,setACC_val); |
Tomo1213 | 1:b2bd1511307e | 452 | sendProAcc(2,setACC_val); |
Tomo1213 | 1:b2bd1511307e | 453 | sendProAcc(3,setACC_val); |
Tomo1213 | 1:b2bd1511307e | 454 | sendProAcc(4,setACC_val); |
ngokystk | 0:b4b94eb28093 | 455 | } |
ngokystk | 0:b4b94eb28093 | 456 | |
Tomo1213 | 1:b2bd1511307e | 457 | void set_DEC(int setDEC_val){ |
Tomo1213 | 1:b2bd1511307e | 458 | sendProDec(1,setDEC_val); |
Tomo1213 | 1:b2bd1511307e | 459 | sendProDec(2,setDEC_val); |
Tomo1213 | 1:b2bd1511307e | 460 | sendProDec(3,setDEC_val); |
Tomo1213 | 1:b2bd1511307e | 461 | sendProDec(4,setDEC_val); |
Tomo1213 | 1:b2bd1511307e | 462 | } |
ngokystk | 0:b4b94eb28093 | 463 | |
Tomo1213 | 1:b2bd1511307e | 464 | void set_MODE_V(){ |
Tomo1213 | 1:b2bd1511307e | 465 | sendOPModeV(1); |
Tomo1213 | 1:b2bd1511307e | 466 | sendOPModeV(2); |
Tomo1213 | 1:b2bd1511307e | 467 | sendOPModeV(3); |
Tomo1213 | 1:b2bd1511307e | 468 | sendOPModeV(4); |
Tomo1213 | 1:b2bd1511307e | 469 | } |
ngokystk | 0:b4b94eb28093 | 470 | |
Tomo1213 | 1:b2bd1511307e | 471 | void set_MODE_T(){ |
Tomo1213 | 1:b2bd1511307e | 472 | sendOPModeT(1); |
Tomo1213 | 1:b2bd1511307e | 473 | sendOPModeT(2); |
Tomo1213 | 1:b2bd1511307e | 474 | sendOPModeT(3); |
Tomo1213 | 1:b2bd1511307e | 475 | sendOPModeT(4); |
Tomo1213 | 1:b2bd1511307e | 476 | } |
ngokystk | 0:b4b94eb28093 | 477 | |
ngokystk | 0:b4b94eb28093 | 478 | //0x2F-6060-00-fd-//-//-// |
ngokystk | 0:b4b94eb28093 | 479 | void sendOPModeT(int nodeID){ |
ngokystk | 0:b4b94eb28093 | 480 | canmsgTx.id = 0x600+nodeID; |
ngokystk | 0:b4b94eb28093 | 481 | canmsgTx.len = 5; //Data Length |
ngokystk | 0:b4b94eb28093 | 482 | canmsgTx.data[0] = 0x2F;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| |
ngokystk | 0:b4b94eb28093 | 483 | canmsgTx.data[1] = 0x60;//Index LowByte |
ngokystk | 0:b4b94eb28093 | 484 | canmsgTx.data[2] = 0x60;//Index HighByte |
ngokystk | 0:b4b94eb28093 | 485 | canmsgTx.data[3] = 0x00;//sub-Index |
ngokystk | 0:b4b94eb28093 | 486 | canmsgTx.data[4] = 0xFD;//data:fd = "current Mode" |
ngokystk | 0:b4b94eb28093 | 487 | /* |
ngokystk | 0:b4b94eb28093 | 488 | canmsgTx.data[5] = 0x00;//data:(user value) |
ngokystk | 0:b4b94eb28093 | 489 | canmsgTx.data[6] = 0x00;//data:(user value) |
ngokystk | 0:b4b94eb28093 | 490 | canmsgTx.data[7] = 0x00;//data:(user value) |
ngokystk | 0:b4b94eb28093 | 491 | */ |
ngokystk | 0:b4b94eb28093 | 492 | printCANTX(); //CAN送信データをPCに表示 |
ngokystk | 0:b4b94eb28093 | 493 | canPort.write(canmsgTx);//CANでデータ送信 |
ngokystk | 0:b4b94eb28093 | 494 | wait_ms(1); |
ngokystk | 0:b4b94eb28093 | 495 | } |
Tomo1213 | 1:b2bd1511307e | 496 | |
Tomo1213 | 3:c39c14cfc811 | 497 | |
ngokystk | 0:b4b94eb28093 | 498 | //0x2F-6060-00-03-//-//-// |
ngokystk | 0:b4b94eb28093 | 499 | void sendOPModeV(int nodeID){ |
ngokystk | 0:b4b94eb28093 | 500 | canmsgTx.id = 0x600+nodeID; |
ngokystk | 0:b4b94eb28093 | 501 | canmsgTx.len = 5; //Data Length |
ngokystk | 0:b4b94eb28093 | 502 | canmsgTx.data[0] = 0x2F;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| |
ngokystk | 0:b4b94eb28093 | 503 | canmsgTx.data[1] = 0x60;//Index LowByte |
ngokystk | 0:b4b94eb28093 | 504 | canmsgTx.data[2] = 0x60;//Index HighByte |
ngokystk | 0:b4b94eb28093 | 505 | canmsgTx.data[3] = 0x00;//sub-Index |
ngokystk | 0:b4b94eb28093 | 506 | canmsgTx.data[4] = 0x03;//data:0x03 = "Profile Velocity Mode" |
ngokystk | 0:b4b94eb28093 | 507 | /* |
ngokystk | 0:b4b94eb28093 | 508 | canmsgTx.data[5] = 0x00;//data:(user value) |
ngokystk | 0:b4b94eb28093 | 509 | canmsgTx.data[6] = 0x00;//data:(user value) |
ngokystk | 0:b4b94eb28093 | 510 | canmsgTx.data[7] = 0x00;//data:(user value) |
ngokystk | 0:b4b94eb28093 | 511 | */ |
ngokystk | 0:b4b94eb28093 | 512 | printCANTX(); //CAN送信データをPCに表示 |
ngokystk | 0:b4b94eb28093 | 513 | canPort.write(canmsgTx);//CANでデータ送信 |
ngokystk | 0:b4b94eb28093 | 514 | wait_ms(1); |
ngokystk | 0:b4b94eb28093 | 515 | } |
Tomo1213 | 1:b2bd1511307e | 516 | |
ngokystk | 0:b4b94eb28093 | 517 | //0x2B-6040-00-0000-//-// |
ngokystk | 0:b4b94eb28093 | 518 | void sendCtrlRS(int nodeID){ |
ngokystk | 0:b4b94eb28093 | 519 | canmsgTx.id = 0x600+nodeID; |
ngokystk | 0:b4b94eb28093 | 520 | canmsgTx.len = 6; //Data Length |
ngokystk | 0:b4b94eb28093 | 521 | canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| |
ngokystk | 0:b4b94eb28093 | 522 | canmsgTx.data[1] = 0x40;//Index LowByte |
ngokystk | 0:b4b94eb28093 | 523 | canmsgTx.data[2] = 0x60;//Index HighByte |
ngokystk | 0:b4b94eb28093 | 524 | canmsgTx.data[3] = 0x00;//sub-Index |
ngokystk | 0:b4b94eb28093 | 525 | canmsgTx.data[4] = 0x80;//data:0x00"80" = "Controlword(Shutdown)" |
ngokystk | 0:b4b94eb28093 | 526 | canmsgTx.data[5] = 0x00;//data:0x"00"80 |
ngokystk | 0:b4b94eb28093 | 527 | /* |
ngokystk | 0:b4b94eb28093 | 528 | canmsgTx.data[6] = 0x00;//data:(user value) |
ngokystk | 0:b4b94eb28093 | 529 | canmsgTx.data[7] = 0x00;//data:(user value) |
ngokystk | 0:b4b94eb28093 | 530 | */ |
ngokystk | 0:b4b94eb28093 | 531 | printCANTX(); //CAN送信データをPCに表示 |
ngokystk | 0:b4b94eb28093 | 532 | canPort.write(canmsgTx);//CANでデータ送信 |
ngokystk | 0:b4b94eb28093 | 533 | wait_ms(1); |
ngokystk | 0:b4b94eb28093 | 534 | } |
Tomo1213 | 1:b2bd1511307e | 535 | |
Tomo1213 | 1:b2bd1511307e | 536 | |
ngokystk | 0:b4b94eb28093 | 537 | //0x2B-6040-00-0006-//-// |
ngokystk | 0:b4b94eb28093 | 538 | void sendCtrlSD(int nodeID){ |
ngokystk | 0:b4b94eb28093 | 539 | canmsgTx.id = 0x600+nodeID; |
ngokystk | 0:b4b94eb28093 | 540 | canmsgTx.len = 6; //Data Length |
ngokystk | 0:b4b94eb28093 | 541 | canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| |
ngokystk | 0:b4b94eb28093 | 542 | canmsgTx.data[1] = 0x40;//Index LowByte |
ngokystk | 0:b4b94eb28093 | 543 | canmsgTx.data[2] = 0x60;//Index HighByte |
ngokystk | 0:b4b94eb28093 | 544 | canmsgTx.data[3] = 0x00;//sub-Index |
ngokystk | 0:b4b94eb28093 | 545 | canmsgTx.data[4] = 0x06;//data:0x00"06" = "Controlword(Shutdown)" |
ngokystk | 0:b4b94eb28093 | 546 | canmsgTx.data[5] = 0x00;//data:0x"00"06 |
ngokystk | 0:b4b94eb28093 | 547 | /* |
ngokystk | 0:b4b94eb28093 | 548 | canmsgTx.data[6] = 0x00;//data:(user value) |
ngokystk | 0:b4b94eb28093 | 549 | canmsgTx.data[7] = 0x00;//data:(user value) |
ngokystk | 0:b4b94eb28093 | 550 | */ |
ngokystk | 0:b4b94eb28093 | 551 | printCANTX(); //CAN送信データをPCに表示 |
ngokystk | 0:b4b94eb28093 | 552 | canPort.write(canmsgTx);//CANでデータ送信 |
ngokystk | 0:b4b94eb28093 | 553 | wait_ms(1); |
ngokystk | 0:b4b94eb28093 | 554 | } |
Tomo1213 | 1:b2bd1511307e | 555 | |
ngokystk | 0:b4b94eb28093 | 556 | //0x2B-6040-00-000F-//-// |
ngokystk | 0:b4b94eb28093 | 557 | void sendCtrlEN(int nodeID){ |
ngokystk | 0:b4b94eb28093 | 558 | canmsgTx.id = 0x600+nodeID; |
ngokystk | 0:b4b94eb28093 | 559 | canmsgTx.len = 6; //Data Length |
ngokystk | 0:b4b94eb28093 | 560 | canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| |
ngokystk | 0:b4b94eb28093 | 561 | canmsgTx.data[1] = 0x40;//Index LowByte |
ngokystk | 0:b4b94eb28093 | 562 | canmsgTx.data[2] = 0x60;//Index HighByte |
ngokystk | 0:b4b94eb28093 | 563 | canmsgTx.data[3] = 0x00;//sub-Index |
ngokystk | 0:b4b94eb28093 | 564 | canmsgTx.data[4] = 0x0F;//data:0x00"0F" = "Controlword(Enable)" |
ngokystk | 0:b4b94eb28093 | 565 | canmsgTx.data[5] = 0x00;//data:0x"00"0F |
ngokystk | 0:b4b94eb28093 | 566 | /* |
ngokystk | 0:b4b94eb28093 | 567 | canmsgTx.data[6] = 0x00;//data:(user value) |
ngokystk | 0:b4b94eb28093 | 568 | canmsgTx.data[7] = 0x00;//data:(user value) |
ngokystk | 0:b4b94eb28093 | 569 | */ |
ngokystk | 0:b4b94eb28093 | 570 | //printCANTX(); //CAN送信データをPCに表示 |
ngokystk | 0:b4b94eb28093 | 571 | canPort.write(canmsgTx);//CANでデータ送信 |
ngokystk | 0:b4b94eb28093 | 572 | wait_ms(1); |
ngokystk | 0:b4b94eb28093 | 573 | } |
Tomo1213 | 1:b2bd1511307e | 574 | |
ngokystk | 0:b4b94eb28093 | 575 | //0x2B-6040-00-000B-//-// |
ngokystk | 0:b4b94eb28093 | 576 | void sendCtrlQS(int nodeID){ |
ngokystk | 0:b4b94eb28093 | 577 | canmsgTx.id = 0x600+nodeID; |
ngokystk | 0:b4b94eb28093 | 578 | canmsgTx.len = 6; //Data Length |
ngokystk | 0:b4b94eb28093 | 579 | canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| |
ngokystk | 0:b4b94eb28093 | 580 | canmsgTx.data[1] = 0x40;//Index LowByte |
ngokystk | 0:b4b94eb28093 | 581 | canmsgTx.data[2] = 0x60;//Index HighByte |
ngokystk | 0:b4b94eb28093 | 582 | canmsgTx.data[3] = 0x00;//sub-Index |
ngokystk | 0:b4b94eb28093 | 583 | canmsgTx.data[4] = 0x0B;//data:0x00"0B" = "Quick Stop" |
ngokystk | 0:b4b94eb28093 | 584 | canmsgTx.data[5] = 0x00;//data:0x"00"0B |
ngokystk | 0:b4b94eb28093 | 585 | /* |
ngokystk | 0:b4b94eb28093 | 586 | canmsgTx.data[6] = 0x00;//data:(user value) |
ngokystk | 0:b4b94eb28093 | 587 | canmsgTx.data[7] = 0x00;//data:(user value) |
ngokystk | 0:b4b94eb28093 | 588 | */ |
ngokystk | 0:b4b94eb28093 | 589 | printCANTX(); //CAN送信データをPCに表示 |
ngokystk | 0:b4b94eb28093 | 590 | canPort.write(canmsgTx);//CANでデータ送信 |
ngokystk | 0:b4b94eb28093 | 591 | wait_ms(1); |
ngokystk | 0:b4b94eb28093 | 592 | } |
Tomo1213 | 1:b2bd1511307e | 593 | |
ngokystk | 0:b4b94eb28093 | 594 | //0x2B-6040-00-010F-//-// |
ngokystk | 0:b4b94eb28093 | 595 | void sendCtrlHL(int nodeID){ |
ngokystk | 0:b4b94eb28093 | 596 | canmsgTx.id = 0x600+nodeID; |
ngokystk | 0:b4b94eb28093 | 597 | canmsgTx.len = 6; //Data Length |
ngokystk | 0:b4b94eb28093 | 598 | canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| |
ngokystk | 0:b4b94eb28093 | 599 | canmsgTx.data[1] = 0x40;//Index LowByte |
ngokystk | 0:b4b94eb28093 | 600 | canmsgTx.data[2] = 0x60;//Index HighByte |
ngokystk | 0:b4b94eb28093 | 601 | canmsgTx.data[3] = 0x00;//sub-Index |
ngokystk | 0:b4b94eb28093 | 602 | canmsgTx.data[4] = 0x0F;//data:0x01"0F" = "Halt" |
ngokystk | 0:b4b94eb28093 | 603 | canmsgTx.data[5] = 0x01;//data:0x"01"0F |
ngokystk | 0:b4b94eb28093 | 604 | /* |
ngokystk | 0:b4b94eb28093 | 605 | canmsgTx.data[6] = 0x00;//data:(user value) |
ngokystk | 0:b4b94eb28093 | 606 | canmsgTx.data[7] = 0x00;//data:(user value) |
ngokystk | 0:b4b94eb28093 | 607 | */ |
ngokystk | 0:b4b94eb28093 | 608 | printCANTX(); //CAN送信データをPCに表示 |
ngokystk | 0:b4b94eb28093 | 609 | canPort.write(canmsgTx);//CANでデータ送信 |
ngokystk | 0:b4b94eb28093 | 610 | wait_ms(1); |
ngokystk | 0:b4b94eb28093 | 611 | } |
Tomo1213 | 1:b2bd1511307e | 612 | |
ngokystk | 0:b4b94eb28093 | 613 | //0x2B-60FF-00-[user data(4Byte)] |
ngokystk | 0:b4b94eb28093 | 614 | void sendTgtTrq(int nodeID,int trq){ |
ngokystk | 0:b4b94eb28093 | 615 | //pc.printf("%dmA0x%08x\r\n",trq,trq); //回転数送信データの表示 |
ngokystk | 0:b4b94eb28093 | 616 | canmsgTx.id = 0x600+nodeID; |
ngokystk | 0:b4b94eb28093 | 617 | canmsgTx.len = 6; //Data Length |
ngokystk | 0:b4b94eb28093 | 618 | canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| |
ngokystk | 0:b4b94eb28093 | 619 | canmsgTx.data[1] = 0x30;//Index LowByte 71 |
ngokystk | 0:b4b94eb28093 | 620 | canmsgTx.data[2] = 0x20;//Index HighByte 60 |
ngokystk | 0:b4b94eb28093 | 621 | canmsgTx.data[3] = 0x00;//sub-Index |
ngokystk | 0:b4b94eb28093 | 622 | //下位から1Byteずつdataに格納 |
ngokystk | 0:b4b94eb28093 | 623 | if(trq<0){ |
Tomo1213 | 3:c39c14cfc811 | 624 | trq=0xFFFF+trq+1; |
ngokystk | 0:b4b94eb28093 | 625 | } |
ngokystk | 0:b4b94eb28093 | 626 | |
ngokystk | 0:b4b94eb28093 | 627 | //pc.printf("iii%d\r\n",trq); |
ngokystk | 0:b4b94eb28093 | 628 | //canmsgTx.data[7]=((trq>>24)&0xFF); |
ngokystk | 0:b4b94eb28093 | 629 | //canmsgTx.data[6]=((trq>>16)&0xFF); |
ngokystk | 0:b4b94eb28093 | 630 | |
ngokystk | 0:b4b94eb28093 | 631 | canmsgTx.data[5]=((trq>>8)&0xFF); |
ngokystk | 0:b4b94eb28093 | 632 | canmsgTx.data[4]=((trq>>0)&0xFF); |
ngokystk | 0:b4b94eb28093 | 633 | //printCANTX(); //CAN送信データをPCに表示 |
ngokystk | 0:b4b94eb28093 | 634 | canPort.write(canmsgTx);//CANでデータ送信 |
ngokystk | 0:b4b94eb28093 | 635 | wait_ms(2); |
ngokystk | 0:b4b94eb28093 | 636 | //send Enable |
ngokystk | 0:b4b94eb28093 | 637 | //pc.printf("Send Enable Command\r\n"); |
ngokystk | 0:b4b94eb28093 | 638 | //sendCtrlEN(nodeID); |
ngokystk | 0:b4b94eb28093 | 639 | //wait(0.5); |
ngokystk | 0:b4b94eb28093 | 640 | } |
Tomo1213 | 1:b2bd1511307e | 641 | |
Tomo1213 | 3:c39c14cfc811 | 642 | |
Tomo1213 | 3:c39c14cfc811 | 643 | |
Tomo1213 | 3:c39c14cfc811 | 644 | |
ngokystk | 0:b4b94eb28093 | 645 | //0x2B-60FF-00-[user data(4Byte)] |
ngokystk | 0:b4b94eb28093 | 646 | void sendTgtVel(int nodeID,int rpm){ |
ngokystk | 0:b4b94eb28093 | 647 | //pc.printf("%drpm|0x%08x\r\n",rpm,rpm); //回転数送信データの表示 |
ngokystk | 0:b4b94eb28093 | 648 | canmsgTx.id = 0x600+nodeID; |
ngokystk | 0:b4b94eb28093 | 649 | canmsgTx.len = 8; //Data Length |
ngokystk | 0:b4b94eb28093 | 650 | canmsgTx.data[0] = 0x23;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| |
ngokystk | 0:b4b94eb28093 | 651 | canmsgTx.data[1] = 0xFF;//Index LowByte |
ngokystk | 0:b4b94eb28093 | 652 | canmsgTx.data[2] = 0x60;//Index HighByte |
ngokystk | 0:b4b94eb28093 | 653 | canmsgTx.data[3] = 0x00;//sub-Index |
ngokystk | 0:b4b94eb28093 | 654 | //下位から1Byteずつdataに格納 |
ngokystk | 0:b4b94eb28093 | 655 | |
ngokystk | 0:b4b94eb28093 | 656 | //pc.printf("%d\r\n",rpm); |
ngokystk | 0:b4b94eb28093 | 657 | if(rpm<0){ |
Tomo1213 | 3:c39c14cfc811 | 658 | rpm=0xFFFFFFFF+rpm+1; |
ngokystk | 0:b4b94eb28093 | 659 | } |
ngokystk | 0:b4b94eb28093 | 660 | canmsgTx.data[7]=((rpm>>24)&0xFF); |
ngokystk | 0:b4b94eb28093 | 661 | canmsgTx.data[6]=((rpm>>16)&0xFF); |
ngokystk | 0:b4b94eb28093 | 662 | canmsgTx.data[5]=((rpm>>8)&0xFF); |
ngokystk | 0:b4b94eb28093 | 663 | canmsgTx.data[4]=((rpm>>0)&0xFF); |
ngokystk | 0:b4b94eb28093 | 664 | |
ngokystk | 0:b4b94eb28093 | 665 | //printCANTX(); //CAN送信データをPCに表示 |
ngokystk | 0:b4b94eb28093 | 666 | canPort.write(canmsgTx);//CANでデータ送信 |
ngokystk | 0:b4b94eb28093 | 667 | wait_ms(1); |
ngokystk | 0:b4b94eb28093 | 668 | |
ngokystk | 0:b4b94eb28093 | 669 | } |
Tomo1213 | 3:c39c14cfc811 | 670 | |
ngokystk | 0:b4b94eb28093 | 671 | void readActVel(int nodeID){ |
ngokystk | 0:b4b94eb28093 | 672 | //値が欲しいobjectのアドレスを送る |
ngokystk | 0:b4b94eb28093 | 673 | canmsgTx.id = 0x600+nodeID; |
ngokystk | 0:b4b94eb28093 | 674 | canmsgTx.len = 4; //Data Length |
ngokystk | 0:b4b94eb28093 | 675 | canmsgTx.data[0] = 0x40;//|0Byte:40| |
ngokystk | 0:b4b94eb28093 | 676 | canmsgTx.data[1] = 0x6C;//Index LowByte |
ngokystk | 0:b4b94eb28093 | 677 | canmsgTx.data[2] = 0x60;//Index HighByte |
ngokystk | 0:b4b94eb28093 | 678 | canmsgTx.data[3] = 0x00;//sub-Index |
ngokystk | 0:b4b94eb28093 | 679 | canPort.write(canmsgTx); |
ngokystk | 0:b4b94eb28093 | 680 | wait_ms(1); |
ngokystk | 0:b4b94eb28093 | 681 | } |
Tomo1213 | 1:b2bd1511307e | 682 | |
ngokystk | 0:b4b94eb28093 | 683 | void readActPos(int nodeID){ |
ngokystk | 0:b4b94eb28093 | 684 | //値が欲しいobjectのアドレスを送る |
ngokystk | 0:b4b94eb28093 | 685 | canmsgTx.id = 0x600+nodeID; |
ngokystk | 0:b4b94eb28093 | 686 | canmsgTx.len = 4; //Data Length |
ngokystk | 0:b4b94eb28093 | 687 | canmsgTx.data[0] = 0x40;//|0Byte:40| |
ngokystk | 0:b4b94eb28093 | 688 | canmsgTx.data[1] = 0x64;//Index LowByte |
ngokystk | 0:b4b94eb28093 | 689 | canmsgTx.data[2] = 0x60;//Index HighByte |
ngokystk | 0:b4b94eb28093 | 690 | canmsgTx.data[3] = 0x00;//sub-Index |
ngokystk | 0:b4b94eb28093 | 691 | canPort.write(canmsgTx); |
ngokystk | 0:b4b94eb28093 | 692 | wait_ms(1); |
ngokystk | 0:b4b94eb28093 | 693 | } |
Tomo1213 | 1:b2bd1511307e | 694 | |
ngokystk | 0:b4b94eb28093 | 695 | //加速度指定 |
ngokystk | 0:b4b94eb28093 | 696 | void sendProAcc(int nodeID,int rpm){ |
ngokystk | 0:b4b94eb28093 | 697 | if(rpm < 0){ |
ngokystk | 0:b4b94eb28093 | 698 | rpm += 0xFFFFFFFF; |
ngokystk | 0:b4b94eb28093 | 699 | } |
ngokystk | 0:b4b94eb28093 | 700 | // pc.printf("%drpm|0x%08x\r\n",rpm,rpm); //回転数送信データの表示 |
ngokystk | 0:b4b94eb28093 | 701 | canmsgTx.id = 0x600+nodeID; |
ngokystk | 0:b4b94eb28093 | 702 | canmsgTx.len = 8; //Data Length |
ngokystk | 0:b4b94eb28093 | 703 | canmsgTx.data[0] = 0x23;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| |
ngokystk | 0:b4b94eb28093 | 704 | canmsgTx.data[1] = 0x83;//Index LowByte |
ngokystk | 0:b4b94eb28093 | 705 | canmsgTx.data[2] = 0x60;//Index HighByte |
ngokystk | 0:b4b94eb28093 | 706 | canmsgTx.data[3] = 0x00;//sub-Index |
ngokystk | 0:b4b94eb28093 | 707 | //下位から1Byteずつdataに格納 |
ngokystk | 0:b4b94eb28093 | 708 | canmsgTx.data[4] = (rpm >> 0) & 0xFF; |
ngokystk | 0:b4b94eb28093 | 709 | canmsgTx.data[5] = (rpm >> 8) & 0xFF; |
ngokystk | 0:b4b94eb28093 | 710 | canmsgTx.data[6] = (rpm >> 16) & 0xFF; |
ngokystk | 0:b4b94eb28093 | 711 | canmsgTx.data[7] = (rpm >> 24) & 0xFF; |
ngokystk | 0:b4b94eb28093 | 712 | // printCANTX(); //CAN送信データをPCに表示 |
ngokystk | 0:b4b94eb28093 | 713 | canPort.write(canmsgTx);//CANでデータ送信 |
ngokystk | 0:b4b94eb28093 | 714 | wait(0.01); |
ngokystk | 0:b4b94eb28093 | 715 | //send Enable |
ngokystk | 0:b4b94eb28093 | 716 | // pc.printf("Send Enable Command\r\n"); |
ngokystk | 0:b4b94eb28093 | 717 | sendCtrlEN(nodeID); |
ngokystk | 0:b4b94eb28093 | 718 | wait(0.01); |
ngokystk | 0:b4b94eb28093 | 719 | } |
Tomo1213 | 1:b2bd1511307e | 720 | |
ngokystk | 0:b4b94eb28093 | 721 | //減速度指定 |
ngokystk | 0:b4b94eb28093 | 722 | void sendProDec(int nodeID,int rpm){ |
ngokystk | 0:b4b94eb28093 | 723 | if(rpm < 0){ |
ngokystk | 0:b4b94eb28093 | 724 | rpm += 0xFFFFFFFF; |
Tomo1213 | 3:c39c14cfc811 | 725 | } |
ngokystk | 0:b4b94eb28093 | 726 | // pc.printf("%drpm|0x%08x\r\n",rpm,rpm); //回転数送信データの表示 |
ngokystk | 0:b4b94eb28093 | 727 | canmsgTx.id = 0x600+nodeID; |
ngokystk | 0:b4b94eb28093 | 728 | canmsgTx.len = 8; //Data Length |
ngokystk | 0:b4b94eb28093 | 729 | canmsgTx.data[0] = 0x23;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| |
ngokystk | 0:b4b94eb28093 | 730 | canmsgTx.data[1] = 0x84;//Index LowByte |
ngokystk | 0:b4b94eb28093 | 731 | canmsgTx.data[2] = 0x60;//Index HighByte |
ngokystk | 0:b4b94eb28093 | 732 | canmsgTx.data[3] = 0x00;//sub-Index |
ngokystk | 0:b4b94eb28093 | 733 | //下位から1Byteずつdataに格納 |
ngokystk | 0:b4b94eb28093 | 734 | canmsgTx.data[4] = (rpm >> 0) & 0xFF; |
ngokystk | 0:b4b94eb28093 | 735 | canmsgTx.data[5] = (rpm >> 8) & 0xFF; |
ngokystk | 0:b4b94eb28093 | 736 | canmsgTx.data[6] = (rpm >> 16) & 0xFF; |
ngokystk | 0:b4b94eb28093 | 737 | canmsgTx.data[7] = (rpm >> 24) & 0xFF; |
ngokystk | 0:b4b94eb28093 | 738 | // printCANTX(); //CAN送信データをPCに表示 |
ngokystk | 0:b4b94eb28093 | 739 | canPort.write(canmsgTx);//CANでデータ送信 |
ngokystk | 0:b4b94eb28093 | 740 | wait(0.01); |
ngokystk | 0:b4b94eb28093 | 741 | //send Enable |
ngokystk | 0:b4b94eb28093 | 742 | // pc.printf("Send Enable Command\r\n"); |
ngokystk | 0:b4b94eb28093 | 743 | sendCtrlEN(nodeID); |
ngokystk | 0:b4b94eb28093 | 744 | wait(0.01); |
ngokystk | 0:b4b94eb28093 | 745 | } |
Tomo1213 | 1:b2bd1511307e | 746 | |
Tomo1213 | 1:b2bd1511307e | 747 | |
ngokystk | 0:b4b94eb28093 | 748 | //送信データの表示 |
ngokystk | 0:b4b94eb28093 | 749 | void printCANTX(void){ |
ngokystk | 0:b4b94eb28093 | 750 | //0x canID|Byte0|Byte1|Byte2|Byte3|Byte4|Byte5|Byte6|Byte7| |
Tomo1213 | 4:1bd08c9d92a9 | 751 | //pc.printf("0x%3x|",canmsgTx.id); |
ngokystk | 0:b4b94eb28093 | 752 | for(char i=0;i < canmsgTx.len;i++){ |
Tomo1213 | 4:1bd08c9d92a9 | 753 | //pc.printf("%02x|",canmsgTx.data[i]); |
ngokystk | 0:b4b94eb28093 | 754 | } |
ngokystk | 0:b4b94eb28093 | 755 | //pc.printf("\r\n"); |
ngokystk | 0:b4b94eb28093 | 756 | } |
Tomo1213 | 1:b2bd1511307e | 757 | |
ngokystk | 0:b4b94eb28093 | 758 | //受信データの表示 |
Tomo1213 | 1:b2bd1511307e | 759 | |
ngokystk | 0:b4b94eb28093 | 760 | void printCANRX(void){ |
ngokystk | 0:b4b94eb28093 | 761 | //0x canID|Byte0|Byte1|Byte2|Byte3|Byte4|Byte5|Byte6|Byte7| |
ngokystk | 0:b4b94eb28093 | 762 | //pc.printf("0x%3x|",canmsgRx.id); |
ngokystk | 0:b4b94eb28093 | 763 | for(char i=0;i < canmsgRx.len;i++){ |
ngokystk | 0:b4b94eb28093 | 764 | //pc.printf("%02x|",canmsgRx.data[i]); |
ngokystk | 0:b4b94eb28093 | 765 | } |
Tomo1213 | 4:1bd08c9d92a9 | 766 | //pc.printf("\r\n"); |
ngokystk | 0:b4b94eb28093 | 767 | } |
Tomo1213 | 1:b2bd1511307e | 768 | |
ngokystk | 0:b4b94eb28093 | 769 | void CANdataRX(void){ |
ngokystk | 0:b4b94eb28093 | 770 | canPort.read(canmsgRx); |
ngokystk | 0:b4b94eb28093 | 771 | printCANRX(); |
ngokystk | 0:b4b94eb28093 | 772 | } |
Tomo1213 | 1:b2bd1511307e | 773 | |
ngokystk | 0:b4b94eb28093 | 774 | void SerialRX(void){ |
ngokystk | 0:b4b94eb28093 | 775 | Serialdata = pc.getc(); |
ngokystk | 0:b4b94eb28093 | 776 | //pc.printf("%c\r\n",Serialdata); |
Tomo1213 | 4:1bd08c9d92a9 | 777 | } |