Kanta Takasu / Mbed 2 deprecated YUKA_FIN

gohome ok

Dependencies:   mbed HCSR04

main.cpp@9:71b780f8d345, 2021-12-21 (annotated)

Committer:
Tomo1213
Date:
Tue Dec 21 00:59:40 2021 +0000
Revision:
9:71b780f8d345
Parent:
8:dd676df43fe7
Child:
10:91d4b6117470
hobokansei

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