Kanta Takasu / Mbed 2 deprecated YUKA_FIN

gohome ok

Dependencies:   mbed HCSR04

main.cpp@10:91d4b6117470, 2021-12-21 (annotated)

Committer:
tks1
Date:
Tue Dec 21 06:40:21 2021 +0000
Revision:
10:91d4b6117470
Parent:
9:71b780f8d345
Child:
11:d87a09eb77b9
gohome

Who changed what in which revision?

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