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