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