Kanta Takasu
gohome
Revision 10:91d4b6117470, committed 2021-12-21
- Comitter:
- tks1
- Date:
- Tue Dec 21 06:40:21 2021 +0000
- Parent:
- 9:71b780f8d345
- Commit message:
- gohome
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 71b780f8d345 -r 91d4b6117470 main.cpp
--- a/main.cpp Tue Dec 21 00:59:40 2021 +0000
+++ b/main.cpp Tue Dec 21 06:40:21 2021 +0000
@@ -6,8 +6,8 @@
//エンコーダ読み取りによる入力
//加減速度調整パラメータ実装
//PID制御による機体角度補正_
-//aoki
-
+//aoki
+
#include "mbed.h"
#include "hcsr04.h"
@@ -25,30 +25,32 @@
#define WALL_MIN 25
#define Standby_Time 10
#define GETOFF_TIME 1
-
+
+
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
-
+
Timer t;
-Timer Clean_right;
+Timer cr_t;
double Time = 0;
-double Clean_right_time = 0;
+double cr_time = 0;
+int crflag=0;
Serial pc(USBTX, USBRX);
char Serialdata;
BusOut LED(PA_4,PB_6,PB_7,PA_9);
-
+
//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
@@ -90,699 +92,765 @@
void vel_right(int);
void vel_right_con(int);
void vel_left(int);
+int encX = 0;
+int encY = 0;
+int posX =0;
+int posY =0;
+
+//--------//
+void update_pos()
+{
+ readActPos(1);
+ encX=canmsgRx.data[4]+canmsgRx.data[5]*256+canmsgRx.data[6]*65536;
+ if(encX > 8388608) {
+ encX -= 0x1000000;//16進数でのマイナス処理 16^7 = 16,777,216 = 0x1000000
+ }
+ wait_ms(10);
+ readActPos(2);
+ encY=canmsgRx.data[4]+canmsgRx.data[5]*256+canmsgRx.data[6]*65536;
+ if(encY > 8388608) {
+ encY -= 0x1000000;//16進数でのマイナス処理 16^7 = 16,777,216 = 0x1000000
+ }
+ wait_ms(10);
+ posX = (encX*0.707)-(encY*0.707);
+ posY = (-encX*0.707)-(encY*0.707);
+ //printf("encX=[%4d],encY=[%4d],posX=[%4d],posY=[%d]\r\n",encX,encY,posX,posY);
+}
+void gohome()
+{
+ update_pos();
+ while(posX>0) {
+ update_pos();
+ set_ACC(1000);//加速度設定
+ set_DEC(1000);//減速度設定
+ set_MODE_V();//速度制御モード送信
+ vel_right(-300);//前進速度指令
+ }
+ vel_right(0);
+ while(posY>0) {
+ update_pos();
+ set_ACC(1000);//加速度設定
+ set_DEC(1000);//減速度設定
+ set_MODE_V();//速度制御モード送信
+ vel_forward_con(-300);//前進速度指令
+ }
+
+}
//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);
-}
+ 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
+ pc.attach(SerialRX);
+ //pc.baud(115200);
+
+ //CAN
+ canPort.frequency(1000000); //Bit Rate:1MHz
+ canPort.attach(CANdataRX,CAN::RxIrq);
+ int node1 = 1; //CAN node Setting
+ int node2 = 2;
+ int node3 = 3;
+ int node4 = 4;
+
+ //エンコーダ関係
+ int ActPos = 0;
+ int Init_Pos = 0;
+
+ //超音波センサ関係パラメータ
+ int dist1,dist2,dist3,dist4;
+
+ //PID制御関係
+ //角度調整パラメータ
+
+
+#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);
+ //-------------起動時に必ず送信---------------
+ //オペレーティングモードを送信
+ sendOPModeT(node1);
+ sendOPModeT(node2);
+ sendOPModeT(node3);
+ sendOPModeT(node4);
-int nodeall=4;
-
-int main(){
- //Serial
- pc.attach(SerialRX);
- //pc.baud(115200);
-
- //CAN
- canPort.frequency(1000000); //Bit Rate:1MHz
- canPort.attach(CANdataRX,CAN::RxIrq);
- int node1 = 1; //CAN node Setting
- int node2 = 2;
- int node3 = 3;
- int node4 = 4;
-
- //エンコーダ関係
- int ActPos = 0;
- int Init_Pos = 0;
-
- //超音波センサ関係パラメータ
- int dist1,dist2,dist3,dist4;
-
- //PID制御関係
- //角度調整パラメータ
+ //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);
+
+ //トルク設定
+ 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 ride_count = 0;
+
+ int X_DIST_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) {
+ ActPos -= 0x1000000;//16進数でのマイナス処理 16^7 = 16,777,216 = 0x1000000
+ }
+ Init_Pos = ActPos;//起動時の角度を保存
+ t.reset();
+ t.start();
+ cr_t.reset();
- #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);
- //-------------起動時に必ず送信---------------
- //オペレーティングモードを送信
- sendOPModeT(node1);
- 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);
-
- //トルク設定
- 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 ride_count = 0;
+ //set_MODE_T();
+
+ printf("\nstart\r\n");
+ LED = 0b1111;
+
+ while(1) {
+
+ Time = t.read();
+ cr_time = cr_t.read();
+
+ //pc.printf("state_1:%d state_2:%d\r\n",state_1,state_2);
+ pc.printf("state_1:%d state_2:%d ACT:%d TMP:%d \r\n",state_1,state_2,dist3,X_DIST_TMP);
+ readActPos(1);
+ ActPos=canmsgRx.data[4]+canmsgRx.data[5]*256+canmsgRx.data[6]*65536;
+ if(ActPos > 8388608) {
+ ActPos -= 0x1000000;//16進数でのマイナス処理 16^7 = 16,777,216 = 0x1000000
+ //printf("check\r\n");
+ }
+ dist3 = get_cm_n(u3, 5);
+ dist1 = get_cm_n(u1, 5);
+ dist2 = get_cm_n(u2, 5);
+
+ /*--------------------------*/
+ //
+ 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++;
+ LED = 0b0111;
+ state_1 = 1;
+ } else if(ActPos > (Init_Pos + KICK)) { //右入力検出
+ ride_count++;
+ LED = LED = 0b1101;;
+ state_1 = 11;
+ } else {
+ set_MODE_T();
+ LED = 0b1111;
+ }
+ }
- int X_DIST_TMP = 0;
- int dist1_ori = 0;
- int dist2_ori = 0;
+ } else if(state_1 == 1) { //前進→壁検出フェーズ
+ if(dist1 < WALL && dist1 >= WALL_MIN) {
+ vel_stop();
+ wait(GETOFF_TIME);
+ LED = 0b1011;
+ state_1 = 2;
+ } 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) {
+ vel_stop();
+ LED = 0b1111;
+ t.reset();
+ t.start();
+ state_1 = 0;
+ wait(1.0);
+ } else {
+ vel_backward_con(RPM_RIDE);
+ }
+ } else if(state_1 == 11) { //右進→壁検出フェーズ
+ if(dist3 < WALL && dist3 >= WALL_MIN) {
+ vel_stop();
+ wait(GETOFF_TIME);
+ LED = 0b1110;
+ state_1 = 12;
+ } 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) {
+ vel_stop();
+ LED = 1111;
+ t.reset();
+ t.start();
+ state_1 = 0;
+ wait(1.0);
+ } else {
+ vel_left(RPM_RIDE);
+ }
- dist1 = 0;
- readActPos(1);
- ActPos=canmsgRx.data[4]+canmsgRx.data[5]*256+canmsgRx.data[6]*65536;
- if(ActPos > 8388608){
- ActPos -= 0x1000000;//16進数でのマイナス処理 16^7 = 16,777,216 = 0x1000000
- }
- Init_Pos = ActPos;//起動時の角度を保存
- t.reset();
- t.start();
- Clean_right.reset();
-
-
- //set_MODE_T();
-
- printf("\nstart\r\n");
- LED = 0b1111;
+ //////////////////////////////
+ } else if(state_1 == 20) { //消毒モード
- while(1){
-
- Time = t.read();
- Clean_right_time = Clean_right.read();
-
- //pc.printf("state_1:%d state_2:%d\r\n",state_1,state_2);
- pc.printf("state_1:%d state_2:%d ACT:%d TMP:%d \r\n",state_1,state_2,dist3,X_DIST_TMP);
- readActPos(1);
- ActPos=canmsgRx.data[4]+canmsgRx.data[5]*256+canmsgRx.data[6]*65536;
- 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);
-
- /*--------------------------*/
- //
- 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++;
- LED = 0b0111;
- state_1 = 1;
- }else if(ActPos > (Init_Pos + KICK)){ //右入力検出
- ride_count++;
- LED = LED = 0b1101;;
- state_1 = 11;
- }else{
- set_MODE_T();
- LED = 0b1111;
+ if(state_2 == 0) {
+ if(dist1 < WALL && dist1 >= WALL_MIN) {
+ X_DIST_TMP = dist3;
+ state_2 = 1;
+ } else {
+ set_ACC(ACC_CLEAN);//加速度設定
+ set_DEC(DEC_CLEAN);//減速度設定
+ set_MODE_V();//速度制御モード送信
+ vel_forward_con(RPM_CLEAN);//前進速度指令
+ }
+ } else if(state_2 == 1) {
+ int dist3_tmp = get_cm_n(u1,5);
+ wait_ms(10);
+ // if(dist3 < WALL && dist3 >= WALL_MIN){
+ // if(dist3_tmp<10){
+ update_pos();
+ if(posX<12000) {
+ gohome();
+ state_2 = 4;
+ } else {
+ set_ACC(ACC_CLEAN);//加速度設定
+ set_DEC(DEC_CLEAN);//減速度設定
+ set_MODE_V();//速度制御モード送信
+ vel_right(RPM_CLEAN);//右進速度指令
+ wait(CLEAN_OFFSET);
+ state_2 = 2;
+ }
+
+ } else if(state_2 == 2) {
+ if(ActPos > -3000) {
+ state_2 = 3;
+ } else {
+ set_ACC(ACC_CLEAN);//加速度設定
+ set_DEC(DEC_CLEAN);//減速度設定
+ set_MODE_V();//速度制御モード送信
+ vel_backward_con(RPM_CLEAN);//後進速度指令
+ }
+ } else if(state_2 == 3) {
+ 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);//右進速度指令
+ wait(CLEAN_OFFSET);
+ state_2 = 0;
+ }
+ } else if(state_2 == 4) {
+ if(ActPos < 3000) {
+ state_2 = 5;
+ } else {
+ vel_left(RPM_CLEAN);
+ }
+ } else if(state_2 == 5) {
+ if(ActPos > -3000) {
+ t.reset();
+ t.start();
+ state_1 = 0;
+ state_2 = 0;
+ } else {
+ vel_backward_con(RPM_CLEAN);
+ }
+ }
}
}
-
- }else if(state_1 == 1){//前進→壁検出フェーズ
- if(dist1 < WALL && dist1 >= WALL_MIN){
- vel_stop();
- wait(GETOFF_TIME);
- LED = 0b1011;
- state_1 = 2;
- }else{
- set_ACC(ACC_RIDE);//加速度設定
- set_DEC(DEC_CLEAN);//減速度設定
- set_MODE_V();//速度制御モード送信
- vel_forward_con(RPM_RIDE);//前進速度指令
+ }
+
+ 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);
}
- }else if(state_1 == 2){//前進からの帰還フェーズ
- if(ActPos > -3000){
- vel_stop();
- LED = 0b1111;
- t.reset();
- t.start();
- state_1 = 0;
- wait(1.0);
- }else{
- vel_backward_con(RPM_RIDE);
+ }
+ 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);
}
- }else if(state_1 == 11){//右進→壁検出フェーズ
- if(dist3 < WALL && dist3 >= WALL_MIN){
- vel_stop();
- wait(GETOFF_TIME);
- LED = 0b1110;
- state_1 = 12;
- }else{
- set_ACC(ACC_RIDE);//加速度設定
- set_DEC(DEC_RIDE);//減速度設定
- set_MODE_V();//速度制御モード送信
- vel_right(RPM_RIDE);//R進速度指令
+ }
+
+
+ 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);
}
- }else if(state_1 == 12){//右進からの帰還フェーズ
- if(ActPos < 3000){
- vel_stop();
- LED = 1111;
- t.reset();
- t.start();
- state_1 = 0;
- wait(1.0);
- }else{
- vel_left(RPM_RIDE);
- }
- }else if(state_1 == 20){//消毒モード
- if(state_2 == 0){
- if(dist1 < WALL && dist1 >= WALL_MIN){
- X_DIST_TMP = dist3;
- state_2 = 1;
- }else{
- set_ACC(ACC_CLEAN);//加速度設定
- set_DEC(DEC_CLEAN);//減速度設定
- set_MODE_V();//速度制御モード送信
- vel_forward_con(RPM_CLEAN);//前進速度指令
- }
- }else if(state_2 == 1){
- 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);//右進速度指令
- wait(CLEAN_OFFSET);
- state_2 = 2;
- }
-
- }else if(state_2 == 2){
- if(ActPos > -3000){
- state_2 = 3;
- }else{
- set_ACC(ACC_CLEAN);//加速度設定
- set_DEC(DEC_CLEAN);//減速度設定
- set_MODE_V();//速度制御モード送信
- vel_backward_con(RPM_CLEAN);//後進速度指令
- }
- }else if(state_2 == 3){
- 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);//右進速度指令
- wait(CLEAN_OFFSET);
- state_2 = 0;
- }
- }else if(state_2 == 4){
- if(ActPos < 3000){
- state_2 = 5;
- }else{
- vel_left(RPM_CLEAN);
- }
- }else if(state_2 == 5){
- if(ActPos > -3000){
- t.reset();
- t.start();
- state_1 = 0;
- state_2 = 0;
- }else{
- vel_backward_con(RPM_CLEAN);
- }
+ }
+
+
+ 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_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_backward_con(int rpmA) {
+ int dis1 = get_cm_n(u1,5);
+ int dis2 = get_cm_n(u2,5);
-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);
- }
-}
+ //速度を指定
+ 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_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_forward(int rpmA) {
-void vel_stop(){
- //速度を指定
- sendTgtVel(1,0);
- sendTgtVel(2,0);
- sendTgtVel(3,0);
- sendTgtVel(4,0);
- //指令値を送信
- for(int i=1;i<= 4;i++){
- sendCtrlEN(i);
- }
-}
+ //速度を指定
+ sendTgtVel(1,rpmA*(-1));
+ sendTgtVel(2,rpmA*(-1));
+ sendTgtVel(3,rpmA);
+ sendTgtVel(4,rpmA);
+ //指令値を送信
+ 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_forward_con(int rpmA) {
+ int dis1 = get_cm_n(u1,5);
+ int dis2 = get_cm_n(u2,5);
-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);
- }
-}
+ //速度を指定
+ 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 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_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_V(){
- sendOPModeV(1);
- sendOPModeV(2);
- sendOPModeV(3);
- sendOPModeV(4);
-}
-
-void set_MODE_T(){
- sendOPModeT(1);
- sendOPModeT(2);
- sendOPModeT(3);
- sendOPModeT(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);
-}
-
-
+ 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);
-}
-
+ 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);
-}
-
-
+ 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);
-}
-
+ 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);
-}
-
+ 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);
-}
-
+ 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);
-}
-
+ 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);
-}
-
-
-
-
+ 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 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;
- }
+ 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;
+ 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
+ canPort.write(canmsgTx);//CANでデータ送信
+ wait(0.01);
+ //send Enable
// pc.printf("Send Enable Command\r\n");
- sendCtrlEN(nodeID);
- wait(0.01);
-}
-
+ sendCtrlEN(nodeID);
+ wait(0.01);
+ }
+
//減速度指定
-void sendProDec(int nodeID,int rpm){
- if(rpm < 0){
- rpm += 0xFFFFFFFF;
- }
+ 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;
+ 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
+ canPort.write(canmsgTx);//CANでデータ送信
+ wait(0.01);
+ //send Enable
// pc.printf("Send Enable Command\r\n");
- sendCtrlEN(nodeID);
- wait(0.01);
-}
-
-
+ 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 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
+
+ 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