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