Kanta Takasu
/
YUKA_tks
yuka_tks
Revision 6:dd35cc3b5ca0, committed 2021-12-20
- Comitter:
- tks1
- Date:
- Mon Dec 20 15:49:55 2021 +0000
- Parent:
- 5:f21e1a75f98b
- Commit message:
- yuka_tks
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
yuka.h | Show annotated file Show diff for this revision Revisions of this file |
diff -r f21e1a75f98b -r dd35cc3b5ca0 main.cpp --- a/main.cpp Mon Dec 20 14:13:29 2021 +0000 +++ b/main.cpp Mon Dec 20 15:49:55 2021 +0000 @@ -1,137 +1,31 @@ //2021/12/20更新 -//YUKA本番機用プログラム -//入力切替確認済み -//一定時間の入力なしで動作切替 -//ジグザグ動作実装前 -//エンコーダ読み取りによる入力 -//加減速度調整パラメータ実装 -//PID制御による機体角度補正_ -//aoki - -#include "mbed.h" -#include "hcsr04.h" - -#define RPM_RIDE 400 -#define RPM_CLEAN 400 -#define ACC_RIDE 1000 -#define DEC_RIDE 700 -#define ACC_CLEAN 1000 -#define DEC_CLEAN 1000 - -#define KICK 2000 -#define CLEAN_OFFSET 6000 - -#define WALL 45 -#define WALL_MIN 25 -#define Standby_Time 10 -#define GETOFF_TIME 3 - -DigitalOut led1(LED1); -DigitalOut led2(LED2); -DigitalOut led3(LED3); -DigitalOut led4(LED4); - -Timer t; -double Time = 0; - -Serial pc(USBTX, USBRX); -Serial LED(PB_6,PB_7); -char Serialdata; -BusOut myled(LED1, LED2, LED3, LED4); - -//HCSR04 u1(p13, p14), u2(p11, p12), u3(p23, p24), u4(p25, p26); // Trigger(DO), Echo(PWMIN); LPC1768 -HCSR04 u1(PA_0, PA_1),u2(PA_5, PA_6),u3(PB_0, PA_10); // Trigger(DO), Echo(PWMIN); f303k8 - -CANMessage canmsgTx; -CANMessage canmsgRx; -//CAN canPort(p30, p29); //CAN name(PinName rd, PinName td) LPC1768 -CAN canPort(PA_11, PA_12); //CAN name(PinName rd, PinName td) F303k8 - -//プロトタイプ宣言 -//------------------send関数------------------- -//mode Setting -void sendOPModeT(int); //Operating Mode -void sendOPModeV(int); //Operating Mode -//Control Word -void sendCtrlRS(int); //Reset -void sendCtrlSD(int); //Shutdown -void sendCtrlEN(int); //Switch on & Enable -void sendCtrlQS(int); //Quick Stop -void sendCtrlHL(int); //Halt -//Velocity Setting -void sendTgtVel(int,int); //Target Velocity -//Torque Setting -void sendTgtTrq(int,int); //Target Torque -//Acceleration Setting -void sendProAcc(int,int); //Plof Acceleration -void sendProDec(int,int); //Plof Deceleration -//------------------read関数------------------- -void readActVel(int); //Actual Velocity -void readActPos(int); //Actual Position -//-------------------その他-------------------- -void printCANTX(void); //CAN送信データをPCに表示 -void printCANRX(void); //CAN受信データをPCに表示 -void CANdataRX(void); //CAN受信処理 -void SerialRX(void); //Serial受信処理 -//--------------------------------------------- -void set_ACC(int); -void set_DEC(int); -void set_MODE_V(void); -void set_MODE_T(void); - -void vel_stop(void); - -void vel_forward(int); -void vel_forward_con(int); -void vel_backward(int); -void vel_backward_con(int); -void vel_right(int); -void vel_right_con(int); -void vel_left(int); - -//unsigned int get_cm_n(HCSR04, unsigned int); -//USE -> unsigned int dist_UnitA = get_cm_n(u2, 5); -unsigned int get_cm_n(HCSR04 &echo_unit,int echo_n){ - unsigned int sampled_dist=0; - for (int iter_n = 0; iter_n <echo_n; iter_n++){ - echo_unit.start(); - sampled_dist += echo_unit.get_dist_cm(); - } - return (sampled_dist / echo_n); -} - -int nodeall=4; - -int main(){ - //Serial +//aoki +#include "yuka.h" +Timer t_obj; +double auto_time = 0; +int main() +{ pc.attach(SerialRX); //pc.baud(115200); - //CAN canPort.frequency(1000000); //Bit Rate:1MHz canPort.attach(CANdataRX,CAN::RxIrq); - int node1 = 1; //CAN node Setting + //CAN node Setting + int node1 = 1; int node2 = 2; int node3 = 3; int node4 = 4; - + //エンコーダ関係 - int ActPos = 0; - int Init_Pos = 0; - - //超音波センサ関係パラメータ - int dist1,dist2,dist3,dist4; + int ActPos1 = 0; + int ActPos2 = 0; - //PID制御関係 - //角度調整パラメータ - + int Init_Pos = 0; + //超音波センサ関係パラメータ + int dist1; + int dist2; + int dist3; - #define DELTA_T1 0.1 - #define target_val1 0 - #define Kp1 3 - #define Ki1 0 - #define Kd1 0 - pc.printf("YUKA PROGRAM START\r\n"); wait(0.1); //-------------起動時に必ず送信--------------- @@ -140,638 +34,189 @@ sendOPModeT(node2); sendOPModeT(node3); sendOPModeT(node4); - //Shutdown,Enableコマンド送信|リセット sendCtrlSD(node1); sendCtrlSD(node2); sendCtrlSD(node3); sendCtrlSD(node4); - + sendCtrlEN(node1); sendCtrlEN(node2); sendCtrlEN(node3); sendCtrlEN(node4); - + //初期加減速度 int Acc = 2000; int Dec = 2000; - sendProAcc(1,Acc); sendProAcc(2,Acc); sendProAcc(3,Acc); sendProAcc(4,Acc); - + sendProDec(1,Dec); sendProDec(2,Dec); sendProDec(3,Dec); - sendProDec(4,Dec); - + sendProDec(4,Dec); //トルク設定 int trq = 100; //torque Setting[mA] - + sendTgtTrq(1,trq); sendTgtTrq(2,trq); sendTgtTrq(3,trq); sendTgtTrq(4,trq); - + int state_1 = 0; - int state_2 = 0; + int state_2 = 0; int ride_count = 0; int X_POS_TMP = 0; int dist1_ori = 0; int dist2_ori = 0; - dist1 = 0; readActPos(1); ActPos=canmsgRx.data[4]+canmsgRx.data[5]*256+canmsgRx.data[6]*65536; - if(ActPos > 8388608){ + if(ActPos > 8388608) { ActPos -= 0x1000000;//16進数でのマイナス処理 16^7 = 16,777,216 = 0x1000000 } Init_Pos = ActPos;//起動時の角度を保存 - t.reset(); - t.start(); - + t_obj.reset(); + t_obj.start(); //set_MODE_T(); - printf("\nstart\r\n"); - - while(1){ - - Time = t.read(); + while(1) { + auto_time = t_obj.read(); pc.printf("state_1:%d state_2:%d\r\n",state_1,state_2); - LED.printf("%d",state_1); + LED.printf("%d",state_1); readActPos(1); ActPos=canmsgRx.data[4]+canmsgRx.data[5]*256+canmsgRx.data[6]*65536; - if(ActPos > 8388608){ + if(ActPos > 8388608) { ActPos -= 0x1000000;//16進数でのマイナス処理 16^7 = 16,777,216 = 0x1000000 //printf("check\r\n"); } - dist1 = get_cm_n(u1, 5); - dist2 = get_cm_n(u2, 5); - dist3 = get_cm_n(u3, 5); - + dist1 = get_cm_n(u1, 3); + dist2 = get_cm_n(u2, 3); + dist3 = get_cm_n(u3, 3); /*--------------------------*/ - // - if(state_1 == 0){//入力判断フェーズ + if(state_1 == 0) { //入力判断フェーズ state_2 = 0; - if(ride_count >= 2 && Time > Standby_Time){ - state_1 = 20; - }else{ - if(ActPos < (Init_Pos - KICK)){ //前入力検出 - ride_count++; + if(ride_count >= 2 && auto_time > Standby_Time) { + state_1 = 20; + } else { + if(ActPos < (Init_Pos - KICK)) { //前入力検出 + ride_count++; state_1 = 1; - }else if(ActPos > (Init_Pos + KICK)){ //右入力検出 + } else if(ActPos > (Init_Pos + KICK)) { //右入力検出 ride_count++; state_1 = 11; - }else{ + } else { set_MODE_T(); } } - - }else if(state_1 == 1){//前進→壁検出フェーズ - if(dist1 < WALL && dist1 >= WALL_MIN){ + + } else if(state_1 == 1) { //前進→壁検出フェーズ + if(dist1 < WALL && dist1 >= WALL_MIN) { vel_stop(); wait(GETOFF_TIME); state_1 = 2; - }else{ + } else { set_ACC(ACC_RIDE);//加速度設定 set_DEC(DEC_CLEAN);//減速度設定 set_MODE_V();//速度制御モード送信 vel_forward_con(RPM_RIDE);//前進速度指令 } - }else if(state_1 == 2){//前進からの帰還フェーズ - if(ActPos > -3000){ + } else if(state_1 == 2) { //前進からの帰還フェーズ + if(ActPos > -3000) { vel_stop(); - t.reset(); - t.start(); + t_obj.reset(); + t_obj.start(); state_1 = 0; wait(1.0); - }else{ + } else { vel_backward_con(RPM_RIDE); } - }else if(state_1 == 11){//右進→壁検出フェーズ - if(dist3 < WALL && dist3 >= WALL_MIN){ + } else if(state_1 == 11) { //右進→壁検出フェーズ + if(dist3 < WALL && dist3 >= WALL_MIN) { vel_stop(); wait(GETOFF_TIME); state_1 = 12; - }else{ + } else { set_ACC(ACC_RIDE);//加速度設定 set_DEC(DEC_RIDE);//減速度設定 set_MODE_V();//速度制御モード送信 vel_right(RPM_RIDE);//R進速度指令 } - }else if(state_1 == 12){//右進からの帰還フェーズ - if(ActPos < 3000){ + } else if(state_1 == 12) { //右進からの帰還フェーズ + if(ActPos < 3000) { vel_stop(); - t.reset(); - t.start(); + t_obj.reset(); + t_obj.start(); state_1 = 0; wait(1.0); - }else{ + } else { vel_left(RPM_RIDE); - } - }else if(state_1 == 20){//消毒モード - if(state_2 == 0){ - if(dist1 < WALL && dist1 >= WALL_MIN){ + } + } else if(state_1 == 20) { //消毒モード + if(state_2 == 0) { + if(dist1 < WALL && dist1 >= WALL_MIN) { X_POS_TMP = ActPos; state_2 = 1; - }else{ + } else { set_ACC(ACC_CLEAN);//加速度設定 set_DEC(DEC_CLEAN);//減速度設定 set_MODE_V();//速度制御モード送信 vel_forward_con(RPM_CLEAN);//前進速度指令 - } - }else if(state_2 == 1){ - if(abs(ActPos - X_POS_TMP) > CLEAN_OFFSET){ + } + } else if(state_2 == 1) { + if(abs(ActPos - X_POS_TMP) > CLEAN_OFFSET) { state_2 == 2; - }else{ - if(dist3 < WALL && dist3 >= WALL_MIN){ + } else { + if(dist3 < WALL && dist3 >= WALL_MIN) { state_2 = 4; - }else{ + } else { set_ACC(ACC_CLEAN);//加速度設定 set_DEC(DEC_CLEAN);//減速度設定 set_MODE_V();//速度制御モード送信 vel_right(RPM_CLEAN);//右進速度指令 } } - }else if(state_2 == 2){ - if(ActPos > -3000){ + } else if(state_2 == 2) { + if(ActPos > -3000) { state_2 = 3; - }else{ + } else { set_ACC(ACC_CLEAN);//加速度設定 set_DEC(DEC_CLEAN);//減速度設定 set_MODE_V();//速度制御モード送信 vel_backward_con(RPM_CLEAN);//後進速度指令 } - }else if(state_2 == 3){ - if(abs(ActPos - X_POS_TMP ) > CLEAN_OFFSET){ + } else if(state_2 == 3) { + if(abs(ActPos - X_POS_TMP ) > CLEAN_OFFSET) { state_2 == 0; - }else{ - if(dist3 < WALL && dist3 >= WALL_MIN){ + } else { + if(dist3 < WALL && dist3 >= WALL_MIN) { state_2 = 4; - }else{ - set_ACC(ACC_CLEAN);//加速度設定 - set_DEC(DEC_CLEAN);//減速度設定 - set_MODE_V();//速度制御モード送信 - vel_right(RPM_CLEAN);//右進速度指令 + } else { + set_ACC(ACC_CLEAN);//加速度設定 + set_DEC(DEC_CLEAN);//減速度設定 + set_MODE_V();//速度制御モード送信 + vel_right(RPM_CLEAN);//右進速度指令 } - } - }else if(state_2 == 4){ - if(ActPos < 3000){ + } + } else if(state_2 == 4) { + if(ActPos < 3000) { state_2 = 5; - }else{ + } else { vel_left(RPM_CLEAN); - } - }else if(state_2 == 5){ - if(ActPos > -3000){ - t.reset(); - t.start(); + } + } else if(state_2 == 5) { + if(ActPos > -3000) { + t_obj.reset(); + t_obj.start(); state_1 = 0; state_2 = 0; - }else{ + } else { vel_backward_con(RPM_CLEAN); } } - } - } -} - -void vel_right(int rpm){ - sendTgtVel(1,rpm); - sendTgtVel(2,rpm*(-1)); - sendTgtVel(3,rpm*(-1)); - sendTgtVel(4,rpm); - for(int i=1;i<= 4;i++){ - sendCtrlEN(i); - } -} -void vel_right_con(int rpmA){ - int dis1 = get_cm_n(u1,5); - int dis2 = get_cm_n(u2,5); - - //速度を指定 - int robot_angle = ((dis1 - dis2)*5); - sendTgtVel(1,rpmA+robot_angle); - sendTgtVel(2,rpmA*(-1)+robot_angle); - sendTgtVel(3,rpmA*(-1)+robot_angle); - sendTgtVel(4,rpmA+robot_angle); - //指令値を送信 - for(int i=1;i<= 4;i++){ - sendCtrlEN(i); - } -} - - -void vel_left(int rpm){ - sendTgtVel(1,rpm*(-1)); - sendTgtVel(2,rpm); - sendTgtVel(3,rpm); - sendTgtVel(4,rpm*(-1)); - for(int i=1;i<= 4;i++){ - sendCtrlEN(i); - } -} - - -void vel_left_con(int rpmA){ - int dis1 = get_cm_n(u1,5); - int dis2 = get_cm_n(u2,5); - - //速度を指定 - int robot_angle = ((dis1 - dis2)*5); - sendTgtVel(1,rpmA*(-1)+robot_angle); - sendTgtVel(2,rpmA+robot_angle); - sendTgtVel(3,rpmA+robot_angle); - sendTgtVel(4,rpmA*(-1)+robot_angle); - //指令値を送信 - for(int i=1;i<= 4;i++){ - sendCtrlEN(i); - } -} - -void vel_stop(){ - //速度を指定 - sendTgtVel(1,0); - sendTgtVel(2,0); - sendTgtVel(3,0); - sendTgtVel(4,0); - //指令値を送信 - for(int i=1;i<= 4;i++){ - sendCtrlEN(i); - } -} - -void vel_backward(int rpm){ - //速度を指定 - sendTgtVel(1,rpm); - sendTgtVel(2,rpm); - sendTgtVel(3,rpm*(-1)); - sendTgtVel(4,rpm*(-1)); - //指令値を送信 - for(int i=1;i<= 4;i++){ - sendCtrlEN(i); - } -} - -void vel_backward_con(int rpmA){ - int dis1 = get_cm_n(u1,5); - int dis2 = get_cm_n(u2,5); - - //速度を指定 - int robot_angle = ((dis1 - dis2)*10); - sendTgtVel(1,rpmA+robot_angle); - sendTgtVel(2,rpmA+robot_angle); - sendTgtVel(3,rpmA*(-1)+robot_angle); - sendTgtVel(4,rpmA*(-1)+robot_angle); - //指令値を送信 - for(int i=1;i<= 4;i++){ - sendCtrlEN(i); - } -} - - -void vel_forward_con(int rpmA){ - int dis1 = get_cm_n(u1,5); - int dis2 = get_cm_n(u2,5); - - //速度を指定 - int robot_angle = ((dis1 - dis2)*10); - sendTgtVel(1,rpmA*(-1)+robot_angle); - sendTgtVel(2,rpmA*(-1)+robot_angle); - sendTgtVel(3,rpmA+robot_angle); - sendTgtVel(4,rpmA+robot_angle); - //指令値を送信 - for(int i=1;i<= 4;i++){ - sendCtrlEN(i); + } } -} - -void set_ACC(int setACC_val){ - sendProAcc(1,setACC_val); - sendProAcc(2,setACC_val); - sendProAcc(3,setACC_val); - sendProAcc(4,setACC_val); -} - -void set_DEC(int setDEC_val){ - sendProDec(1,setDEC_val); - sendProDec(2,setDEC_val); - sendProDec(3,setDEC_val); - sendProDec(4,setDEC_val); -} - -void set_MODE_V(){ - sendOPModeV(1); - sendOPModeV(2); - sendOPModeV(3); - sendOPModeV(4); -} - -void set_MODE_T(){ - sendOPModeT(1); - sendOPModeT(2); - sendOPModeT(3); - sendOPModeT(4); -} - -//0x2F-6060-00-fd-//-//-// -void sendOPModeT(int nodeID){ - canmsgTx.id = 0x600+nodeID; - canmsgTx.len = 5; //Data Length - canmsgTx.data[0] = 0x2F;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| - canmsgTx.data[1] = 0x60;//Index LowByte - canmsgTx.data[2] = 0x60;//Index HighByte - canmsgTx.data[3] = 0x00;//sub-Index - canmsgTx.data[4] = 0xFD;//data:fd = "current Mode" - /* - canmsgTx.data[5] = 0x00;//data:(user value) - canmsgTx.data[6] = 0x00;//data:(user value) - canmsgTx.data[7] = 0x00;//data:(user value) - */ - printCANTX(); //CAN送信データをPCに表示 - canPort.write(canmsgTx);//CANでデータ送信 - wait_ms(1); -} - - -//0x2F-6060-00-03-//-//-// -void sendOPModeV(int nodeID){ - canmsgTx.id = 0x600+nodeID; - canmsgTx.len = 5; //Data Length - canmsgTx.data[0] = 0x2F;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| - canmsgTx.data[1] = 0x60;//Index LowByte - canmsgTx.data[2] = 0x60;//Index HighByte - canmsgTx.data[3] = 0x00;//sub-Index - canmsgTx.data[4] = 0x03;//data:0x03 = "Profile Velocity Mode" - /* - canmsgTx.data[5] = 0x00;//data:(user value) - canmsgTx.data[6] = 0x00;//data:(user value) - canmsgTx.data[7] = 0x00;//data:(user value) - */ - printCANTX(); //CAN送信データをPCに表示 - canPort.write(canmsgTx);//CANでデータ送信 - wait_ms(1); -} - -//0x2B-6040-00-0000-//-// -void sendCtrlRS(int nodeID){ - canmsgTx.id = 0x600+nodeID; - canmsgTx.len = 6; //Data Length - canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| - canmsgTx.data[1] = 0x40;//Index LowByte - canmsgTx.data[2] = 0x60;//Index HighByte - canmsgTx.data[3] = 0x00;//sub-Index - canmsgTx.data[4] = 0x80;//data:0x00"80" = "Controlword(Shutdown)" - canmsgTx.data[5] = 0x00;//data:0x"00"80 - /* - canmsgTx.data[6] = 0x00;//data:(user value) - canmsgTx.data[7] = 0x00;//data:(user value) - */ - printCANTX(); //CAN送信データをPCに表示 - canPort.write(canmsgTx);//CANでデータ送信 - wait_ms(1); -} - - -//0x2B-6040-00-0006-//-// -void sendCtrlSD(int nodeID){ - canmsgTx.id = 0x600+nodeID; - canmsgTx.len = 6; //Data Length - canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| - canmsgTx.data[1] = 0x40;//Index LowByte - canmsgTx.data[2] = 0x60;//Index HighByte - canmsgTx.data[3] = 0x00;//sub-Index - canmsgTx.data[4] = 0x06;//data:0x00"06" = "Controlword(Shutdown)" - canmsgTx.data[5] = 0x00;//data:0x"00"06 - /* - canmsgTx.data[6] = 0x00;//data:(user value) - canmsgTx.data[7] = 0x00;//data:(user value) - */ - printCANTX(); //CAN送信データをPCに表示 - canPort.write(canmsgTx);//CANでデータ送信 - wait_ms(1); -} - -//0x2B-6040-00-000F-//-// -void sendCtrlEN(int nodeID){ - canmsgTx.id = 0x600+nodeID; - canmsgTx.len = 6; //Data Length - canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| - canmsgTx.data[1] = 0x40;//Index LowByte - canmsgTx.data[2] = 0x60;//Index HighByte - canmsgTx.data[3] = 0x00;//sub-Index - canmsgTx.data[4] = 0x0F;//data:0x00"0F" = "Controlword(Enable)" - canmsgTx.data[5] = 0x00;//data:0x"00"0F - /* - canmsgTx.data[6] = 0x00;//data:(user value) - canmsgTx.data[7] = 0x00;//data:(user value) - */ - //printCANTX(); //CAN送信データをPCに表示 - canPort.write(canmsgTx);//CANでデータ送信 - wait_ms(1); -} - -//0x2B-6040-00-000B-//-// -void sendCtrlQS(int nodeID){ - canmsgTx.id = 0x600+nodeID; - canmsgTx.len = 6; //Data Length - canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| - canmsgTx.data[1] = 0x40;//Index LowByte - canmsgTx.data[2] = 0x60;//Index HighByte - canmsgTx.data[3] = 0x00;//sub-Index - canmsgTx.data[4] = 0x0B;//data:0x00"0B" = "Quick Stop" - canmsgTx.data[5] = 0x00;//data:0x"00"0B - /* - canmsgTx.data[6] = 0x00;//data:(user value) - canmsgTx.data[7] = 0x00;//data:(user value) - */ - printCANTX(); //CAN送信データをPCに表示 - canPort.write(canmsgTx);//CANでデータ送信 - wait_ms(1); -} - -//0x2B-6040-00-010F-//-// -void sendCtrlHL(int nodeID){ - canmsgTx.id = 0x600+nodeID; - canmsgTx.len = 6; //Data Length - canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| - canmsgTx.data[1] = 0x40;//Index LowByte - canmsgTx.data[2] = 0x60;//Index HighByte - canmsgTx.data[3] = 0x00;//sub-Index - canmsgTx.data[4] = 0x0F;//data:0x01"0F" = "Halt" - canmsgTx.data[5] = 0x01;//data:0x"01"0F - /* - canmsgTx.data[6] = 0x00;//data:(user value) - canmsgTx.data[7] = 0x00;//data:(user value) - */ - printCANTX(); //CAN送信データをPCに表示 - canPort.write(canmsgTx);//CANでデータ送信 - wait_ms(1); -} - -//0x2B-60FF-00-[user data(4Byte)] -void sendTgtTrq(int nodeID,int trq){ - //pc.printf("%dmA0x%08x\r\n",trq,trq); //回転数送信データの表示 - canmsgTx.id = 0x600+nodeID; - canmsgTx.len = 6; //Data Length - canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| - canmsgTx.data[1] = 0x30;//Index LowByte 71 - canmsgTx.data[2] = 0x20;//Index HighByte 60 - canmsgTx.data[3] = 0x00;//sub-Index - //下位から1Byteずつdataに格納 - if(trq<0){ - trq=0xFFFF+trq+1; - } - - //pc.printf("iii%d\r\n",trq); - //canmsgTx.data[7]=((trq>>24)&0xFF); - //canmsgTx.data[6]=((trq>>16)&0xFF); - - canmsgTx.data[5]=((trq>>8)&0xFF); - canmsgTx.data[4]=((trq>>0)&0xFF); - //printCANTX(); //CAN送信データをPCに表示 - canPort.write(canmsgTx);//CANでデータ送信 - wait_ms(2); - //send Enable - //pc.printf("Send Enable Command\r\n"); - //sendCtrlEN(nodeID); - //wait(0.5); -} - - - - -//0x2B-60FF-00-[user data(4Byte)] -void sendTgtVel(int nodeID,int rpm){ - //pc.printf("%drpm|0x%08x\r\n",rpm,rpm); //回転数送信データの表示 - canmsgTx.id = 0x600+nodeID; - canmsgTx.len = 8; //Data Length - canmsgTx.data[0] = 0x23;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| - canmsgTx.data[1] = 0xFF;//Index LowByte - canmsgTx.data[2] = 0x60;//Index HighByte - canmsgTx.data[3] = 0x00;//sub-Index - //下位から1Byteずつdataに格納 - - //pc.printf("%d\r\n",rpm); - if(rpm<0){ - rpm=0xFFFFFFFF+rpm+1; - } - canmsgTx.data[7]=((rpm>>24)&0xFF); - canmsgTx.data[6]=((rpm>>16)&0xFF); - canmsgTx.data[5]=((rpm>>8)&0xFF); - canmsgTx.data[4]=((rpm>>0)&0xFF); - - //printCANTX(); //CAN送信データをPCに表示 - canPort.write(canmsgTx);//CANでデータ送信 - wait_ms(1); - -} - -void readActVel(int nodeID){ - //値が欲しいobjectのアドレスを送る - canmsgTx.id = 0x600+nodeID; - canmsgTx.len = 4; //Data Length - canmsgTx.data[0] = 0x40;//|0Byte:40| - canmsgTx.data[1] = 0x6C;//Index LowByte - canmsgTx.data[2] = 0x60;//Index HighByte - canmsgTx.data[3] = 0x00;//sub-Index - canPort.write(canmsgTx); - wait_ms(1); -} - -void readActPos(int nodeID){ - //値が欲しいobjectのアドレスを送る - canmsgTx.id = 0x600+nodeID; - canmsgTx.len = 4; //Data Length - canmsgTx.data[0] = 0x40;//|0Byte:40| - canmsgTx.data[1] = 0x64;//Index LowByte - canmsgTx.data[2] = 0x60;//Index HighByte - canmsgTx.data[3] = 0x00;//sub-Index - canPort.write(canmsgTx); - wait_ms(1); -} - -//加速度指定 -void sendProAcc(int nodeID,int rpm){ - if(rpm < 0){ - rpm += 0xFFFFFFFF; - } -// pc.printf("%drpm|0x%08x\r\n",rpm,rpm); //回転数送信データの表示 - canmsgTx.id = 0x600+nodeID; - canmsgTx.len = 8; //Data Length - canmsgTx.data[0] = 0x23;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| - canmsgTx.data[1] = 0x83;//Index LowByte - canmsgTx.data[2] = 0x60;//Index HighByte - canmsgTx.data[3] = 0x00;//sub-Index - //下位から1Byteずつdataに格納 - canmsgTx.data[4] = (rpm >> 0) & 0xFF; - canmsgTx.data[5] = (rpm >> 8) & 0xFF; - canmsgTx.data[6] = (rpm >> 16) & 0xFF; - canmsgTx.data[7] = (rpm >> 24) & 0xFF; -// printCANTX(); //CAN送信データをPCに表示 - canPort.write(canmsgTx);//CANでデータ送信 - wait(0.01); - //send Enable -// pc.printf("Send Enable Command\r\n"); - sendCtrlEN(nodeID); - wait(0.01); -} - -//減速度指定 -void sendProDec(int nodeID,int rpm){ - if(rpm < 0){ - rpm += 0xFFFFFFFF; - } -// pc.printf("%drpm|0x%08x\r\n",rpm,rpm); //回転数送信データの表示 - canmsgTx.id = 0x600+nodeID; - canmsgTx.len = 8; //Data Length - canmsgTx.data[0] = 0x23;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| - canmsgTx.data[1] = 0x84;//Index LowByte - canmsgTx.data[2] = 0x60;//Index HighByte - canmsgTx.data[3] = 0x00;//sub-Index - //下位から1Byteずつdataに格納 - canmsgTx.data[4] = (rpm >> 0) & 0xFF; - canmsgTx.data[5] = (rpm >> 8) & 0xFF; - canmsgTx.data[6] = (rpm >> 16) & 0xFF; - canmsgTx.data[7] = (rpm >> 24) & 0xFF; -// printCANTX(); //CAN送信データをPCに表示 - canPort.write(canmsgTx);//CANでデータ送信 - wait(0.01); - //send Enable -// pc.printf("Send Enable Command\r\n"); - sendCtrlEN(nodeID); - wait(0.01); -} - - -//送信データの表示 -void printCANTX(void){ - //0x canID|Byte0|Byte1|Byte2|Byte3|Byte4|Byte5|Byte6|Byte7| - //pc.printf("0x%3x|",canmsgTx.id); - for(char i=0;i < canmsgTx.len;i++){ - //pc.printf("%02x|",canmsgTx.data[i]); - } - //pc.printf("\r\n"); -} - -//受信データの表示 - -void printCANRX(void){ - //0x canID|Byte0|Byte1|Byte2|Byte3|Byte4|Byte5|Byte6|Byte7| - //pc.printf("0x%3x|",canmsgRx.id); - for(char i=0;i < canmsgRx.len;i++){ - //pc.printf("%02x|",canmsgRx.data[i]); - } - //pc.printf("\r\n"); -} - -void CANdataRX(void){ - canPort.read(canmsgRx); - printCANRX(); -} - -void SerialRX(void){ - Serialdata = pc.getc(); - //pc.printf("%c\r\n",Serialdata); } \ No newline at end of file
diff -r f21e1a75f98b -r dd35cc3b5ca0 yuka.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/yuka.h Mon Dec 20 15:49:55 2021 +0000 @@ -0,0 +1,536 @@ +#include "mbed.h" +#include "hcsr04.h" + +#define RPM_RIDE 400 +#define RPM_CLEAN 400 +#define ACC_RIDE 1000 +#define DEC_RIDE 700 +#define ACC_CLEAN 1000 +#define DEC_CLEAN 1000 + +#define KICK 2000 +#define CLEAN_OFFSET 6000 + +#define WALL 45 +#define WALL_MIN 25 +#define Standby_Time 10 +#define GETOFF_TIME 3 + + +//プロトタイプ宣言 +//------------------send関数------------------- +//mode Setting +void sendOPModeT(int); //Operating Mode +void sendOPModeV(int); //Operating Mode +//Control Word +void sendCtrlRS(int); //Reset +void sendCtrlSD(int); //Shutdown +void sendCtrlEN(int); //Switch on & Enable +void sendCtrlQS(int); //Quick Stop +void sendCtrlHL(int); //Halt +//Velocity Setting +void sendTgtVel(int,int); //Target Velocity +//Torque Setting +void sendTgtTrq(int,int); //Target Torque +//Acceleration Setting +void sendProAcc(int,int); //Plof Acceleration +void sendProDec(int,int); //Plof Deceleration +//------------------read関数------------------- +void readActVel(int); //Actual Velocity +void readActPos(int); //Actual Position +//-------------------その他-------------------- +void printCANTX(void); //CAN送信データをPCに表示 +void printCANRX(void); //CAN受信データをPCに表示 +void CANdataRX(void); //CAN受信処理 +void SerialRX(void); //Serial受信処理 +//--------------------------------------------- +void set_ACC(int); +void set_DEC(int); +void set_MODE_V(void); +void set_MODE_T(void); + +void vel_stop(void); + +void vel_forward(int); +void vel_forward_con(int); +void vel_backward(int); +void vel_backward_con(int); +void vel_right(int); +void vel_right_con(int); +void vel_left(int); + + +Serial pc(USBTX, USBRX); +Serial LED(PB_6,PB_7); +char Serialdata; +BusOut myled(LED1, LED2, LED3, LED4); + +//HCSR04 u1(p13, p14), u2(p11, p12), u3(p23, p24), u4(p25, p26); // Trigger(DO), Echo(PWMIN); LPC1768 +HCSR04 u1(PA_0, PA_1),u2(PA_5, PA_6),u3(PB_0, PA_10); // Trigger(DO), Echo(PWMIN); f303k8 + +CANMessage canmsgTx; +CANMessage canmsgRx; +//CAN canPort(p30, p29); //CAN name(PinName rd, PinName td) LPC1768 +CAN canPort(PA_11, PA_12); //CAN name(PinName rd, PinName td) F303k8 + +int nodeall=4; + +DigitalOut led1(LED1); +DigitalOut led2(LED2); +DigitalOut led3(LED3); +DigitalOut led4(LED4); + + + +//unsigned int get_cm_n(HCSR04, unsigned int); +//USE -> unsigned int dist_UnitA = get_cm_n(u2, 5); +unsigned int get_cm_n(HCSR04 &echo_unit,int echo_n){ + unsigned int sampled_dist=0; + for (int iter_n = 0; iter_n <echo_n; iter_n++){ + echo_unit.start(); + sampled_dist += echo_unit.get_dist_cm(); + } + return (sampled_dist / echo_n); +} + + +void set_ACC(int setACC_val){ + sendProAcc(1,setACC_val); + sendProAcc(2,setACC_val); + sendProAcc(3,setACC_val); + sendProAcc(4,setACC_val); +} + +void set_DEC(int setDEC_val){ + sendProDec(1,setDEC_val); + sendProDec(2,setDEC_val); + sendProDec(3,setDEC_val); + sendProDec(4,setDEC_val); +} + +void set_MODE_V(){ + sendOPModeV(1); + sendOPModeV(2); + sendOPModeV(3); + sendOPModeV(4); +} + +void set_MODE_T(){ + sendOPModeT(1); + sendOPModeT(2); + sendOPModeT(3); + sendOPModeT(4); +} + +//0x2F-6060-00-fd-//-//-// +void sendOPModeT(int nodeID){ + canmsgTx.id = 0x600+nodeID; + canmsgTx.len = 5; //Data Length + canmsgTx.data[0] = 0x2F;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| + canmsgTx.data[1] = 0x60;//Index LowByte + canmsgTx.data[2] = 0x60;//Index HighByte + canmsgTx.data[3] = 0x00;//sub-Index + canmsgTx.data[4] = 0xFD;//data:fd = "current Mode" + /* + canmsgTx.data[5] = 0x00;//data:(user value) + canmsgTx.data[6] = 0x00;//data:(user value) + canmsgTx.data[7] = 0x00;//data:(user value) + */ + printCANTX(); //CAN送信データをPCに表示 + canPort.write(canmsgTx);//CANでデータ送信 + wait_ms(1); +} + + +//0x2F-6060-00-03-//-//-// +void sendOPModeV(int nodeID){ + canmsgTx.id = 0x600+nodeID; + canmsgTx.len = 5; //Data Length + canmsgTx.data[0] = 0x2F;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| + canmsgTx.data[1] = 0x60;//Index LowByte + canmsgTx.data[2] = 0x60;//Index HighByte + canmsgTx.data[3] = 0x00;//sub-Index + canmsgTx.data[4] = 0x03;//data:0x03 = "Profile Velocity Mode" + /* + canmsgTx.data[5] = 0x00;//data:(user value) + canmsgTx.data[6] = 0x00;//data:(user value) + canmsgTx.data[7] = 0x00;//data:(user value) + */ + printCANTX(); //CAN送信データをPCに表示 + canPort.write(canmsgTx);//CANでデータ送信 + wait_ms(1); +} + +//0x2B-6040-00-0000-//-// +void sendCtrlRS(int nodeID){ + canmsgTx.id = 0x600+nodeID; + canmsgTx.len = 6; //Data Length + canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| + canmsgTx.data[1] = 0x40;//Index LowByte + canmsgTx.data[2] = 0x60;//Index HighByte + canmsgTx.data[3] = 0x00;//sub-Index + canmsgTx.data[4] = 0x80;//data:0x00"80" = "Controlword(Shutdown)" + canmsgTx.data[5] = 0x00;//data:0x"00"80 + /* + canmsgTx.data[6] = 0x00;//data:(user value) + canmsgTx.data[7] = 0x00;//data:(user value) + */ + printCANTX(); //CAN送信データをPCに表示 + canPort.write(canmsgTx);//CANでデータ送信 + wait_ms(1); +} + + +//0x2B-6040-00-0006-//-// +void sendCtrlSD(int nodeID){ + canmsgTx.id = 0x600+nodeID; + canmsgTx.len = 6; //Data Length + canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| + canmsgTx.data[1] = 0x40;//Index LowByte + canmsgTx.data[2] = 0x60;//Index HighByte + canmsgTx.data[3] = 0x00;//sub-Index + canmsgTx.data[4] = 0x06;//data:0x00"06" = "Controlword(Shutdown)" + canmsgTx.data[5] = 0x00;//data:0x"00"06 + /* + canmsgTx.data[6] = 0x00;//data:(user value) + canmsgTx.data[7] = 0x00;//data:(user value) + */ + printCANTX(); //CAN送信データをPCに表示 + canPort.write(canmsgTx);//CANでデータ送信 + wait_ms(1); +} + +//0x2B-6040-00-000F-//-// +void sendCtrlEN(int nodeID){ + canmsgTx.id = 0x600+nodeID; + canmsgTx.len = 6; //Data Length + canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| + canmsgTx.data[1] = 0x40;//Index LowByte + canmsgTx.data[2] = 0x60;//Index HighByte + canmsgTx.data[3] = 0x00;//sub-Index + canmsgTx.data[4] = 0x0F;//data:0x00"0F" = "Controlword(Enable)" + canmsgTx.data[5] = 0x00;//data:0x"00"0F + /* + canmsgTx.data[6] = 0x00;//data:(user value) + canmsgTx.data[7] = 0x00;//data:(user value) + */ + //printCANTX(); //CAN送信データをPCに表示 + canPort.write(canmsgTx);//CANでデータ送信 + wait_ms(1); +} + +//0x2B-6040-00-000B-//-// +void sendCtrlQS(int nodeID){ + canmsgTx.id = 0x600+nodeID; + canmsgTx.len = 6; //Data Length + canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| + canmsgTx.data[1] = 0x40;//Index LowByte + canmsgTx.data[2] = 0x60;//Index HighByte + canmsgTx.data[3] = 0x00;//sub-Index + canmsgTx.data[4] = 0x0B;//data:0x00"0B" = "Quick Stop" + canmsgTx.data[5] = 0x00;//data:0x"00"0B + /* + canmsgTx.data[6] = 0x00;//data:(user value) + canmsgTx.data[7] = 0x00;//data:(user value) + */ + printCANTX(); //CAN送信データをPCに表示 + canPort.write(canmsgTx);//CANでデータ送信 + wait_ms(1); +} + +//0x2B-6040-00-010F-//-// +void sendCtrlHL(int nodeID){ + canmsgTx.id = 0x600+nodeID; + canmsgTx.len = 6; //Data Length + canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| + canmsgTx.data[1] = 0x40;//Index LowByte + canmsgTx.data[2] = 0x60;//Index HighByte + canmsgTx.data[3] = 0x00;//sub-Index + canmsgTx.data[4] = 0x0F;//data:0x01"0F" = "Halt" + canmsgTx.data[5] = 0x01;//data:0x"01"0F + /* + canmsgTx.data[6] = 0x00;//data:(user value) + canmsgTx.data[7] = 0x00;//data:(user value) + */ + printCANTX(); //CAN送信データをPCに表示 + canPort.write(canmsgTx);//CANでデータ送信 + wait_ms(1); +} + +//0x2B-60FF-00-[user data(4Byte)] +void sendTgtTrq(int nodeID,int trq){ + //pc.printf("%dmA0x%08x\r\n",trq,trq); //回転数送信データの表示 + canmsgTx.id = 0x600+nodeID; + canmsgTx.len = 6; //Data Length + canmsgTx.data[0] = 0x2B;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| + canmsgTx.data[1] = 0x30;//Index LowByte 71 + canmsgTx.data[2] = 0x20;//Index HighByte 60 + canmsgTx.data[3] = 0x00;//sub-Index + //下位から1Byteずつdataに格納 + if(trq<0){ + trq=0xFFFF+trq+1; + } + + //pc.printf("iii%d\r\n",trq); + //canmsgTx.data[7]=((trq>>24)&0xFF); + //canmsgTx.data[6]=((trq>>16)&0xFF); + + canmsgTx.data[5]=((trq>>8)&0xFF); + canmsgTx.data[4]=((trq>>0)&0xFF); + //printCANTX(); //CAN送信データをPCに表示 + canPort.write(canmsgTx);//CANでデータ送信 + wait_ms(2); + //send Enable + //pc.printf("Send Enable Command\r\n"); + //sendCtrlEN(nodeID); + //wait(0.5); +} + + + + +//0x2B-60FF-00-[user data(4Byte)] +void sendTgtVel(int nodeID,int rpm){ + //pc.printf("%drpm|0x%08x\r\n",rpm,rpm); //回転数送信データの表示 + canmsgTx.id = 0x600+nodeID; + canmsgTx.len = 8; //Data Length + canmsgTx.data[0] = 0x23;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| + canmsgTx.data[1] = 0xFF;//Index LowByte + canmsgTx.data[2] = 0x60;//Index HighByte + canmsgTx.data[3] = 0x00;//sub-Index + //下位から1Byteずつdataに格納 + + //pc.printf("%d\r\n",rpm); + if(rpm<0){ + rpm=0xFFFFFFFF+rpm+1; + } + canmsgTx.data[7]=((rpm>>24)&0xFF); + canmsgTx.data[6]=((rpm>>16)&0xFF); + canmsgTx.data[5]=((rpm>>8)&0xFF); + canmsgTx.data[4]=((rpm>>0)&0xFF); + + //printCANTX(); //CAN送信データをPCに表示 + canPort.write(canmsgTx);//CANでデータ送信 + wait_ms(1); + +} + +void readActVel(int nodeID){ + //値が欲しいobjectのアドレスを送る + canmsgTx.id = 0x600+nodeID; + canmsgTx.len = 4; //Data Length + canmsgTx.data[0] = 0x40;//|0Byte:40| + canmsgTx.data[1] = 0x6C;//Index LowByte + canmsgTx.data[2] = 0x60;//Index HighByte + canmsgTx.data[3] = 0x00;//sub-Index + canPort.write(canmsgTx); + wait_ms(1); +} + +void readActPos(int nodeID){ + //値が欲しいobjectのアドレスを送る + canmsgTx.id = 0x600+nodeID; + canmsgTx.len = 4; //Data Length + canmsgTx.data[0] = 0x40;//|0Byte:40| + canmsgTx.data[1] = 0x64;//Index LowByte + canmsgTx.data[2] = 0x60;//Index HighByte + canmsgTx.data[3] = 0x00;//sub-Index + canPort.write(canmsgTx); + wait_ms(1); +} + +//加速度指定 +void sendProAcc(int nodeID,int rpm){ + if(rpm < 0){ + rpm += 0xFFFFFFFF; + } +// pc.printf("%drpm|0x%08x\r\n",rpm,rpm); //回転数送信データの表示 + canmsgTx.id = 0x600+nodeID; + canmsgTx.len = 8; //Data Length + canmsgTx.data[0] = 0x23;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| + canmsgTx.data[1] = 0x83;//Index LowByte + canmsgTx.data[2] = 0x60;//Index HighByte + canmsgTx.data[3] = 0x00;//sub-Index + //下位から1Byteずつdataに格納 + canmsgTx.data[4] = (rpm >> 0) & 0xFF; + canmsgTx.data[5] = (rpm >> 8) & 0xFF; + canmsgTx.data[6] = (rpm >> 16) & 0xFF; + canmsgTx.data[7] = (rpm >> 24) & 0xFF; +// printCANTX(); //CAN送信データをPCに表示 + canPort.write(canmsgTx);//CANでデータ送信 + wait(0.01); + //send Enable +// pc.printf("Send Enable Command\r\n"); + sendCtrlEN(nodeID); + wait(0.01); +} + +//減速度指定 +void sendProDec(int nodeID,int rpm){ + if(rpm < 0){ + rpm += 0xFFFFFFFF; + } +// pc.printf("%drpm|0x%08x\r\n",rpm,rpm); //回転数送信データの表示 + canmsgTx.id = 0x600+nodeID; + canmsgTx.len = 8; //Data Length + canmsgTx.data[0] = 0x23;//|0Byte:40|1Byte:2F|2Byte:2B|4Byte:23|other:22| + canmsgTx.data[1] = 0x84;//Index LowByte + canmsgTx.data[2] = 0x60;//Index HighByte + canmsgTx.data[3] = 0x00;//sub-Index + //下位から1Byteずつdataに格納 + canmsgTx.data[4] = (rpm >> 0) & 0xFF; + canmsgTx.data[5] = (rpm >> 8) & 0xFF; + canmsgTx.data[6] = (rpm >> 16) & 0xFF; + canmsgTx.data[7] = (rpm >> 24) & 0xFF; +// printCANTX(); //CAN送信データをPCに表示 + canPort.write(canmsgTx);//CANでデータ送信 + wait(0.01); + //send Enable +// pc.printf("Send Enable Command\r\n"); + sendCtrlEN(nodeID); + wait(0.01); +} + + +//送信データの表示 +void printCANTX(void){ + //0x canID|Byte0|Byte1|Byte2|Byte3|Byte4|Byte5|Byte6|Byte7| + //pc.printf("0x%3x|",canmsgTx.id); + for(char i=0;i < canmsgTx.len;i++){ + //pc.printf("%02x|",canmsgTx.data[i]); + } + //pc.printf("\r\n"); +} + +//受信データの表示 + +void printCANRX(void){ + //0x canID|Byte0|Byte1|Byte2|Byte3|Byte4|Byte5|Byte6|Byte7| + //pc.printf("0x%3x|",canmsgRx.id); + for(char i=0;i < canmsgRx.len;i++){ + //pc.printf("%02x|",canmsgRx.data[i]); + } + //pc.printf("\r\n"); +} + +void CANdataRX(void){ + canPort.read(canmsgRx); + printCANRX(); +} + +void SerialRX(void){ + Serialdata = pc.getc(); + //pc.printf("%c\r\n",Serialdata); +} + + +void vel_right(int rpm){ + sendTgtVel(1,rpm); + sendTgtVel(2,rpm*(-1)); + sendTgtVel(3,rpm*(-1)); + sendTgtVel(4,rpm); + for(int i=1;i<= 4;i++){ + sendCtrlEN(i); + } +} +void vel_right_con(int rpmA){ + int dis1 = get_cm_n(u1,5); + int dis2 = get_cm_n(u2,5); + + //速度を指定 + int robot_angle = ((dis1 - dis2)*5); + sendTgtVel(1,rpmA+robot_angle); + sendTgtVel(2,rpmA*(-1)+robot_angle); + sendTgtVel(3,rpmA*(-1)+robot_angle); + sendTgtVel(4,rpmA+robot_angle); + //指令値を送信 + for(int i=1;i<= 4;i++){ + sendCtrlEN(i); + } +} + + +void vel_left(int rpm){ + sendTgtVel(1,rpm*(-1)); + sendTgtVel(2,rpm); + sendTgtVel(3,rpm); + sendTgtVel(4,rpm*(-1)); + for(int i=1;i<= 4;i++){ + sendCtrlEN(i); + } +} + + +void vel_left_con(int rpmA){ + int dis1 = get_cm_n(u1,5); + int dis2 = get_cm_n(u2,5); + + //速度を指定 + int robot_angle = ((dis1 - dis2)*5); + sendTgtVel(1,rpmA*(-1)+robot_angle); + sendTgtVel(2,rpmA+robot_angle); + sendTgtVel(3,rpmA+robot_angle); + sendTgtVel(4,rpmA*(-1)+robot_angle); + //指令値を送信 + for(int i=1;i<= 4;i++){ + sendCtrlEN(i); + } +} + +void vel_stop(){ + //速度を指定 + sendTgtVel(1,0); + sendTgtVel(2,0); + sendTgtVel(3,0); + sendTgtVel(4,0); + //指令値を送信 + for(int i=1;i<= 4;i++){ + sendCtrlEN(i); + } +} + +void vel_backward(int rpm){ + //速度を指定 + sendTgtVel(1,rpm); + sendTgtVel(2,rpm); + sendTgtVel(3,rpm*(-1)); + sendTgtVel(4,rpm*(-1)); + //指令値を送信 + for(int i=1;i<= 4;i++){ + sendCtrlEN(i); + } +} + +void vel_backward_con(int rpmA){ + int dis1 = get_cm_n(u1,5); + int dis2 = get_cm_n(u2,5); + + //速度を指定 + int robot_angle = ((dis1 - dis2)*10); + sendTgtVel(1,rpmA+robot_angle); + sendTgtVel(2,rpmA+robot_angle); + sendTgtVel(3,rpmA*(-1)+robot_angle); + sendTgtVel(4,rpmA*(-1)+robot_angle); + //指令値を送信 + for(int i=1;i<= 4;i++){ + sendCtrlEN(i); + } +} + + +void vel_forward_con(int rpmA){ + int dis1 = get_cm_n(u1,5); + int dis2 = get_cm_n(u2,5); + + //速度を指定 + int robot_angle = ((dis1 - dis2)*10); + sendTgtVel(1,rpmA*(-1)+robot_angle); + sendTgtVel(2,rpmA*(-1)+robot_angle); + sendTgtVel(3,rpmA+robot_angle); + sendTgtVel(4,rpmA+robot_angle); + //指令値を送信 + for(int i=1;i<= 4;i++){ + sendCtrlEN(i); + } +} + \ No newline at end of file