Tomohiro Aoki / Mbed 2 deprecated YUKA_ak_yn

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed HCSR04

main.cpp@10:49d153e666c7, 2021-12-20 (annotated)

Committer:
Tomo1213
Date:
Mon Dec 20 11:05:13 2021 +0000
Revision:
10:49d153e666c7
Parent:
9:51325cc6496a 
a

Who changed what in which revision?

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