Kanta Takasu
/
YUKA_ak_home_pcon
add pcon f and b
main.cpp@4:e1aa0b348aeb, 2021-12-19 (annotated)
- Committer:
- tks1
- Date:
- Sun Dec 19 15:43:05 2021 +0000
- Revision:
- 4:e1aa0b348aeb
- Parent:
- 3:c39c14cfc811
- Child:
- 5:a7c3f446a1f1
sonic print; front onry
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ngokystk | 0:b4b94eb28093 | 1 | //2021/12/18更新 |
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 | 1:b2bd1511307e | 14 | #define ACC_B 1000 |
Tomo1213 | 1:b2bd1511307e | 15 | #define DEC_B 700 |
Tomo1213 | 1:b2bd1511307e | 16 | #define ACC_C 1000 |
Tomo1213 | 1:b2bd1511307e | 17 | #define DEC_C 1000 |
ngokystk | 0:b4b94eb28093 | 18 | |
Tomo1213 | 1:b2bd1511307e | 19 | #define KICK 2000 |
Tomo1213 | 1:b2bd1511307e | 20 | |
Tomo1213 | 3:c39c14cfc811 | 21 | |
Tomo1213 | 3:c39c14cfc811 | 22 | |
ngokystk | 0:b4b94eb28093 | 23 | DigitalOut led1(LED1); |
ngokystk | 0:b4b94eb28093 | 24 | DigitalOut led2(LED2); |
ngokystk | 0:b4b94eb28093 | 25 | DigitalOut led3(LED3); |
ngokystk | 0:b4b94eb28093 | 26 | DigitalOut led4(LED4); |
Tomo1213 | 1:b2bd1511307e | 27 | |
ngokystk | 0:b4b94eb28093 | 28 | Timer t; |
ngokystk | 0:b4b94eb28093 | 29 | double Time = 0; |
ngokystk | 0:b4b94eb28093 | 30 | #define Standby_Time 10 |
ngokystk | 0:b4b94eb28093 | 31 | Serial pc(USBTX, USBRX); |
ngokystk | 0:b4b94eb28093 | 32 | char Serialdata; |
ngokystk | 0:b4b94eb28093 | 33 | BusOut myled(LED1, LED2, LED3, LED4); |
Tomo1213 | 1:b2bd1511307e | 34 | |
ngokystk | 0:b4b94eb28093 | 35 | //HCSR04 u1(p13, p14), u2(p11, p12), u3(p23, p24), u4(p25, p26); // Trigger(DO), Echo(PWMIN); LPC1768 |
ngokystk | 0:b4b94eb28093 | 36 | HCSR04 u1(PA_0, PA_1),u2(PA_5, PA_6),u3(PB_0, PA_10); // Trigger(DO), Echo(PWMIN); f303k8 |
Tomo1213 | 1:b2bd1511307e | 37 | |
ngokystk | 0:b4b94eb28093 | 38 | CANMessage canmsgTx; |
ngokystk | 0:b4b94eb28093 | 39 | CANMessage canmsgRx; |
ngokystk | 0:b4b94eb28093 | 40 | //CAN canPort(p30, p29); //CAN name(PinName rd, PinName td) LPC1768 |
ngokystk | 0:b4b94eb28093 | 41 | CAN canPort(PA_11, PA_12); //CAN name(PinName rd, PinName td) F303k8 |
Tomo1213 | 1:b2bd1511307e | 42 | |
ngokystk | 0:b4b94eb28093 | 43 | //プロトタイプ宣言 |
ngokystk | 0:b4b94eb28093 | 44 | //------------------send関数------------------- |
ngokystk | 0:b4b94eb28093 | 45 | //mode Setting |
ngokystk | 0:b4b94eb28093 | 46 | void sendOPModeT(int); //Operating Mode |
ngokystk | 0:b4b94eb28093 | 47 | void sendOPModeV(int); //Operating Mode |
ngokystk | 0:b4b94eb28093 | 48 | //Control Word |
ngokystk | 0:b4b94eb28093 | 49 | void sendCtrlRS(int); //Reset |
ngokystk | 0:b4b94eb28093 | 50 | void sendCtrlSD(int); //Shutdown |
ngokystk | 0:b4b94eb28093 | 51 | void sendCtrlEN(int); //Switch on & Enable |
ngokystk | 0:b4b94eb28093 | 52 | void sendCtrlQS(int); //Quick Stop |
ngokystk | 0:b4b94eb28093 | 53 | void sendCtrlHL(int); //Halt |
ngokystk | 0:b4b94eb28093 | 54 | //Velocity Setting |
ngokystk | 0:b4b94eb28093 | 55 | void sendTgtVel(int,int); //Target Velocity |
ngokystk | 0:b4b94eb28093 | 56 | //Torque Setting |
ngokystk | 0:b4b94eb28093 | 57 | void sendTgtTrq(int,int); //Target Torque |
ngokystk | 0:b4b94eb28093 | 58 | //Acceleration Setting |
ngokystk | 0:b4b94eb28093 | 59 | void sendProAcc(int,int); //Plof Acceleration |
ngokystk | 0:b4b94eb28093 | 60 | void sendProDec(int,int); //Plof Deceleration |
ngokystk | 0:b4b94eb28093 | 61 | //------------------read関数------------------- |
ngokystk | 0:b4b94eb28093 | 62 | void readActVel(int); //Actual Velocity |
ngokystk | 0:b4b94eb28093 | 63 | void readActPos(int); //Actual Position |
ngokystk | 0:b4b94eb28093 | 64 | //-------------------その他-------------------- |
ngokystk | 0:b4b94eb28093 | 65 | void printCANTX(void); //CAN送信データをPCに表示 |
ngokystk | 0:b4b94eb28093 | 66 | void printCANRX(void); //CAN受信データをPCに表示 |
ngokystk | 0:b4b94eb28093 | 67 | void CANdataRX(void); //CAN受信処理 |
ngokystk | 0:b4b94eb28093 | 68 | void SerialRX(void); //Serial受信処理 |
Tomo1213 | 1:b2bd1511307e | 69 | //--------------------------------------------- |
Tomo1213 | 1:b2bd1511307e | 70 | void set_ACC(int); |
Tomo1213 | 1:b2bd1511307e | 71 | void set_DEC(int); |
Tomo1213 | 1:b2bd1511307e | 72 | void set_MODE_V(void); |
Tomo1213 | 1:b2bd1511307e | 73 | void set_MODE_T(void); |
ngokystk | 0:b4b94eb28093 | 74 | |
ngokystk | 0:b4b94eb28093 | 75 | int nodeall=4; |
Tomo1213 | 1:b2bd1511307e | 76 | |
ngokystk | 0:b4b94eb28093 | 77 | int main(){ |
ngokystk | 0:b4b94eb28093 | 78 | //Serial |
ngokystk | 0:b4b94eb28093 | 79 | pc.attach(SerialRX); |
ngokystk | 0:b4b94eb28093 | 80 | //pc.baud(115200); |
ngokystk | 0:b4b94eb28093 | 81 | |
ngokystk | 0:b4b94eb28093 | 82 | //CAN |
ngokystk | 0:b4b94eb28093 | 83 | canPort.frequency(1000000); //Bit Rate:1MHz |
ngokystk | 0:b4b94eb28093 | 84 | canPort.attach(CANdataRX,CAN::RxIrq); |
ngokystk | 0:b4b94eb28093 | 85 | int node1 = 1; //CAN node Setting |
ngokystk | 0:b4b94eb28093 | 86 | int node2 = 2; |
ngokystk | 0:b4b94eb28093 | 87 | int node3 = 3; |
ngokystk | 0:b4b94eb28093 | 88 | int node4 = 4; |
ngokystk | 0:b4b94eb28093 | 89 | |
Tomo1213 | 3:c39c14cfc811 | 90 | //モータ回転数 |
Tomo1213 | 3:c39c14cfc811 | 91 | int rpm = 600; |
Tomo1213 | 3:c39c14cfc811 | 92 | |
ngokystk | 0:b4b94eb28093 | 93 | //各モータ回転数 |
Tomo1213 | 3:c39c14cfc811 | 94 | int rpm1 = rpm; //Velocity Setting[rpm] |
Tomo1213 | 3:c39c14cfc811 | 95 | int rpm2 = rpm; //Velocity Setting[rpm] |
Tomo1213 | 3:c39c14cfc811 | 96 | int rpm3 = rpm; //Velocity Setting[rpm] |
Tomo1213 | 3:c39c14cfc811 | 97 | int rpm4 = rpm; //Velocity Setting[rpm] |
ngokystk | 0:b4b94eb28093 | 98 | |
ngokystk | 0:b4b94eb28093 | 99 | //エンコーダ関係 |
ngokystk | 0:b4b94eb28093 | 100 | int ActPos = 0; |
ngokystk | 0:b4b94eb28093 | 101 | int Init_Pos = 0; |
ngokystk | 0:b4b94eb28093 | 102 | |
ngokystk | 0:b4b94eb28093 | 103 | //超音波センサ関係パラメータ |
Tomo1213 | 3:c39c14cfc811 | 104 | int max_rpm = rpm + 200; |
Tomo1213 | 3:c39c14cfc811 | 105 | int min_rpm = rpm -200; |
ngokystk | 0:b4b94eb28093 | 106 | int wall_distance = 0; |
ngokystk | 0:b4b94eb28093 | 107 | int dist1, dist2,dist3, dist4; |
ngokystk | 0:b4b94eb28093 | 108 | |
ngokystk | 0:b4b94eb28093 | 109 | |
ngokystk | 0:b4b94eb28093 | 110 | //PID制御関係 |
ngokystk | 0:b4b94eb28093 | 111 | //角度調整パラメータ |
ngokystk | 0:b4b94eb28093 | 112 | int p1, i1, d1; |
ngokystk | 0:b4b94eb28093 | 113 | int error_before1 = 0; |
ngokystk | 0:b4b94eb28093 | 114 | int error_now1 = 0; |
ngokystk | 0:b4b94eb28093 | 115 | int feedback_val1; |
ngokystk | 0:b4b94eb28093 | 116 | int integral1; |
ngokystk | 0:b4b94eb28093 | 117 | int pid_sum1; |
ngokystk | 0:b4b94eb28093 | 118 | #define DELTA_T1 0.1 |
ngokystk | 0:b4b94eb28093 | 119 | #define target_val1 0 |
ngokystk | 0:b4b94eb28093 | 120 | #define Kp1 3 |
ngokystk | 0:b4b94eb28093 | 121 | #define Ki1 0 |
ngokystk | 0:b4b94eb28093 | 122 | #define Kd1 0 |
Tomo1213 | 1:b2bd1511307e | 123 | |
ngokystk | 0:b4b94eb28093 | 124 | pc.printf("YUKA PROGRAM START\r\n"); |
ngokystk | 0:b4b94eb28093 | 125 | wait(0.1); |
ngokystk | 0:b4b94eb28093 | 126 | //-------------起動時に必ず送信--------------- |
ngokystk | 0:b4b94eb28093 | 127 | //オペレーティングモードを送信 |
Tomo1213 | 3:c39c14cfc811 | 128 | sendOPModeT(node1); |
Tomo1213 | 3:c39c14cfc811 | 129 | sendOPModeT(node2); |
Tomo1213 | 3:c39c14cfc811 | 130 | sendOPModeT(node3); |
Tomo1213 | 3:c39c14cfc811 | 131 | sendOPModeT(node4); |
Tomo1213 | 3:c39c14cfc811 | 132 | |
ngokystk | 0:b4b94eb28093 | 133 | //Shutdown,Enableコマンド送信|リセット |
ngokystk | 0:b4b94eb28093 | 134 | sendCtrlSD(node1); |
ngokystk | 0:b4b94eb28093 | 135 | sendCtrlSD(node2); |
ngokystk | 0:b4b94eb28093 | 136 | sendCtrlSD(node3); |
ngokystk | 0:b4b94eb28093 | 137 | sendCtrlSD(node4); |
Tomo1213 | 1:b2bd1511307e | 138 | |
ngokystk | 0:b4b94eb28093 | 139 | sendCtrlEN(node1); |
ngokystk | 0:b4b94eb28093 | 140 | sendCtrlEN(node2); |
ngokystk | 0:b4b94eb28093 | 141 | sendCtrlEN(node3); |
ngokystk | 0:b4b94eb28093 | 142 | sendCtrlEN(node4); |
ngokystk | 0:b4b94eb28093 | 143 | |
ngokystk | 0:b4b94eb28093 | 144 | //初期加減速度 |
Tomo1213 | 3:c39c14cfc811 | 145 | int Acc = 2000; |
Tomo1213 | 3:c39c14cfc811 | 146 | int Dec = 2000; |
Tomo1213 | 3:c39c14cfc811 | 147 | |
Tomo1213 | 3:c39c14cfc811 | 148 | sendProAcc(1,Acc); |
Tomo1213 | 3:c39c14cfc811 | 149 | sendProAcc(2,Acc); |
Tomo1213 | 3:c39c14cfc811 | 150 | sendProAcc(3,Acc); |
Tomo1213 | 3:c39c14cfc811 | 151 | sendProAcc(4,Acc); |
Tomo1213 | 3:c39c14cfc811 | 152 | |
Tomo1213 | 3:c39c14cfc811 | 153 | sendProDec(1,Dec); |
Tomo1213 | 3:c39c14cfc811 | 154 | sendProDec(2,Dec); |
Tomo1213 | 3:c39c14cfc811 | 155 | sendProDec(3,Dec); |
Tomo1213 | 3:c39c14cfc811 | 156 | sendProDec(4,Dec); |
Tomo1213 | 3:c39c14cfc811 | 157 | |
ngokystk | 0:b4b94eb28093 | 158 | //トルク設定 |
ngokystk | 0:b4b94eb28093 | 159 | int trq = 100; //torque Setting[mA] |
ngokystk | 0:b4b94eb28093 | 160 | |
ngokystk | 0:b4b94eb28093 | 161 | sendTgtTrq(1,trq); |
ngokystk | 0:b4b94eb28093 | 162 | sendTgtTrq(2,trq); |
ngokystk | 0:b4b94eb28093 | 163 | sendTgtTrq(3,trq); |
ngokystk | 0:b4b94eb28093 | 164 | sendTgtTrq(4,trq); |
Tomo1213 | 1:b2bd1511307e | 165 | |
Tomo1213 | 1:b2bd1511307e | 166 | while(1){ |
Tomo1213 | 1:b2bd1511307e | 167 | |
Tomo1213 | 1:b2bd1511307e | 168 | ActPos = 0; |
Tomo1213 | 1:b2bd1511307e | 169 | Init_Pos = 0; |
tks1 | 4:e1aa0b348aeb | 170 | dist1 = 0; |
Tomo1213 | 1:b2bd1511307e | 171 | readActPos(1); |
Tomo1213 | 1:b2bd1511307e | 172 | ActPos=canmsgRx.data[4]+canmsgRx.data[5]*256+canmsgRx.data[6]*65536; |
Tomo1213 | 1:b2bd1511307e | 173 | if(ActPos > 8388608){ |
Tomo1213 | 1:b2bd1511307e | 174 | ActPos -= 0x1000000;//16進数でのマイナス処理 16^7 = 16,777,216 = 0x1000000 |
ngokystk | 0:b4b94eb28093 | 175 | } |
Tomo1213 | 1:b2bd1511307e | 176 | Init_Pos = ActPos;//起動時の角度を保存 |
ngokystk | 0:b4b94eb28093 | 177 | t.reset(); |
ngokystk | 0:b4b94eb28093 | 178 | t.start(); |
Tomo1213 | 1:b2bd1511307e | 179 | |
ngokystk | 0:b4b94eb28093 | 180 | while(1){ |
ngokystk | 0:b4b94eb28093 | 181 | |
Tomo1213 | 1:b2bd1511307e | 182 | Time = t.read(); |
ngokystk | 0:b4b94eb28093 | 183 | |
Tomo1213 | 1:b2bd1511307e | 184 | readActPos(1); |
Tomo1213 | 1:b2bd1511307e | 185 | ActPos=canmsgRx.data[4]+canmsgRx.data[5]*256+canmsgRx.data[6]*65536; |
Tomo1213 | 1:b2bd1511307e | 186 | if(ActPos > 8388608){ |
Tomo1213 | 1:b2bd1511307e | 187 | ActPos -= 0x1000000;//16進数でのマイナス処理 16^7 = 16,777,216 = 0x1000000 |
ngokystk | 0:b4b94eb28093 | 188 | //printf("check\r\n"); |
Tomo1213 | 1:b2bd1511307e | 189 | } |
tks1 | 4:e1aa0b348aeb | 190 | u1.start(); |
tks1 | 4:e1aa0b348aeb | 191 | u2.start(); |
tks1 | 4:e1aa0b348aeb | 192 | |
tks1 | 4:e1aa0b348aeb | 193 | dist1 = u1.get_dist_cm(); |
tks1 | 4:e1aa0b348aeb | 194 | dist2 = u2.get_dist_cm(); |
tks1 | 4:e1aa0b348aeb | 195 | |
Tomo1213 | 1:b2bd1511307e | 196 | printf("dsit1 %d\r\n",dist1); |
Tomo1213 | 1:b2bd1511307e | 197 | /*--------------------------*/ |
Tomo1213 | 1:b2bd1511307e | 198 | //前方向に入力があった時 |
tks1 | 4:e1aa0b348aeb | 199 | if(0){//ActPos < Init_Pos - KICK){ |
ngokystk | 0:b4b94eb28093 | 200 | |
Tomo1213 | 1:b2bd1511307e | 201 | set_ACC(ACC_B); |
Tomo1213 | 1:b2bd1511307e | 202 | set_DEC(ACC_C); |
Tomo1213 | 1:b2bd1511307e | 203 | |
Tomo1213 | 1:b2bd1511307e | 204 | //速度制御モード送信 |
Tomo1213 | 1:b2bd1511307e | 205 | set_MODE_V(); |
ngokystk | 0:b4b94eb28093 | 206 | |
Tomo1213 | 1:b2bd1511307e | 207 | while(1){ |
ngokystk | 0:b4b94eb28093 | 208 | |
Tomo1213 | 1:b2bd1511307e | 209 | u1.start(); |
Tomo1213 | 1:b2bd1511307e | 210 | u2.start(); |
Tomo1213 | 1:b2bd1511307e | 211 | dist1 = u1.get_dist_cm(); |
Tomo1213 | 1:b2bd1511307e | 212 | dist2 = u2.get_dist_cm(); |
ngokystk | 0:b4b94eb28093 | 213 | |
Tomo1213 | 1:b2bd1511307e | 214 | //速度を指定 |
Tomo1213 | 3:c39c14cfc811 | 215 | sendTgtVel(1,rpm*(-1)); |
Tomo1213 | 3:c39c14cfc811 | 216 | sendTgtVel(2,rpm*(-1)); |
Tomo1213 | 3:c39c14cfc811 | 217 | sendTgtVel(3,rpm); |
Tomo1213 | 3:c39c14cfc811 | 218 | sendTgtVel(4,rpm); |
Tomo1213 | 1:b2bd1511307e | 219 | //指令値を送信 |
Tomo1213 | 1:b2bd1511307e | 220 | for(int i=1;i<= 4;i++){ |
Tomo1213 | 1:b2bd1511307e | 221 | sendCtrlEN(i); |
Tomo1213 | 1:b2bd1511307e | 222 | } |
ngokystk | 0:b4b94eb28093 | 223 | |
Tomo1213 | 1:b2bd1511307e | 224 | if(dist1 < 100 && dist1 >= 70){ |
Tomo1213 | 1:b2bd1511307e | 225 | break; |
Tomo1213 | 1:b2bd1511307e | 226 | } |
Tomo1213 | 1:b2bd1511307e | 227 | printf("dsit1 %d\r\n",dist1); |
Tomo1213 | 1:b2bd1511307e | 228 | } |
ngokystk | 0:b4b94eb28093 | 229 | |
Tomo1213 | 1:b2bd1511307e | 230 | //速度を指定 |
Tomo1213 | 1:b2bd1511307e | 231 | sendTgtVel(1,0); |
Tomo1213 | 1:b2bd1511307e | 232 | sendTgtVel(2,0); |
Tomo1213 | 1:b2bd1511307e | 233 | sendTgtVel(3,0); |
Tomo1213 | 1:b2bd1511307e | 234 | sendTgtVel(4,0); |
Tomo1213 | 1:b2bd1511307e | 235 | //指令値を送信 |
Tomo1213 | 1:b2bd1511307e | 236 | for(int i=1;i<= 4;i++){ |
Tomo1213 | 1:b2bd1511307e | 237 | sendCtrlEN(i); |
Tomo1213 | 1:b2bd1511307e | 238 | } |
Tomo1213 | 1:b2bd1511307e | 239 | //実行時間 |
Tomo1213 | 1:b2bd1511307e | 240 | printf("stop!!\r\n"); |
Tomo1213 | 1:b2bd1511307e | 241 | wait(5.0); |
Tomo1213 | 1:b2bd1511307e | 242 | |
Tomo1213 | 1:b2bd1511307e | 243 | //printf("2\r\n"); |
Tomo1213 | 1:b2bd1511307e | 244 | |
Tomo1213 | 1:b2bd1511307e | 245 | //速度を指定 |
Tomo1213 | 1:b2bd1511307e | 246 | |
Tomo1213 | 1:b2bd1511307e | 247 | while(1){ |
Tomo1213 | 1:b2bd1511307e | 248 | readActPos(1); |
Tomo1213 | 1:b2bd1511307e | 249 | ActPos=canmsgRx.data[4]+canmsgRx.data[5]*256+canmsgRx.data[6]*65536; |
Tomo1213 | 1:b2bd1511307e | 250 | if(ActPos > 8388608){ |
Tomo1213 | 1:b2bd1511307e | 251 | ActPos -= 0x1000000;//16進数でのマイナス処理 16^7 = 16,777,216 = 0x1000000 |
Tomo1213 | 1:b2bd1511307e | 252 | //printf("check\r\n"); |
Tomo1213 | 1:b2bd1511307e | 253 | } |
ngokystk | 0:b4b94eb28093 | 254 | |
Tomo1213 | 1:b2bd1511307e | 255 | if(ActPos > -10000){ |
Tomo1213 | 1:b2bd1511307e | 256 | break; |
Tomo1213 | 1:b2bd1511307e | 257 | } |
Tomo1213 | 3:c39c14cfc811 | 258 | |
Tomo1213 | 3:c39c14cfc811 | 259 | sendTgtVel(1,rpm); |
Tomo1213 | 3:c39c14cfc811 | 260 | sendTgtVel(2,rpm); |
Tomo1213 | 3:c39c14cfc811 | 261 | sendTgtVel(3,rpm*(-1)); |
Tomo1213 | 3:c39c14cfc811 | 262 | sendTgtVel(4,rpm*(-1)); |
Tomo1213 | 1:b2bd1511307e | 263 | //指令値を送信 |
Tomo1213 | 1:b2bd1511307e | 264 | for(int i=1;i<= 4;i++){ |
Tomo1213 | 1:b2bd1511307e | 265 | sendCtrlEN(i); |
Tomo1213 | 1:b2bd1511307e | 266 | } |
Tomo1213 | 1:b2bd1511307e | 267 | //printf("3\r\n"); |
Tomo1213 | 1:b2bd1511307e | 268 | } |
Tomo1213 | 1:b2bd1511307e | 269 | |
Tomo1213 | 1:b2bd1511307e | 270 | //ブレーキ |
Tomo1213 | 3:c39c14cfc811 | 271 | sendTgtVel(1,0); |
Tomo1213 | 3:c39c14cfc811 | 272 | sendTgtVel(2,0); |
Tomo1213 | 3:c39c14cfc811 | 273 | sendTgtVel(3,0); |
Tomo1213 | 3:c39c14cfc811 | 274 | sendTgtVel(4,0); |
Tomo1213 | 1:b2bd1511307e | 275 | for(int i=1;i<= 4;i++){ |
Tomo1213 | 1:b2bd1511307e | 276 | sendCtrlEN(i); |
Tomo1213 | 1:b2bd1511307e | 277 | } |
ngokystk | 0:b4b94eb28093 | 278 | |
Tomo1213 | 1:b2bd1511307e | 279 | wait(0.5); |
Tomo1213 | 1:b2bd1511307e | 280 | |
Tomo1213 | 1:b2bd1511307e | 281 | set_MODE_T(); |
ngokystk | 0:b4b94eb28093 | 282 | |
Tomo1213 | 1:b2bd1511307e | 283 | dist1 = 0; |
Tomo1213 | 1:b2bd1511307e | 284 | |
Tomo1213 | 1:b2bd1511307e | 285 | wait(3.0); |
Tomo1213 | 1:b2bd1511307e | 286 | break; |
Tomo1213 | 1:b2bd1511307e | 287 | } |
ngokystk | 0:b4b94eb28093 | 288 | |
ngokystk | 0:b4b94eb28093 | 289 | /*--------------------------*/ |
ngokystk | 0:b4b94eb28093 | 290 | |
ngokystk | 0:b4b94eb28093 | 291 | /*--------------------------*/ |
ngokystk | 0:b4b94eb28093 | 292 | //右方向に入力があった時 |
Tomo1213 | 1:b2bd1511307e | 293 | |
ngokystk | 0:b4b94eb28093 | 294 | } |
tks1 | 4:e1aa0b348aeb | 295 | wait(0.5); |
ngokystk | 0:b4b94eb28093 | 296 | } |
Tomo1213 | 1:b2bd1511307e | 297 | } |
Tomo1213 | 1:b2bd1511307e | 298 | |
Tomo1213 | 1:b2bd1511307e | 299 | void set_ACC(int setACC_val){ |
Tomo1213 | 1:b2bd1511307e | 300 | sendProAcc(1,setACC_val); |
Tomo1213 | 1:b2bd1511307e | 301 | sendProAcc(2,setACC_val); |
Tomo1213 | 1:b2bd1511307e | 302 | sendProAcc(3,setACC_val); |
Tomo1213 | 1:b2bd1511307e | 303 | sendProAcc(4,setACC_val); |
ngokystk | 0:b4b94eb28093 | 304 | } |
ngokystk | 0:b4b94eb28093 | 305 | |
Tomo1213 | 1:b2bd1511307e | 306 | void set_DEC(int setDEC_val){ |
Tomo1213 | 1:b2bd1511307e | 307 | sendProDec(1,setDEC_val); |
Tomo1213 | 1:b2bd1511307e | 308 | sendProDec(2,setDEC_val); |
Tomo1213 | 1:b2bd1511307e | 309 | sendProDec(3,setDEC_val); |
Tomo1213 | 1:b2bd1511307e | 310 | sendProDec(4,setDEC_val); |
Tomo1213 | 1:b2bd1511307e | 311 | } |
ngokystk | 0:b4b94eb28093 | 312 | |
Tomo1213 | 1:b2bd1511307e | 313 | void set_MODE_V(){ |
Tomo1213 | 1:b2bd1511307e | 314 | sendOPModeV(1); |
Tomo1213 | 1:b2bd1511307e | 315 | sendOPModeV(2); |
Tomo1213 | 1:b2bd1511307e | 316 | sendOPModeV(3); |
Tomo1213 | 1:b2bd1511307e | 317 | sendOPModeV(4); |
Tomo1213 | 1:b2bd1511307e | 318 | } |
ngokystk | 0:b4b94eb28093 | 319 | |
Tomo1213 | 1:b2bd1511307e | 320 | void set_MODE_T(){ |
Tomo1213 | 1:b2bd1511307e | 321 | sendOPModeT(1); |
Tomo1213 | 1:b2bd1511307e | 322 | sendOPModeT(2); |
Tomo1213 | 1:b2bd1511307e | 323 | sendOPModeT(3); |
Tomo1213 | 1:b2bd1511307e | 324 | sendOPModeT(4); |
Tomo1213 | 1:b2bd1511307e | 325 | } |
ngokystk | 0:b4b94eb28093 | 326 | |
ngokystk | 0:b4b94eb28093 | 327 | //0x2F-6060-00-fd-//-//-// |
ngokystk | 0:b4b94eb28093 | 328 | void sendOPModeT(int nodeID){ |
ngokystk | 0:b4b94eb28093 | 329 | canmsgTx.id = 0x600+nodeID; |
ngokystk | 0:b4b94eb28093 | 330 | canmsgTx.len = 5; //Data Length |
ngokystk | 0:b4b94eb28093 | 331 | canmsgTx.data[0] = 0x2F;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| |
ngokystk | 0:b4b94eb28093 | 332 | canmsgTx.data[1] = 0x60;//Index LowByte |
ngokystk | 0:b4b94eb28093 | 333 | canmsgTx.data[2] = 0x60;//Index HighByte |
ngokystk | 0:b4b94eb28093 | 334 | canmsgTx.data[3] = 0x00;//sub-Index |
ngokystk | 0:b4b94eb28093 | 335 | canmsgTx.data[4] = 0xFD;//data:fd = "current Mode" |
ngokystk | 0:b4b94eb28093 | 336 | /* |
ngokystk | 0:b4b94eb28093 | 337 | canmsgTx.data[5] = 0x00;//data:(user value) |
ngokystk | 0:b4b94eb28093 | 338 | canmsgTx.data[6] = 0x00;//data:(user value) |
ngokystk | 0:b4b94eb28093 | 339 | canmsgTx.data[7] = 0x00;//data:(user value) |
ngokystk | 0:b4b94eb28093 | 340 | */ |
ngokystk | 0:b4b94eb28093 | 341 | printCANTX(); //CAN送信データをPCに表示 |
ngokystk | 0:b4b94eb28093 | 342 | canPort.write(canmsgTx);//CANでデータ送信 |
ngokystk | 0:b4b94eb28093 | 343 | wait_ms(1); |
ngokystk | 0:b4b94eb28093 | 344 | } |
Tomo1213 | 1:b2bd1511307e | 345 | |
Tomo1213 | 3:c39c14cfc811 | 346 | |
ngokystk | 0:b4b94eb28093 | 347 | //0x2F-6060-00-03-//-//-// |
ngokystk | 0:b4b94eb28093 | 348 | void sendOPModeV(int nodeID){ |
ngokystk | 0:b4b94eb28093 | 349 | canmsgTx.id = 0x600+nodeID; |
ngokystk | 0:b4b94eb28093 | 350 | canmsgTx.len = 5; //Data Length |
ngokystk | 0:b4b94eb28093 | 351 | canmsgTx.data[0] = 0x2F;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| |
ngokystk | 0:b4b94eb28093 | 352 | canmsgTx.data[1] = 0x60;//Index LowByte |
ngokystk | 0:b4b94eb28093 | 353 | canmsgTx.data[2] = 0x60;//Index HighByte |
ngokystk | 0:b4b94eb28093 | 354 | canmsgTx.data[3] = 0x00;//sub-Index |
ngokystk | 0:b4b94eb28093 | 355 | canmsgTx.data[4] = 0x03;//data:0x03 = "Profile Velocity Mode" |
ngokystk | 0:b4b94eb28093 | 356 | /* |
ngokystk | 0:b4b94eb28093 | 357 | canmsgTx.data[5] = 0x00;//data:(user value) |
ngokystk | 0:b4b94eb28093 | 358 | canmsgTx.data[6] = 0x00;//data:(user value) |
ngokystk | 0:b4b94eb28093 | 359 | canmsgTx.data[7] = 0x00;//data:(user value) |
ngokystk | 0:b4b94eb28093 | 360 | */ |
ngokystk | 0:b4b94eb28093 | 361 | printCANTX(); //CAN送信データをPCに表示 |
ngokystk | 0:b4b94eb28093 | 362 | canPort.write(canmsgTx);//CANでデータ送信 |
ngokystk | 0:b4b94eb28093 | 363 | wait_ms(1); |
ngokystk | 0:b4b94eb28093 | 364 | } |
Tomo1213 | 1:b2bd1511307e | 365 | |
ngokystk | 0:b4b94eb28093 | 366 | //0x2B-6040-00-0000-//-// |
ngokystk | 0:b4b94eb28093 | 367 | void sendCtrlRS(int nodeID){ |
ngokystk | 0:b4b94eb28093 | 368 | canmsgTx.id = 0x600+nodeID; |
ngokystk | 0:b4b94eb28093 | 369 | canmsgTx.len = 6; //Data Length |
ngokystk | 0:b4b94eb28093 | 370 | canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| |
ngokystk | 0:b4b94eb28093 | 371 | canmsgTx.data[1] = 0x40;//Index LowByte |
ngokystk | 0:b4b94eb28093 | 372 | canmsgTx.data[2] = 0x60;//Index HighByte |
ngokystk | 0:b4b94eb28093 | 373 | canmsgTx.data[3] = 0x00;//sub-Index |
ngokystk | 0:b4b94eb28093 | 374 | canmsgTx.data[4] = 0x80;//data:0x00"80" = "Controlword(Shutdown)" |
ngokystk | 0:b4b94eb28093 | 375 | canmsgTx.data[5] = 0x00;//data:0x"00"80 |
ngokystk | 0:b4b94eb28093 | 376 | /* |
ngokystk | 0:b4b94eb28093 | 377 | canmsgTx.data[6] = 0x00;//data:(user value) |
ngokystk | 0:b4b94eb28093 | 378 | canmsgTx.data[7] = 0x00;//data:(user value) |
ngokystk | 0:b4b94eb28093 | 379 | */ |
ngokystk | 0:b4b94eb28093 | 380 | printCANTX(); //CAN送信データをPCに表示 |
ngokystk | 0:b4b94eb28093 | 381 | canPort.write(canmsgTx);//CANでデータ送信 |
ngokystk | 0:b4b94eb28093 | 382 | wait_ms(1); |
ngokystk | 0:b4b94eb28093 | 383 | } |
Tomo1213 | 1:b2bd1511307e | 384 | |
Tomo1213 | 1:b2bd1511307e | 385 | |
ngokystk | 0:b4b94eb28093 | 386 | //0x2B-6040-00-0006-//-// |
ngokystk | 0:b4b94eb28093 | 387 | void sendCtrlSD(int nodeID){ |
ngokystk | 0:b4b94eb28093 | 388 | canmsgTx.id = 0x600+nodeID; |
ngokystk | 0:b4b94eb28093 | 389 | canmsgTx.len = 6; //Data Length |
ngokystk | 0:b4b94eb28093 | 390 | canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| |
ngokystk | 0:b4b94eb28093 | 391 | canmsgTx.data[1] = 0x40;//Index LowByte |
ngokystk | 0:b4b94eb28093 | 392 | canmsgTx.data[2] = 0x60;//Index HighByte |
ngokystk | 0:b4b94eb28093 | 393 | canmsgTx.data[3] = 0x00;//sub-Index |
ngokystk | 0:b4b94eb28093 | 394 | canmsgTx.data[4] = 0x06;//data:0x00"06" = "Controlword(Shutdown)" |
ngokystk | 0:b4b94eb28093 | 395 | canmsgTx.data[5] = 0x00;//data:0x"00"06 |
ngokystk | 0:b4b94eb28093 | 396 | /* |
ngokystk | 0:b4b94eb28093 | 397 | canmsgTx.data[6] = 0x00;//data:(user value) |
ngokystk | 0:b4b94eb28093 | 398 | canmsgTx.data[7] = 0x00;//data:(user value) |
ngokystk | 0:b4b94eb28093 | 399 | */ |
ngokystk | 0:b4b94eb28093 | 400 | printCANTX(); //CAN送信データをPCに表示 |
ngokystk | 0:b4b94eb28093 | 401 | canPort.write(canmsgTx);//CANでデータ送信 |
ngokystk | 0:b4b94eb28093 | 402 | wait_ms(1); |
ngokystk | 0:b4b94eb28093 | 403 | } |
Tomo1213 | 1:b2bd1511307e | 404 | |
ngokystk | 0:b4b94eb28093 | 405 | //0x2B-6040-00-000F-//-// |
ngokystk | 0:b4b94eb28093 | 406 | void sendCtrlEN(int nodeID){ |
ngokystk | 0:b4b94eb28093 | 407 | canmsgTx.id = 0x600+nodeID; |
ngokystk | 0:b4b94eb28093 | 408 | canmsgTx.len = 6; //Data Length |
ngokystk | 0:b4b94eb28093 | 409 | canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| |
ngokystk | 0:b4b94eb28093 | 410 | canmsgTx.data[1] = 0x40;//Index LowByte |
ngokystk | 0:b4b94eb28093 | 411 | canmsgTx.data[2] = 0x60;//Index HighByte |
ngokystk | 0:b4b94eb28093 | 412 | canmsgTx.data[3] = 0x00;//sub-Index |
ngokystk | 0:b4b94eb28093 | 413 | canmsgTx.data[4] = 0x0F;//data:0x00"0F" = "Controlword(Enable)" |
ngokystk | 0:b4b94eb28093 | 414 | canmsgTx.data[5] = 0x00;//data:0x"00"0F |
ngokystk | 0:b4b94eb28093 | 415 | /* |
ngokystk | 0:b4b94eb28093 | 416 | canmsgTx.data[6] = 0x00;//data:(user value) |
ngokystk | 0:b4b94eb28093 | 417 | canmsgTx.data[7] = 0x00;//data:(user value) |
ngokystk | 0:b4b94eb28093 | 418 | */ |
ngokystk | 0:b4b94eb28093 | 419 | //printCANTX(); //CAN送信データをPCに表示 |
ngokystk | 0:b4b94eb28093 | 420 | canPort.write(canmsgTx);//CANでデータ送信 |
ngokystk | 0:b4b94eb28093 | 421 | wait_ms(1); |
ngokystk | 0:b4b94eb28093 | 422 | } |
Tomo1213 | 1:b2bd1511307e | 423 | |
ngokystk | 0:b4b94eb28093 | 424 | //0x2B-6040-00-000B-//-// |
ngokystk | 0:b4b94eb28093 | 425 | void sendCtrlQS(int nodeID){ |
ngokystk | 0:b4b94eb28093 | 426 | canmsgTx.id = 0x600+nodeID; |
ngokystk | 0:b4b94eb28093 | 427 | canmsgTx.len = 6; //Data Length |
ngokystk | 0:b4b94eb28093 | 428 | canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| |
ngokystk | 0:b4b94eb28093 | 429 | canmsgTx.data[1] = 0x40;//Index LowByte |
ngokystk | 0:b4b94eb28093 | 430 | canmsgTx.data[2] = 0x60;//Index HighByte |
ngokystk | 0:b4b94eb28093 | 431 | canmsgTx.data[3] = 0x00;//sub-Index |
ngokystk | 0:b4b94eb28093 | 432 | canmsgTx.data[4] = 0x0B;//data:0x00"0B" = "Quick Stop" |
ngokystk | 0:b4b94eb28093 | 433 | canmsgTx.data[5] = 0x00;//data:0x"00"0B |
ngokystk | 0:b4b94eb28093 | 434 | /* |
ngokystk | 0:b4b94eb28093 | 435 | canmsgTx.data[6] = 0x00;//data:(user value) |
ngokystk | 0:b4b94eb28093 | 436 | canmsgTx.data[7] = 0x00;//data:(user value) |
ngokystk | 0:b4b94eb28093 | 437 | */ |
ngokystk | 0:b4b94eb28093 | 438 | printCANTX(); //CAN送信データをPCに表示 |
ngokystk | 0:b4b94eb28093 | 439 | canPort.write(canmsgTx);//CANでデータ送信 |
ngokystk | 0:b4b94eb28093 | 440 | wait_ms(1); |
ngokystk | 0:b4b94eb28093 | 441 | } |
Tomo1213 | 1:b2bd1511307e | 442 | |
ngokystk | 0:b4b94eb28093 | 443 | //0x2B-6040-00-010F-//-// |
ngokystk | 0:b4b94eb28093 | 444 | void sendCtrlHL(int nodeID){ |
ngokystk | 0:b4b94eb28093 | 445 | canmsgTx.id = 0x600+nodeID; |
ngokystk | 0:b4b94eb28093 | 446 | canmsgTx.len = 6; //Data Length |
ngokystk | 0:b4b94eb28093 | 447 | canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| |
ngokystk | 0:b4b94eb28093 | 448 | canmsgTx.data[1] = 0x40;//Index LowByte |
ngokystk | 0:b4b94eb28093 | 449 | canmsgTx.data[2] = 0x60;//Index HighByte |
ngokystk | 0:b4b94eb28093 | 450 | canmsgTx.data[3] = 0x00;//sub-Index |
ngokystk | 0:b4b94eb28093 | 451 | canmsgTx.data[4] = 0x0F;//data:0x01"0F" = "Halt" |
ngokystk | 0:b4b94eb28093 | 452 | canmsgTx.data[5] = 0x01;//data:0x"01"0F |
ngokystk | 0:b4b94eb28093 | 453 | /* |
ngokystk | 0:b4b94eb28093 | 454 | canmsgTx.data[6] = 0x00;//data:(user value) |
ngokystk | 0:b4b94eb28093 | 455 | canmsgTx.data[7] = 0x00;//data:(user value) |
ngokystk | 0:b4b94eb28093 | 456 | */ |
ngokystk | 0:b4b94eb28093 | 457 | printCANTX(); //CAN送信データをPCに表示 |
ngokystk | 0:b4b94eb28093 | 458 | canPort.write(canmsgTx);//CANでデータ送信 |
ngokystk | 0:b4b94eb28093 | 459 | wait_ms(1); |
ngokystk | 0:b4b94eb28093 | 460 | } |
Tomo1213 | 1:b2bd1511307e | 461 | |
ngokystk | 0:b4b94eb28093 | 462 | //0x2B-60FF-00-[user data(4Byte)] |
ngokystk | 0:b4b94eb28093 | 463 | void sendTgtTrq(int nodeID,int trq){ |
ngokystk | 0:b4b94eb28093 | 464 | //pc.printf("%dmA0x%08x\r\n",trq,trq); //回転数送信データの表示 |
ngokystk | 0:b4b94eb28093 | 465 | canmsgTx.id = 0x600+nodeID; |
ngokystk | 0:b4b94eb28093 | 466 | canmsgTx.len = 6; //Data Length |
ngokystk | 0:b4b94eb28093 | 467 | canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| |
ngokystk | 0:b4b94eb28093 | 468 | canmsgTx.data[1] = 0x30;//Index LowByte 71 |
ngokystk | 0:b4b94eb28093 | 469 | canmsgTx.data[2] = 0x20;//Index HighByte 60 |
ngokystk | 0:b4b94eb28093 | 470 | canmsgTx.data[3] = 0x00;//sub-Index |
ngokystk | 0:b4b94eb28093 | 471 | //下位から1Byteずつdataに格納 |
ngokystk | 0:b4b94eb28093 | 472 | if(trq<0){ |
Tomo1213 | 3:c39c14cfc811 | 473 | trq=0xFFFF+trq+1; |
ngokystk | 0:b4b94eb28093 | 474 | } |
ngokystk | 0:b4b94eb28093 | 475 | |
ngokystk | 0:b4b94eb28093 | 476 | //pc.printf("iii%d\r\n",trq); |
ngokystk | 0:b4b94eb28093 | 477 | //canmsgTx.data[7]=((trq>>24)&0xFF); |
ngokystk | 0:b4b94eb28093 | 478 | //canmsgTx.data[6]=((trq>>16)&0xFF); |
ngokystk | 0:b4b94eb28093 | 479 | |
ngokystk | 0:b4b94eb28093 | 480 | canmsgTx.data[5]=((trq>>8)&0xFF); |
ngokystk | 0:b4b94eb28093 | 481 | canmsgTx.data[4]=((trq>>0)&0xFF); |
ngokystk | 0:b4b94eb28093 | 482 | //printCANTX(); //CAN送信データをPCに表示 |
ngokystk | 0:b4b94eb28093 | 483 | canPort.write(canmsgTx);//CANでデータ送信 |
ngokystk | 0:b4b94eb28093 | 484 | wait_ms(2); |
ngokystk | 0:b4b94eb28093 | 485 | //send Enable |
ngokystk | 0:b4b94eb28093 | 486 | //pc.printf("Send Enable Command\r\n"); |
ngokystk | 0:b4b94eb28093 | 487 | //sendCtrlEN(nodeID); |
ngokystk | 0:b4b94eb28093 | 488 | //wait(0.5); |
ngokystk | 0:b4b94eb28093 | 489 | } |
Tomo1213 | 1:b2bd1511307e | 490 | |
Tomo1213 | 3:c39c14cfc811 | 491 | |
Tomo1213 | 3:c39c14cfc811 | 492 | |
Tomo1213 | 3:c39c14cfc811 | 493 | |
ngokystk | 0:b4b94eb28093 | 494 | //0x2B-60FF-00-[user data(4Byte)] |
ngokystk | 0:b4b94eb28093 | 495 | void sendTgtVel(int nodeID,int rpm){ |
ngokystk | 0:b4b94eb28093 | 496 | //pc.printf("%drpm|0x%08x\r\n",rpm,rpm); //回転数送信データの表示 |
ngokystk | 0:b4b94eb28093 | 497 | canmsgTx.id = 0x600+nodeID; |
ngokystk | 0:b4b94eb28093 | 498 | canmsgTx.len = 8; //Data Length |
ngokystk | 0:b4b94eb28093 | 499 | canmsgTx.data[0] = 0x23;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| |
ngokystk | 0:b4b94eb28093 | 500 | canmsgTx.data[1] = 0xFF;//Index LowByte |
ngokystk | 0:b4b94eb28093 | 501 | canmsgTx.data[2] = 0x60;//Index HighByte |
ngokystk | 0:b4b94eb28093 | 502 | canmsgTx.data[3] = 0x00;//sub-Index |
ngokystk | 0:b4b94eb28093 | 503 | //下位から1Byteずつdataに格納 |
ngokystk | 0:b4b94eb28093 | 504 | |
ngokystk | 0:b4b94eb28093 | 505 | //pc.printf("%d\r\n",rpm); |
ngokystk | 0:b4b94eb28093 | 506 | if(rpm<0){ |
Tomo1213 | 3:c39c14cfc811 | 507 | rpm=0xFFFFFFFF+rpm+1; |
ngokystk | 0:b4b94eb28093 | 508 | } |
ngokystk | 0:b4b94eb28093 | 509 | canmsgTx.data[7]=((rpm>>24)&0xFF); |
ngokystk | 0:b4b94eb28093 | 510 | canmsgTx.data[6]=((rpm>>16)&0xFF); |
ngokystk | 0:b4b94eb28093 | 511 | canmsgTx.data[5]=((rpm>>8)&0xFF); |
ngokystk | 0:b4b94eb28093 | 512 | canmsgTx.data[4]=((rpm>>0)&0xFF); |
ngokystk | 0:b4b94eb28093 | 513 | |
ngokystk | 0:b4b94eb28093 | 514 | //printCANTX(); //CAN送信データをPCに表示 |
ngokystk | 0:b4b94eb28093 | 515 | canPort.write(canmsgTx);//CANでデータ送信 |
ngokystk | 0:b4b94eb28093 | 516 | wait_ms(1); |
ngokystk | 0:b4b94eb28093 | 517 | |
ngokystk | 0:b4b94eb28093 | 518 | } |
Tomo1213 | 3:c39c14cfc811 | 519 | |
ngokystk | 0:b4b94eb28093 | 520 | void readActVel(int nodeID){ |
ngokystk | 0:b4b94eb28093 | 521 | //値が欲しいobjectのアドレスを送る |
ngokystk | 0:b4b94eb28093 | 522 | canmsgTx.id = 0x600+nodeID; |
ngokystk | 0:b4b94eb28093 | 523 | canmsgTx.len = 4; //Data Length |
ngokystk | 0:b4b94eb28093 | 524 | canmsgTx.data[0] = 0x40;//|0Byte:40| |
ngokystk | 0:b4b94eb28093 | 525 | canmsgTx.data[1] = 0x6C;//Index LowByte |
ngokystk | 0:b4b94eb28093 | 526 | canmsgTx.data[2] = 0x60;//Index HighByte |
ngokystk | 0:b4b94eb28093 | 527 | canmsgTx.data[3] = 0x00;//sub-Index |
ngokystk | 0:b4b94eb28093 | 528 | canPort.write(canmsgTx); |
ngokystk | 0:b4b94eb28093 | 529 | wait_ms(1); |
ngokystk | 0:b4b94eb28093 | 530 | } |
Tomo1213 | 1:b2bd1511307e | 531 | |
ngokystk | 0:b4b94eb28093 | 532 | void readActPos(int nodeID){ |
ngokystk | 0:b4b94eb28093 | 533 | //値が欲しいobjectのアドレスを送る |
ngokystk | 0:b4b94eb28093 | 534 | canmsgTx.id = 0x600+nodeID; |
ngokystk | 0:b4b94eb28093 | 535 | canmsgTx.len = 4; //Data Length |
ngokystk | 0:b4b94eb28093 | 536 | canmsgTx.data[0] = 0x40;//|0Byte:40| |
ngokystk | 0:b4b94eb28093 | 537 | canmsgTx.data[1] = 0x64;//Index LowByte |
ngokystk | 0:b4b94eb28093 | 538 | canmsgTx.data[2] = 0x60;//Index HighByte |
ngokystk | 0:b4b94eb28093 | 539 | canmsgTx.data[3] = 0x00;//sub-Index |
ngokystk | 0:b4b94eb28093 | 540 | canPort.write(canmsgTx); |
ngokystk | 0:b4b94eb28093 | 541 | wait_ms(1); |
ngokystk | 0:b4b94eb28093 | 542 | } |
Tomo1213 | 1:b2bd1511307e | 543 | |
ngokystk | 0:b4b94eb28093 | 544 | //加速度指定 |
ngokystk | 0:b4b94eb28093 | 545 | void sendProAcc(int nodeID,int rpm){ |
ngokystk | 0:b4b94eb28093 | 546 | if(rpm < 0){ |
ngokystk | 0:b4b94eb28093 | 547 | rpm += 0xFFFFFFFF; |
ngokystk | 0:b4b94eb28093 | 548 | } |
ngokystk | 0:b4b94eb28093 | 549 | // pc.printf("%drpm|0x%08x\r\n",rpm,rpm); //回転数送信データの表示 |
ngokystk | 0:b4b94eb28093 | 550 | canmsgTx.id = 0x600+nodeID; |
ngokystk | 0:b4b94eb28093 | 551 | canmsgTx.len = 8; //Data Length |
ngokystk | 0:b4b94eb28093 | 552 | canmsgTx.data[0] = 0x23;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| |
ngokystk | 0:b4b94eb28093 | 553 | canmsgTx.data[1] = 0x83;//Index LowByte |
ngokystk | 0:b4b94eb28093 | 554 | canmsgTx.data[2] = 0x60;//Index HighByte |
ngokystk | 0:b4b94eb28093 | 555 | canmsgTx.data[3] = 0x00;//sub-Index |
ngokystk | 0:b4b94eb28093 | 556 | //下位から1Byteずつdataに格納 |
ngokystk | 0:b4b94eb28093 | 557 | canmsgTx.data[4] = (rpm >> 0) & 0xFF; |
ngokystk | 0:b4b94eb28093 | 558 | canmsgTx.data[5] = (rpm >> 8) & 0xFF; |
ngokystk | 0:b4b94eb28093 | 559 | canmsgTx.data[6] = (rpm >> 16) & 0xFF; |
ngokystk | 0:b4b94eb28093 | 560 | canmsgTx.data[7] = (rpm >> 24) & 0xFF; |
ngokystk | 0:b4b94eb28093 | 561 | // printCANTX(); //CAN送信データをPCに表示 |
ngokystk | 0:b4b94eb28093 | 562 | canPort.write(canmsgTx);//CANでデータ送信 |
ngokystk | 0:b4b94eb28093 | 563 | wait(0.01); |
ngokystk | 0:b4b94eb28093 | 564 | //send Enable |
ngokystk | 0:b4b94eb28093 | 565 | // pc.printf("Send Enable Command\r\n"); |
ngokystk | 0:b4b94eb28093 | 566 | sendCtrlEN(nodeID); |
ngokystk | 0:b4b94eb28093 | 567 | wait(0.01); |
ngokystk | 0:b4b94eb28093 | 568 | } |
Tomo1213 | 1:b2bd1511307e | 569 | |
ngokystk | 0:b4b94eb28093 | 570 | //減速度指定 |
ngokystk | 0:b4b94eb28093 | 571 | void sendProDec(int nodeID,int rpm){ |
ngokystk | 0:b4b94eb28093 | 572 | if(rpm < 0){ |
ngokystk | 0:b4b94eb28093 | 573 | rpm += 0xFFFFFFFF; |
Tomo1213 | 3:c39c14cfc811 | 574 | } |
ngokystk | 0:b4b94eb28093 | 575 | // pc.printf("%drpm|0x%08x\r\n",rpm,rpm); //回転数送信データの表示 |
ngokystk | 0:b4b94eb28093 | 576 | canmsgTx.id = 0x600+nodeID; |
ngokystk | 0:b4b94eb28093 | 577 | canmsgTx.len = 8; //Data Length |
ngokystk | 0:b4b94eb28093 | 578 | canmsgTx.data[0] = 0x23;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| |
ngokystk | 0:b4b94eb28093 | 579 | canmsgTx.data[1] = 0x84;//Index LowByte |
ngokystk | 0:b4b94eb28093 | 580 | canmsgTx.data[2] = 0x60;//Index HighByte |
ngokystk | 0:b4b94eb28093 | 581 | canmsgTx.data[3] = 0x00;//sub-Index |
ngokystk | 0:b4b94eb28093 | 582 | //下位から1Byteずつdataに格納 |
ngokystk | 0:b4b94eb28093 | 583 | canmsgTx.data[4] = (rpm >> 0) & 0xFF; |
ngokystk | 0:b4b94eb28093 | 584 | canmsgTx.data[5] = (rpm >> 8) & 0xFF; |
ngokystk | 0:b4b94eb28093 | 585 | canmsgTx.data[6] = (rpm >> 16) & 0xFF; |
ngokystk | 0:b4b94eb28093 | 586 | canmsgTx.data[7] = (rpm >> 24) & 0xFF; |
ngokystk | 0:b4b94eb28093 | 587 | // printCANTX(); //CAN送信データをPCに表示 |
ngokystk | 0:b4b94eb28093 | 588 | canPort.write(canmsgTx);//CANでデータ送信 |
ngokystk | 0:b4b94eb28093 | 589 | wait(0.01); |
ngokystk | 0:b4b94eb28093 | 590 | //send Enable |
ngokystk | 0:b4b94eb28093 | 591 | // pc.printf("Send Enable Command\r\n"); |
ngokystk | 0:b4b94eb28093 | 592 | sendCtrlEN(nodeID); |
ngokystk | 0:b4b94eb28093 | 593 | wait(0.01); |
ngokystk | 0:b4b94eb28093 | 594 | } |
Tomo1213 | 1:b2bd1511307e | 595 | |
Tomo1213 | 1:b2bd1511307e | 596 | |
ngokystk | 0:b4b94eb28093 | 597 | //送信データの表示 |
ngokystk | 0:b4b94eb28093 | 598 | void printCANTX(void){ |
ngokystk | 0:b4b94eb28093 | 599 | //0x canID|Byte0|Byte1|Byte2|Byte3|Byte4|Byte5|Byte6|Byte7| |
ngokystk | 0:b4b94eb28093 | 600 | pc.printf("0x%3x|",canmsgTx.id); |
ngokystk | 0:b4b94eb28093 | 601 | for(char i=0;i < canmsgTx.len;i++){ |
ngokystk | 0:b4b94eb28093 | 602 | pc.printf("%02x|",canmsgTx.data[i]); |
ngokystk | 0:b4b94eb28093 | 603 | } |
ngokystk | 0:b4b94eb28093 | 604 | //pc.printf("\r\n"); |
ngokystk | 0:b4b94eb28093 | 605 | } |
Tomo1213 | 1:b2bd1511307e | 606 | |
ngokystk | 0:b4b94eb28093 | 607 | //受信データの表示 |
Tomo1213 | 1:b2bd1511307e | 608 | |
ngokystk | 0:b4b94eb28093 | 609 | void printCANRX(void){ |
ngokystk | 0:b4b94eb28093 | 610 | //0x canID|Byte0|Byte1|Byte2|Byte3|Byte4|Byte5|Byte6|Byte7| |
ngokystk | 0:b4b94eb28093 | 611 | //pc.printf("0x%3x|",canmsgRx.id); |
ngokystk | 0:b4b94eb28093 | 612 | for(char i=0;i < canmsgRx.len;i++){ |
ngokystk | 0:b4b94eb28093 | 613 | //pc.printf("%02x|",canmsgRx.data[i]); |
ngokystk | 0:b4b94eb28093 | 614 | } |
ngokystk | 0:b4b94eb28093 | 615 | pc.printf("\r\n"); |
ngokystk | 0:b4b94eb28093 | 616 | } |
Tomo1213 | 1:b2bd1511307e | 617 | |
ngokystk | 0:b4b94eb28093 | 618 | void CANdataRX(void){ |
ngokystk | 0:b4b94eb28093 | 619 | canPort.read(canmsgRx); |
ngokystk | 0:b4b94eb28093 | 620 | printCANRX(); |
ngokystk | 0:b4b94eb28093 | 621 | } |
Tomo1213 | 1:b2bd1511307e | 622 | |
ngokystk | 0:b4b94eb28093 | 623 | void SerialRX(void){ |
ngokystk | 0:b4b94eb28093 | 624 | Serialdata = pc.getc(); |
ngokystk | 0:b4b94eb28093 | 625 | //pc.printf("%c\r\n",Serialdata); |
ngokystk | 0:b4b94eb28093 | 626 | } |
Tomo1213 | 1:b2bd1511307e | 627 | |
Tomo1213 | 1:b2bd1511307e | 628 |