Kanta Takasu / Mbed 2 deprecated YUKA_tks

yuka_tks

Dependencies:   mbed HCSR04

yuka.h@6:dd35cc3b5ca0, 2021-12-20 (annotated)

Committer:
tks1
Date:
Mon Dec 20 15:49:55 2021 +0000
Revision:
6:dd35cc3b5ca0
yuka_tks

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tks1 6:dd35cc3b5ca0 1 #include "mbed.h"
tks1 6:dd35cc3b5ca0 2 #include "hcsr04.h"
tks1 6:dd35cc3b5ca0 3
tks1 6:dd35cc3b5ca0 4 #define RPM_RIDE 400
tks1 6:dd35cc3b5ca0 5 #define RPM_CLEAN 400
tks1 6:dd35cc3b5ca0 6 #define ACC_RIDE 1000
tks1 6:dd35cc3b5ca0 7 #define DEC_RIDE 700
tks1 6:dd35cc3b5ca0 8 #define ACC_CLEAN 1000
tks1 6:dd35cc3b5ca0 9 #define DEC_CLEAN 1000
tks1 6:dd35cc3b5ca0 10
tks1 6:dd35cc3b5ca0 11 #define KICK 2000
tks1 6:dd35cc3b5ca0 12 #define CLEAN_OFFSET 6000
tks1 6:dd35cc3b5ca0 13
tks1 6:dd35cc3b5ca0 14 #define WALL 45
tks1 6:dd35cc3b5ca0 15 #define WALL_MIN 25
tks1 6:dd35cc3b5ca0 16 #define Standby_Time 10
tks1 6:dd35cc3b5ca0 17 #define GETOFF_TIME 3
tks1 6:dd35cc3b5ca0 18
tks1 6:dd35cc3b5ca0 19
tks1 6:dd35cc3b5ca0 20 //プロトタイプ宣言
tks1 6:dd35cc3b5ca0 21 //------------------send関数-------------------
tks1 6:dd35cc3b5ca0 22 //mode Setting
tks1 6:dd35cc3b5ca0 23 void sendOPModeT(int); //Operating Mode
tks1 6:dd35cc3b5ca0 24 void sendOPModeV(int); //Operating Mode
tks1 6:dd35cc3b5ca0 25 //Control Word
tks1 6:dd35cc3b5ca0 26 void sendCtrlRS(int); //Reset
tks1 6:dd35cc3b5ca0 27 void sendCtrlSD(int); //Shutdown
tks1 6:dd35cc3b5ca0 28 void sendCtrlEN(int); //Switch on & Enable
tks1 6:dd35cc3b5ca0 29 void sendCtrlQS(int); //Quick Stop
tks1 6:dd35cc3b5ca0 30 void sendCtrlHL(int); //Halt
tks1 6:dd35cc3b5ca0 31 //Velocity Setting
tks1 6:dd35cc3b5ca0 32 void sendTgtVel(int,int); //Target Velocity
tks1 6:dd35cc3b5ca0 33 //Torque Setting
tks1 6:dd35cc3b5ca0 34 void sendTgtTrq(int,int); //Target Torque
tks1 6:dd35cc3b5ca0 35 //Acceleration Setting
tks1 6:dd35cc3b5ca0 36 void sendProAcc(int,int); //Plof Acceleration
tks1 6:dd35cc3b5ca0 37 void sendProDec(int,int); //Plof Deceleration
tks1 6:dd35cc3b5ca0 38 //------------------read関数-------------------
tks1 6:dd35cc3b5ca0 39 void readActVel(int); //Actual Velocity
tks1 6:dd35cc3b5ca0 40 void readActPos(int); //Actual Position
tks1 6:dd35cc3b5ca0 41 //-------------------その他--------------------
tks1 6:dd35cc3b5ca0 42 void printCANTX(void); //CAN送信データをPCに表示
tks1 6:dd35cc3b5ca0 43 void printCANRX(void); //CAN受信データをPCに表示
tks1 6:dd35cc3b5ca0 44 void CANdataRX(void); //CAN受信処理
tks1 6:dd35cc3b5ca0 45 void SerialRX(void); //Serial受信処理
tks1 6:dd35cc3b5ca0 46 //---------------------------------------------
tks1 6:dd35cc3b5ca0 47 void set_ACC(int);
tks1 6:dd35cc3b5ca0 48 void set_DEC(int);
tks1 6:dd35cc3b5ca0 49 void set_MODE_V(void);
tks1 6:dd35cc3b5ca0 50 void set_MODE_T(void);
tks1 6:dd35cc3b5ca0 51
tks1 6:dd35cc3b5ca0 52 void vel_stop(void);
tks1 6:dd35cc3b5ca0 53
tks1 6:dd35cc3b5ca0 54 void vel_forward(int);
tks1 6:dd35cc3b5ca0 55 void vel_forward_con(int);
tks1 6:dd35cc3b5ca0 56 void vel_backward(int);
tks1 6:dd35cc3b5ca0 57 void vel_backward_con(int);
tks1 6:dd35cc3b5ca0 58 void vel_right(int);
tks1 6:dd35cc3b5ca0 59 void vel_right_con(int);
tks1 6:dd35cc3b5ca0 60 void vel_left(int);
tks1 6:dd35cc3b5ca0 61
tks1 6:dd35cc3b5ca0 62
tks1 6:dd35cc3b5ca0 63 Serial pc(USBTX, USBRX);
tks1 6:dd35cc3b5ca0 64 Serial LED(PB_6,PB_7);
tks1 6:dd35cc3b5ca0 65 char Serialdata;
tks1 6:dd35cc3b5ca0 66 BusOut myled(LED1, LED2, LED3, LED4);
tks1 6:dd35cc3b5ca0 67
tks1 6:dd35cc3b5ca0 68 //HCSR04 u1(p13, p14), u2(p11, p12), u3(p23, p24), u4(p25, p26); // Trigger(DO), Echo(PWMIN); LPC1768
tks1 6:dd35cc3b5ca0 69 HCSR04 u1(PA_0, PA_1),u2(PA_5, PA_6),u3(PB_0, PA_10); // Trigger(DO), Echo(PWMIN); f303k8
tks1 6:dd35cc3b5ca0 70
tks1 6:dd35cc3b5ca0 71 CANMessage canmsgTx;
tks1 6:dd35cc3b5ca0 72 CANMessage canmsgRx;
tks1 6:dd35cc3b5ca0 73 //CAN canPort(p30, p29); //CAN name(PinName rd, PinName td) LPC1768
tks1 6:dd35cc3b5ca0 74 CAN canPort(PA_11, PA_12); //CAN name(PinName rd, PinName td) F303k8
tks1 6:dd35cc3b5ca0 75
tks1 6:dd35cc3b5ca0 76 int nodeall=4;
tks1 6:dd35cc3b5ca0 77
tks1 6:dd35cc3b5ca0 78 DigitalOut led1(LED1);
tks1 6:dd35cc3b5ca0 79 DigitalOut led2(LED2);
tks1 6:dd35cc3b5ca0 80 DigitalOut led3(LED3);
tks1 6:dd35cc3b5ca0 81 DigitalOut led4(LED4);
tks1 6:dd35cc3b5ca0 82
tks1 6:dd35cc3b5ca0 83
tks1 6:dd35cc3b5ca0 84
tks1 6:dd35cc3b5ca0 85 //unsigned int get_cm_n(HCSR04, unsigned int);
tks1 6:dd35cc3b5ca0 86 //USE -> unsigned int dist_UnitA = get_cm_n(u2, 5);
tks1 6:dd35cc3b5ca0 87 unsigned int get_cm_n(HCSR04 &echo_unit,int echo_n){
tks1 6:dd35cc3b5ca0 88 unsigned int sampled_dist=0;
tks1 6:dd35cc3b5ca0 89 for (int iter_n = 0; iter_n <echo_n; iter_n++){
tks1 6:dd35cc3b5ca0 90 echo_unit.start();
tks1 6:dd35cc3b5ca0 91 sampled_dist += echo_unit.get_dist_cm();
tks1 6:dd35cc3b5ca0 92 }
tks1 6:dd35cc3b5ca0 93 return (sampled_dist / echo_n);
tks1 6:dd35cc3b5ca0 94 }
tks1 6:dd35cc3b5ca0 95
tks1 6:dd35cc3b5ca0 96
tks1 6:dd35cc3b5ca0 97 void set_ACC(int setACC_val){
tks1 6:dd35cc3b5ca0 98 sendProAcc(1,setACC_val);
tks1 6:dd35cc3b5ca0 99 sendProAcc(2,setACC_val);
tks1 6:dd35cc3b5ca0 100 sendProAcc(3,setACC_val);
tks1 6:dd35cc3b5ca0 101 sendProAcc(4,setACC_val);
tks1 6:dd35cc3b5ca0 102 }
tks1 6:dd35cc3b5ca0 103
tks1 6:dd35cc3b5ca0 104 void set_DEC(int setDEC_val){
tks1 6:dd35cc3b5ca0 105 sendProDec(1,setDEC_val);
tks1 6:dd35cc3b5ca0 106 sendProDec(2,setDEC_val);
tks1 6:dd35cc3b5ca0 107 sendProDec(3,setDEC_val);
tks1 6:dd35cc3b5ca0 108 sendProDec(4,setDEC_val);
tks1 6:dd35cc3b5ca0 109 }
tks1 6:dd35cc3b5ca0 110
tks1 6:dd35cc3b5ca0 111 void set_MODE_V(){
tks1 6:dd35cc3b5ca0 112 sendOPModeV(1);
tks1 6:dd35cc3b5ca0 113 sendOPModeV(2);
tks1 6:dd35cc3b5ca0 114 sendOPModeV(3);
tks1 6:dd35cc3b5ca0 115 sendOPModeV(4);
tks1 6:dd35cc3b5ca0 116 }
tks1 6:dd35cc3b5ca0 117
tks1 6:dd35cc3b5ca0 118 void set_MODE_T(){
tks1 6:dd35cc3b5ca0 119 sendOPModeT(1);
tks1 6:dd35cc3b5ca0 120 sendOPModeT(2);
tks1 6:dd35cc3b5ca0 121 sendOPModeT(3);
tks1 6:dd35cc3b5ca0 122 sendOPModeT(4);
tks1 6:dd35cc3b5ca0 123 }
tks1 6:dd35cc3b5ca0 124
tks1 6:dd35cc3b5ca0 125 //0x2F-6060-00-fd-//-//-//
tks1 6:dd35cc3b5ca0 126 void sendOPModeT(int nodeID){
tks1 6:dd35cc3b5ca0 127 canmsgTx.id = 0x600+nodeID;
tks1 6:dd35cc3b5ca0 128 canmsgTx.len = 5; //Data Length
tks1 6:dd35cc3b5ca0 129 canmsgTx.data[0] = 0x2F;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
tks1 6:dd35cc3b5ca0 130 canmsgTx.data[1] = 0x60;//Index LowByte
tks1 6:dd35cc3b5ca0 131 canmsgTx.data[2] = 0x60;//Index HighByte
tks1 6:dd35cc3b5ca0 132 canmsgTx.data[3] = 0x00;//sub-Index
tks1 6:dd35cc3b5ca0 133 canmsgTx.data[4] = 0xFD;//data:fd = "current Mode"
tks1 6:dd35cc3b5ca0 134 /*
tks1 6:dd35cc3b5ca0 135 canmsgTx.data[5] = 0x00;//data:(user value)
tks1 6:dd35cc3b5ca0 136 canmsgTx.data[6] = 0x00;//data:(user value)
tks1 6:dd35cc3b5ca0 137 canmsgTx.data[7] = 0x00;//data:(user value)
tks1 6:dd35cc3b5ca0 138 */
tks1 6:dd35cc3b5ca0 139 printCANTX(); //CAN送信データをPCに表示
tks1 6:dd35cc3b5ca0 140 canPort.write(canmsgTx);//CANでデータ送信
tks1 6:dd35cc3b5ca0 141 wait_ms(1);
tks1 6:dd35cc3b5ca0 142 }
tks1 6:dd35cc3b5ca0 143
tks1 6:dd35cc3b5ca0 144
tks1 6:dd35cc3b5ca0 145 //0x2F-6060-00-03-//-//-//
tks1 6:dd35cc3b5ca0 146 void sendOPModeV(int nodeID){
tks1 6:dd35cc3b5ca0 147 canmsgTx.id = 0x600+nodeID;
tks1 6:dd35cc3b5ca0 148 canmsgTx.len = 5; //Data Length
tks1 6:dd35cc3b5ca0 149 canmsgTx.data[0] = 0x2F;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
tks1 6:dd35cc3b5ca0 150 canmsgTx.data[1] = 0x60;//Index LowByte
tks1 6:dd35cc3b5ca0 151 canmsgTx.data[2] = 0x60;//Index HighByte
tks1 6:dd35cc3b5ca0 152 canmsgTx.data[3] = 0x00;//sub-Index
tks1 6:dd35cc3b5ca0 153 canmsgTx.data[4] = 0x03;//data:0x03 = "Profile Velocity Mode"
tks1 6:dd35cc3b5ca0 154 /*
tks1 6:dd35cc3b5ca0 155 canmsgTx.data[5] = 0x00;//data:(user value)
tks1 6:dd35cc3b5ca0 156 canmsgTx.data[6] = 0x00;//data:(user value)
tks1 6:dd35cc3b5ca0 157 canmsgTx.data[7] = 0x00;//data:(user value)
tks1 6:dd35cc3b5ca0 158 */
tks1 6:dd35cc3b5ca0 159 printCANTX(); //CAN送信データをPCに表示
tks1 6:dd35cc3b5ca0 160 canPort.write(canmsgTx);//CANでデータ送信
tks1 6:dd35cc3b5ca0 161 wait_ms(1);
tks1 6:dd35cc3b5ca0 162 }
tks1 6:dd35cc3b5ca0 163
tks1 6:dd35cc3b5ca0 164 //0x2B-6040-00-0000-//-//
tks1 6:dd35cc3b5ca0 165 void sendCtrlRS(int nodeID){
tks1 6:dd35cc3b5ca0 166 canmsgTx.id = 0x600+nodeID;
tks1 6:dd35cc3b5ca0 167 canmsgTx.len = 6; //Data Length
tks1 6:dd35cc3b5ca0 168 canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
tks1 6:dd35cc3b5ca0 169 canmsgTx.data[1] = 0x40;//Index LowByte
tks1 6:dd35cc3b5ca0 170 canmsgTx.data[2] = 0x60;//Index HighByte
tks1 6:dd35cc3b5ca0 171 canmsgTx.data[3] = 0x00;//sub-Index
tks1 6:dd35cc3b5ca0 172 canmsgTx.data[4] = 0x80;//data:0x00"80" = "Controlword(Shutdown)"
tks1 6:dd35cc3b5ca0 173 canmsgTx.data[5] = 0x00;//data:0x"00"80
tks1 6:dd35cc3b5ca0 174 /*
tks1 6:dd35cc3b5ca0 175 canmsgTx.data[6] = 0x00;//data:(user value)
tks1 6:dd35cc3b5ca0 176 canmsgTx.data[7] = 0x00;//data:(user value)
tks1 6:dd35cc3b5ca0 177 */
tks1 6:dd35cc3b5ca0 178 printCANTX(); //CAN送信データをPCに表示
tks1 6:dd35cc3b5ca0 179 canPort.write(canmsgTx);//CANでデータ送信
tks1 6:dd35cc3b5ca0 180 wait_ms(1);
tks1 6:dd35cc3b5ca0 181 }
tks1 6:dd35cc3b5ca0 182
tks1 6:dd35cc3b5ca0 183
tks1 6:dd35cc3b5ca0 184 //0x2B-6040-00-0006-//-//
tks1 6:dd35cc3b5ca0 185 void sendCtrlSD(int nodeID){
tks1 6:dd35cc3b5ca0 186 canmsgTx.id = 0x600+nodeID;
tks1 6:dd35cc3b5ca0 187 canmsgTx.len = 6; //Data Length
tks1 6:dd35cc3b5ca0 188 canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
tks1 6:dd35cc3b5ca0 189 canmsgTx.data[1] = 0x40;//Index LowByte
tks1 6:dd35cc3b5ca0 190 canmsgTx.data[2] = 0x60;//Index HighByte
tks1 6:dd35cc3b5ca0 191 canmsgTx.data[3] = 0x00;//sub-Index
tks1 6:dd35cc3b5ca0 192 canmsgTx.data[4] = 0x06;//data:0x00"06" = "Controlword(Shutdown)"
tks1 6:dd35cc3b5ca0 193 canmsgTx.data[5] = 0x00;//data:0x"00"06
tks1 6:dd35cc3b5ca0 194 /*
tks1 6:dd35cc3b5ca0 195 canmsgTx.data[6] = 0x00;//data:(user value)
tks1 6:dd35cc3b5ca0 196 canmsgTx.data[7] = 0x00;//data:(user value)
tks1 6:dd35cc3b5ca0 197 */
tks1 6:dd35cc3b5ca0 198 printCANTX(); //CAN送信データをPCに表示
tks1 6:dd35cc3b5ca0 199 canPort.write(canmsgTx);//CANでデータ送信
tks1 6:dd35cc3b5ca0 200 wait_ms(1);
tks1 6:dd35cc3b5ca0 201 }
tks1 6:dd35cc3b5ca0 202
tks1 6:dd35cc3b5ca0 203 //0x2B-6040-00-000F-//-//
tks1 6:dd35cc3b5ca0 204 void sendCtrlEN(int nodeID){
tks1 6:dd35cc3b5ca0 205 canmsgTx.id = 0x600+nodeID;
tks1 6:dd35cc3b5ca0 206 canmsgTx.len = 6; //Data Length
tks1 6:dd35cc3b5ca0 207 canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
tks1 6:dd35cc3b5ca0 208 canmsgTx.data[1] = 0x40;//Index LowByte
tks1 6:dd35cc3b5ca0 209 canmsgTx.data[2] = 0x60;//Index HighByte
tks1 6:dd35cc3b5ca0 210 canmsgTx.data[3] = 0x00;//sub-Index
tks1 6:dd35cc3b5ca0 211 canmsgTx.data[4] = 0x0F;//data:0x00"0F" = "Controlword(Enable)"
tks1 6:dd35cc3b5ca0 212 canmsgTx.data[5] = 0x00;//data:0x"00"0F
tks1 6:dd35cc3b5ca0 213 /*
tks1 6:dd35cc3b5ca0 214 canmsgTx.data[6] = 0x00;//data:(user value)
tks1 6:dd35cc3b5ca0 215 canmsgTx.data[7] = 0x00;//data:(user value)
tks1 6:dd35cc3b5ca0 216 */
tks1 6:dd35cc3b5ca0 217 //printCANTX(); //CAN送信データをPCに表示
tks1 6:dd35cc3b5ca0 218 canPort.write(canmsgTx);//CANでデータ送信
tks1 6:dd35cc3b5ca0 219 wait_ms(1);
tks1 6:dd35cc3b5ca0 220 }
tks1 6:dd35cc3b5ca0 221
tks1 6:dd35cc3b5ca0 222 //0x2B-6040-00-000B-//-//
tks1 6:dd35cc3b5ca0 223 void sendCtrlQS(int nodeID){
tks1 6:dd35cc3b5ca0 224 canmsgTx.id = 0x600+nodeID;
tks1 6:dd35cc3b5ca0 225 canmsgTx.len = 6; //Data Length
tks1 6:dd35cc3b5ca0 226 canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
tks1 6:dd35cc3b5ca0 227 canmsgTx.data[1] = 0x40;//Index LowByte
tks1 6:dd35cc3b5ca0 228 canmsgTx.data[2] = 0x60;//Index HighByte
tks1 6:dd35cc3b5ca0 229 canmsgTx.data[3] = 0x00;//sub-Index
tks1 6:dd35cc3b5ca0 230 canmsgTx.data[4] = 0x0B;//data:0x00"0B" = "Quick Stop"
tks1 6:dd35cc3b5ca0 231 canmsgTx.data[5] = 0x00;//data:0x"00"0B
tks1 6:dd35cc3b5ca0 232 /*
tks1 6:dd35cc3b5ca0 233 canmsgTx.data[6] = 0x00;//data:(user value)
tks1 6:dd35cc3b5ca0 234 canmsgTx.data[7] = 0x00;//data:(user value)
tks1 6:dd35cc3b5ca0 235 */
tks1 6:dd35cc3b5ca0 236 printCANTX(); //CAN送信データをPCに表示
tks1 6:dd35cc3b5ca0 237 canPort.write(canmsgTx);//CANでデータ送信
tks1 6:dd35cc3b5ca0 238 wait_ms(1);
tks1 6:dd35cc3b5ca0 239 }
tks1 6:dd35cc3b5ca0 240
tks1 6:dd35cc3b5ca0 241 //0x2B-6040-00-010F-//-//
tks1 6:dd35cc3b5ca0 242 void sendCtrlHL(int nodeID){
tks1 6:dd35cc3b5ca0 243 canmsgTx.id = 0x600+nodeID;
tks1 6:dd35cc3b5ca0 244 canmsgTx.len = 6; //Data Length
tks1 6:dd35cc3b5ca0 245 canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
tks1 6:dd35cc3b5ca0 246 canmsgTx.data[1] = 0x40;//Index LowByte
tks1 6:dd35cc3b5ca0 247 canmsgTx.data[2] = 0x60;//Index HighByte
tks1 6:dd35cc3b5ca0 248 canmsgTx.data[3] = 0x00;//sub-Index
tks1 6:dd35cc3b5ca0 249 canmsgTx.data[4] = 0x0F;//data:0x01"0F" = "Halt"
tks1 6:dd35cc3b5ca0 250 canmsgTx.data[5] = 0x01;//data:0x"01"0F
tks1 6:dd35cc3b5ca0 251 /*
tks1 6:dd35cc3b5ca0 252 canmsgTx.data[6] = 0x00;//data:(user value)
tks1 6:dd35cc3b5ca0 253 canmsgTx.data[7] = 0x00;//data:(user value)
tks1 6:dd35cc3b5ca0 254 */
tks1 6:dd35cc3b5ca0 255 printCANTX(); //CAN送信データをPCに表示
tks1 6:dd35cc3b5ca0 256 canPort.write(canmsgTx);//CANでデータ送信
tks1 6:dd35cc3b5ca0 257 wait_ms(1);
tks1 6:dd35cc3b5ca0 258 }
tks1 6:dd35cc3b5ca0 259
tks1 6:dd35cc3b5ca0 260 //0x2B-60FF-00-[user data(4Byte)]
tks1 6:dd35cc3b5ca0 261 void sendTgtTrq(int nodeID,int trq){
tks1 6:dd35cc3b5ca0 262 //pc.printf("%dmA0x%08x\r\n",trq,trq); //回転数送信データの表示
tks1 6:dd35cc3b5ca0 263 canmsgTx.id = 0x600+nodeID;
tks1 6:dd35cc3b5ca0 264 canmsgTx.len = 6; //Data Length
tks1 6:dd35cc3b5ca0 265 canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
tks1 6:dd35cc3b5ca0 266 canmsgTx.data[1] = 0x30;//Index LowByte 71
tks1 6:dd35cc3b5ca0 267 canmsgTx.data[2] = 0x20;//Index HighByte 60
tks1 6:dd35cc3b5ca0 268 canmsgTx.data[3] = 0x00;//sub-Index
tks1 6:dd35cc3b5ca0 269 //下位から1Byteずつdataに格納
tks1 6:dd35cc3b5ca0 270 if(trq<0){
tks1 6:dd35cc3b5ca0 271 trq=0xFFFF+trq+1;
tks1 6:dd35cc3b5ca0 272 }
tks1 6:dd35cc3b5ca0 273
tks1 6:dd35cc3b5ca0 274 //pc.printf("iii%d\r\n",trq);
tks1 6:dd35cc3b5ca0 275 //canmsgTx.data[7]=((trq>>24)&0xFF);
tks1 6:dd35cc3b5ca0 276 //canmsgTx.data[6]=((trq>>16)&0xFF);
tks1 6:dd35cc3b5ca0 277
tks1 6:dd35cc3b5ca0 278 canmsgTx.data[5]=((trq>>8)&0xFF);
tks1 6:dd35cc3b5ca0 279 canmsgTx.data[4]=((trq>>0)&0xFF);
tks1 6:dd35cc3b5ca0 280 //printCANTX(); //CAN送信データをPCに表示
tks1 6:dd35cc3b5ca0 281 canPort.write(canmsgTx);//CANでデータ送信
tks1 6:dd35cc3b5ca0 282 wait_ms(2);
tks1 6:dd35cc3b5ca0 283 //send Enable
tks1 6:dd35cc3b5ca0 284 //pc.printf("Send Enable Command\r\n");
tks1 6:dd35cc3b5ca0 285 //sendCtrlEN(nodeID);
tks1 6:dd35cc3b5ca0 286 //wait(0.5);
tks1 6:dd35cc3b5ca0 287 }
tks1 6:dd35cc3b5ca0 288
tks1 6:dd35cc3b5ca0 289
tks1 6:dd35cc3b5ca0 290
tks1 6:dd35cc3b5ca0 291
tks1 6:dd35cc3b5ca0 292 //0x2B-60FF-00-[user data(4Byte)]
tks1 6:dd35cc3b5ca0 293 void sendTgtVel(int nodeID,int rpm){
tks1 6:dd35cc3b5ca0 294 //pc.printf("%drpm|0x%08x\r\n",rpm,rpm); //回転数送信データの表示
tks1 6:dd35cc3b5ca0 295 canmsgTx.id = 0x600+nodeID;
tks1 6:dd35cc3b5ca0 296 canmsgTx.len = 8; //Data Length
tks1 6:dd35cc3b5ca0 297 canmsgTx.data[0] = 0x23;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
tks1 6:dd35cc3b5ca0 298 canmsgTx.data[1] = 0xFF;//Index LowByte
tks1 6:dd35cc3b5ca0 299 canmsgTx.data[2] = 0x60;//Index HighByte
tks1 6:dd35cc3b5ca0 300 canmsgTx.data[3] = 0x00;//sub-Index
tks1 6:dd35cc3b5ca0 301 //下位から1Byteずつdataに格納
tks1 6:dd35cc3b5ca0 302
tks1 6:dd35cc3b5ca0 303 //pc.printf("%d\r\n",rpm);
tks1 6:dd35cc3b5ca0 304 if(rpm<0){
tks1 6:dd35cc3b5ca0 305 rpm=0xFFFFFFFF+rpm+1;
tks1 6:dd35cc3b5ca0 306 }
tks1 6:dd35cc3b5ca0 307 canmsgTx.data[7]=((rpm>>24)&0xFF);
tks1 6:dd35cc3b5ca0 308 canmsgTx.data[6]=((rpm>>16)&0xFF);
tks1 6:dd35cc3b5ca0 309 canmsgTx.data[5]=((rpm>>8)&0xFF);
tks1 6:dd35cc3b5ca0 310 canmsgTx.data[4]=((rpm>>0)&0xFF);
tks1 6:dd35cc3b5ca0 311
tks1 6:dd35cc3b5ca0 312 //printCANTX(); //CAN送信データをPCに表示
tks1 6:dd35cc3b5ca0 313 canPort.write(canmsgTx);//CANでデータ送信
tks1 6:dd35cc3b5ca0 314 wait_ms(1);
tks1 6:dd35cc3b5ca0 315
tks1 6:dd35cc3b5ca0 316 }
tks1 6:dd35cc3b5ca0 317
tks1 6:dd35cc3b5ca0 318 void readActVel(int nodeID){
tks1 6:dd35cc3b5ca0 319 //値が欲しいobjectのアドレスを送る
tks1 6:dd35cc3b5ca0 320 canmsgTx.id = 0x600+nodeID;
tks1 6:dd35cc3b5ca0 321 canmsgTx.len = 4; //Data Length
tks1 6:dd35cc3b5ca0 322 canmsgTx.data[0] = 0x40;//|0Byte:40|
tks1 6:dd35cc3b5ca0 323 canmsgTx.data[1] = 0x6C;//Index LowByte
tks1 6:dd35cc3b5ca0 324 canmsgTx.data[2] = 0x60;//Index HighByte
tks1 6:dd35cc3b5ca0 325 canmsgTx.data[3] = 0x00;//sub-Index
tks1 6:dd35cc3b5ca0 326 canPort.write(canmsgTx);
tks1 6:dd35cc3b5ca0 327 wait_ms(1);
tks1 6:dd35cc3b5ca0 328 }
tks1 6:dd35cc3b5ca0 329
tks1 6:dd35cc3b5ca0 330 void readActPos(int nodeID){
tks1 6:dd35cc3b5ca0 331 //値が欲しいobjectのアドレスを送る
tks1 6:dd35cc3b5ca0 332 canmsgTx.id = 0x600+nodeID;
tks1 6:dd35cc3b5ca0 333 canmsgTx.len = 4; //Data Length
tks1 6:dd35cc3b5ca0 334 canmsgTx.data[0] = 0x40;//|0Byte:40|
tks1 6:dd35cc3b5ca0 335 canmsgTx.data[1] = 0x64;//Index LowByte
tks1 6:dd35cc3b5ca0 336 canmsgTx.data[2] = 0x60;//Index HighByte
tks1 6:dd35cc3b5ca0 337 canmsgTx.data[3] = 0x00;//sub-Index
tks1 6:dd35cc3b5ca0 338 canPort.write(canmsgTx);
tks1 6:dd35cc3b5ca0 339 wait_ms(1);
tks1 6:dd35cc3b5ca0 340 }
tks1 6:dd35cc3b5ca0 341
tks1 6:dd35cc3b5ca0 342 //加速度指定
tks1 6:dd35cc3b5ca0 343 void sendProAcc(int nodeID,int rpm){
tks1 6:dd35cc3b5ca0 344 if(rpm < 0){
tks1 6:dd35cc3b5ca0 345 rpm += 0xFFFFFFFF;
tks1 6:dd35cc3b5ca0 346 }
tks1 6:dd35cc3b5ca0 347 // pc.printf("%drpm|0x%08x\r\n",rpm,rpm); //回転数送信データの表示
tks1 6:dd35cc3b5ca0 348 canmsgTx.id = 0x600+nodeID;
tks1 6:dd35cc3b5ca0 349 canmsgTx.len = 8; //Data Length
tks1 6:dd35cc3b5ca0 350 canmsgTx.data[0] = 0x23;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
tks1 6:dd35cc3b5ca0 351 canmsgTx.data[1] = 0x83;//Index LowByte
tks1 6:dd35cc3b5ca0 352 canmsgTx.data[2] = 0x60;//Index HighByte
tks1 6:dd35cc3b5ca0 353 canmsgTx.data[3] = 0x00;//sub-Index
tks1 6:dd35cc3b5ca0 354 //下位から1Byteずつdataに格納
tks1 6:dd35cc3b5ca0 355 canmsgTx.data[4] = (rpm >> 0) & 0xFF;
tks1 6:dd35cc3b5ca0 356 canmsgTx.data[5] = (rpm >> 8) & 0xFF;
tks1 6:dd35cc3b5ca0 357 canmsgTx.data[6] = (rpm >> 16) & 0xFF;
tks1 6:dd35cc3b5ca0 358 canmsgTx.data[7] = (rpm >> 24) & 0xFF;
tks1 6:dd35cc3b5ca0 359 // printCANTX(); //CAN送信データをPCに表示
tks1 6:dd35cc3b5ca0 360 canPort.write(canmsgTx);//CANでデータ送信
tks1 6:dd35cc3b5ca0 361 wait(0.01);
tks1 6:dd35cc3b5ca0 362 //send Enable
tks1 6:dd35cc3b5ca0 363 // pc.printf("Send Enable Command\r\n");
tks1 6:dd35cc3b5ca0 364 sendCtrlEN(nodeID);
tks1 6:dd35cc3b5ca0 365 wait(0.01);
tks1 6:dd35cc3b5ca0 366 }
tks1 6:dd35cc3b5ca0 367
tks1 6:dd35cc3b5ca0 368 //減速度指定
tks1 6:dd35cc3b5ca0 369 void sendProDec(int nodeID,int rpm){
tks1 6:dd35cc3b5ca0 370 if(rpm < 0){
tks1 6:dd35cc3b5ca0 371 rpm += 0xFFFFFFFF;
tks1 6:dd35cc3b5ca0 372 }
tks1 6:dd35cc3b5ca0 373 // pc.printf("%drpm|0x%08x\r\n",rpm,rpm); //回転数送信データの表示
tks1 6:dd35cc3b5ca0 374 canmsgTx.id = 0x600+nodeID;
tks1 6:dd35cc3b5ca0 375 canmsgTx.len = 8; //Data Length
tks1 6:dd35cc3b5ca0 376 canmsgTx.data[0] = 0x23;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22|
tks1 6:dd35cc3b5ca0 377 canmsgTx.data[1] = 0x84;//Index LowByte
tks1 6:dd35cc3b5ca0 378 canmsgTx.data[2] = 0x60;//Index HighByte
tks1 6:dd35cc3b5ca0 379 canmsgTx.data[3] = 0x00;//sub-Index
tks1 6:dd35cc3b5ca0 380 //下位から1Byteずつdataに格納
tks1 6:dd35cc3b5ca0 381 canmsgTx.data[4] = (rpm >> 0) & 0xFF;
tks1 6:dd35cc3b5ca0 382 canmsgTx.data[5] = (rpm >> 8) & 0xFF;
tks1 6:dd35cc3b5ca0 383 canmsgTx.data[6] = (rpm >> 16) & 0xFF;
tks1 6:dd35cc3b5ca0 384 canmsgTx.data[7] = (rpm >> 24) & 0xFF;
tks1 6:dd35cc3b5ca0 385 // printCANTX(); //CAN送信データをPCに表示
tks1 6:dd35cc3b5ca0 386 canPort.write(canmsgTx);//CANでデータ送信
tks1 6:dd35cc3b5ca0 387 wait(0.01);
tks1 6:dd35cc3b5ca0 388 //send Enable
tks1 6:dd35cc3b5ca0 389 // pc.printf("Send Enable Command\r\n");
tks1 6:dd35cc3b5ca0 390 sendCtrlEN(nodeID);
tks1 6:dd35cc3b5ca0 391 wait(0.01);
tks1 6:dd35cc3b5ca0 392 }
tks1 6:dd35cc3b5ca0 393
tks1 6:dd35cc3b5ca0 394
tks1 6:dd35cc3b5ca0 395 //送信データの表示
tks1 6:dd35cc3b5ca0 396 void printCANTX(void){
tks1 6:dd35cc3b5ca0 397 //0x canID|Byte0|Byte1|Byte2|Byte3|Byte4|Byte5|Byte6|Byte7|
tks1 6:dd35cc3b5ca0 398 //pc.printf("0x%3x|",canmsgTx.id);
tks1 6:dd35cc3b5ca0 399 for(char i=0;i < canmsgTx.len;i++){
tks1 6:dd35cc3b5ca0 400 //pc.printf("%02x|",canmsgTx.data[i]);
tks1 6:dd35cc3b5ca0 401 }
tks1 6:dd35cc3b5ca0 402 //pc.printf("\r\n");
tks1 6:dd35cc3b5ca0 403 }
tks1 6:dd35cc3b5ca0 404
tks1 6:dd35cc3b5ca0 405 //受信データの表示
tks1 6:dd35cc3b5ca0 406
tks1 6:dd35cc3b5ca0 407 void printCANRX(void){
tks1 6:dd35cc3b5ca0 408 //0x canID|Byte0|Byte1|Byte2|Byte3|Byte4|Byte5|Byte6|Byte7|
tks1 6:dd35cc3b5ca0 409 //pc.printf("0x%3x|",canmsgRx.id);
tks1 6:dd35cc3b5ca0 410 for(char i=0;i < canmsgRx.len;i++){
tks1 6:dd35cc3b5ca0 411 //pc.printf("%02x|",canmsgRx.data[i]);
tks1 6:dd35cc3b5ca0 412 }
tks1 6:dd35cc3b5ca0 413 //pc.printf("\r\n");
tks1 6:dd35cc3b5ca0 414 }
tks1 6:dd35cc3b5ca0 415
tks1 6:dd35cc3b5ca0 416 void CANdataRX(void){
tks1 6:dd35cc3b5ca0 417 canPort.read(canmsgRx);
tks1 6:dd35cc3b5ca0 418 printCANRX();
tks1 6:dd35cc3b5ca0 419 }
tks1 6:dd35cc3b5ca0 420
tks1 6:dd35cc3b5ca0 421 void SerialRX(void){
tks1 6:dd35cc3b5ca0 422 Serialdata = pc.getc();
tks1 6:dd35cc3b5ca0 423 //pc.printf("%c\r\n",Serialdata);
tks1 6:dd35cc3b5ca0 424 }
tks1 6:dd35cc3b5ca0 425
tks1 6:dd35cc3b5ca0 426
tks1 6:dd35cc3b5ca0 427 void vel_right(int rpm){
tks1 6:dd35cc3b5ca0 428 sendTgtVel(1,rpm);
tks1 6:dd35cc3b5ca0 429 sendTgtVel(2,rpm*(-1));
tks1 6:dd35cc3b5ca0 430 sendTgtVel(3,rpm*(-1));
tks1 6:dd35cc3b5ca0 431 sendTgtVel(4,rpm);
tks1 6:dd35cc3b5ca0 432 for(int i=1;i<= 4;i++){
tks1 6:dd35cc3b5ca0 433 sendCtrlEN(i);
tks1 6:dd35cc3b5ca0 434 }
tks1 6:dd35cc3b5ca0 435 }
tks1 6:dd35cc3b5ca0 436 void vel_right_con(int rpmA){
tks1 6:dd35cc3b5ca0 437 int dis1 = get_cm_n(u1,5);
tks1 6:dd35cc3b5ca0 438 int dis2 = get_cm_n(u2,5);
tks1 6:dd35cc3b5ca0 439
tks1 6:dd35cc3b5ca0 440 //速度を指定
tks1 6:dd35cc3b5ca0 441 int robot_angle = ((dis1 - dis2)*5);
tks1 6:dd35cc3b5ca0 442 sendTgtVel(1,rpmA+robot_angle);
tks1 6:dd35cc3b5ca0 443 sendTgtVel(2,rpmA*(-1)+robot_angle);
tks1 6:dd35cc3b5ca0 444 sendTgtVel(3,rpmA*(-1)+robot_angle);
tks1 6:dd35cc3b5ca0 445 sendTgtVel(4,rpmA+robot_angle);
tks1 6:dd35cc3b5ca0 446 //指令値を送信
tks1 6:dd35cc3b5ca0 447 for(int i=1;i<= 4;i++){
tks1 6:dd35cc3b5ca0 448 sendCtrlEN(i);
tks1 6:dd35cc3b5ca0 449 }
tks1 6:dd35cc3b5ca0 450 }
tks1 6:dd35cc3b5ca0 451
tks1 6:dd35cc3b5ca0 452
tks1 6:dd35cc3b5ca0 453 void vel_left(int rpm){
tks1 6:dd35cc3b5ca0 454 sendTgtVel(1,rpm*(-1));
tks1 6:dd35cc3b5ca0 455 sendTgtVel(2,rpm);
tks1 6:dd35cc3b5ca0 456 sendTgtVel(3,rpm);
tks1 6:dd35cc3b5ca0 457 sendTgtVel(4,rpm*(-1));
tks1 6:dd35cc3b5ca0 458 for(int i=1;i<= 4;i++){
tks1 6:dd35cc3b5ca0 459 sendCtrlEN(i);
tks1 6:dd35cc3b5ca0 460 }
tks1 6:dd35cc3b5ca0 461 }
tks1 6:dd35cc3b5ca0 462
tks1 6:dd35cc3b5ca0 463
tks1 6:dd35cc3b5ca0 464 void vel_left_con(int rpmA){
tks1 6:dd35cc3b5ca0 465 int dis1 = get_cm_n(u1,5);
tks1 6:dd35cc3b5ca0 466 int dis2 = get_cm_n(u2,5);
tks1 6:dd35cc3b5ca0 467
tks1 6:dd35cc3b5ca0 468 //速度を指定
tks1 6:dd35cc3b5ca0 469 int robot_angle = ((dis1 - dis2)*5);
tks1 6:dd35cc3b5ca0 470 sendTgtVel(1,rpmA*(-1)+robot_angle);
tks1 6:dd35cc3b5ca0 471 sendTgtVel(2,rpmA+robot_angle);
tks1 6:dd35cc3b5ca0 472 sendTgtVel(3,rpmA+robot_angle);
tks1 6:dd35cc3b5ca0 473 sendTgtVel(4,rpmA*(-1)+robot_angle);
tks1 6:dd35cc3b5ca0 474 //指令値を送信
tks1 6:dd35cc3b5ca0 475 for(int i=1;i<= 4;i++){
tks1 6:dd35cc3b5ca0 476 sendCtrlEN(i);
tks1 6:dd35cc3b5ca0 477 }
tks1 6:dd35cc3b5ca0 478 }
tks1 6:dd35cc3b5ca0 479
tks1 6:dd35cc3b5ca0 480 void vel_stop(){
tks1 6:dd35cc3b5ca0 481 //速度を指定
tks1 6:dd35cc3b5ca0 482 sendTgtVel(1,0);
tks1 6:dd35cc3b5ca0 483 sendTgtVel(2,0);
tks1 6:dd35cc3b5ca0 484 sendTgtVel(3,0);
tks1 6:dd35cc3b5ca0 485 sendTgtVel(4,0);
tks1 6:dd35cc3b5ca0 486 //指令値を送信
tks1 6:dd35cc3b5ca0 487 for(int i=1;i<= 4;i++){
tks1 6:dd35cc3b5ca0 488 sendCtrlEN(i);
tks1 6:dd35cc3b5ca0 489 }
tks1 6:dd35cc3b5ca0 490 }
tks1 6:dd35cc3b5ca0 491
tks1 6:dd35cc3b5ca0 492 void vel_backward(int rpm){
tks1 6:dd35cc3b5ca0 493 //速度を指定
tks1 6:dd35cc3b5ca0 494 sendTgtVel(1,rpm);
tks1 6:dd35cc3b5ca0 495 sendTgtVel(2,rpm);
tks1 6:dd35cc3b5ca0 496 sendTgtVel(3,rpm*(-1));
tks1 6:dd35cc3b5ca0 497 sendTgtVel(4,rpm*(-1));
tks1 6:dd35cc3b5ca0 498 //指令値を送信
tks1 6:dd35cc3b5ca0 499 for(int i=1;i<= 4;i++){
tks1 6:dd35cc3b5ca0 500 sendCtrlEN(i);
tks1 6:dd35cc3b5ca0 501 }
tks1 6:dd35cc3b5ca0 502 }
tks1 6:dd35cc3b5ca0 503
tks1 6:dd35cc3b5ca0 504 void vel_backward_con(int rpmA){
tks1 6:dd35cc3b5ca0 505 int dis1 = get_cm_n(u1,5);
tks1 6:dd35cc3b5ca0 506 int dis2 = get_cm_n(u2,5);
tks1 6:dd35cc3b5ca0 507
tks1 6:dd35cc3b5ca0 508 //速度を指定
tks1 6:dd35cc3b5ca0 509 int robot_angle = ((dis1 - dis2)*10);
tks1 6:dd35cc3b5ca0 510 sendTgtVel(1,rpmA+robot_angle);
tks1 6:dd35cc3b5ca0 511 sendTgtVel(2,rpmA+robot_angle);
tks1 6:dd35cc3b5ca0 512 sendTgtVel(3,rpmA*(-1)+robot_angle);
tks1 6:dd35cc3b5ca0 513 sendTgtVel(4,rpmA*(-1)+robot_angle);
tks1 6:dd35cc3b5ca0 514 //指令値を送信
tks1 6:dd35cc3b5ca0 515 for(int i=1;i<= 4;i++){
tks1 6:dd35cc3b5ca0 516 sendCtrlEN(i);
tks1 6:dd35cc3b5ca0 517 }
tks1 6:dd35cc3b5ca0 518 }
tks1 6:dd35cc3b5ca0 519
tks1 6:dd35cc3b5ca0 520
tks1 6:dd35cc3b5ca0 521 void vel_forward_con(int rpmA){
tks1 6:dd35cc3b5ca0 522 int dis1 = get_cm_n(u1,5);
tks1 6:dd35cc3b5ca0 523 int dis2 = get_cm_n(u2,5);
tks1 6:dd35cc3b5ca0 524
tks1 6:dd35cc3b5ca0 525 //速度を指定
tks1 6:dd35cc3b5ca0 526 int robot_angle = ((dis1 - dis2)*10);
tks1 6:dd35cc3b5ca0 527 sendTgtVel(1,rpmA*(-1)+robot_angle);
tks1 6:dd35cc3b5ca0 528 sendTgtVel(2,rpmA*(-1)+robot_angle);
tks1 6:dd35cc3b5ca0 529 sendTgtVel(3,rpmA+robot_angle);
tks1 6:dd35cc3b5ca0 530 sendTgtVel(4,rpmA+robot_angle);
tks1 6:dd35cc3b5ca0 531 //指令値を送信
tks1 6:dd35cc3b5ca0 532 for(int i=1;i<= 4;i++){
tks1 6:dd35cc3b5ca0 533 sendCtrlEN(i);
tks1 6:dd35cc3b5ca0 534 }
tks1 6:dd35cc3b5ca0 535 }
tks1 6:dd35cc3b5ca0 536