Tomohiro Aoki
/
YUKA_ak_home_aoki1
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
main.cpp
- Committer:
- ngokystk
- Date:
- 2021-12-19
- Revision:
- 0:b4b94eb28093
- Child:
- 1:b2bd1511307e
File content as of revision 0:b4b94eb28093:
//2021/12/18更新
//YUKA本番機用プログラム
//入力切替確認済み
//一定時間の入力なしで動作切替
//ジグザグ動作実装前
//エンコーダ読み取りによる入力
//加減速度調整パラメータ実装
//PID制御による機体角度補正_
#include "mbed.h"
#include "hcsr04.h"
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);
Timer t;
double Time = 0;
#define Standby_Time 10
Serial pc(USBTX, USBRX);
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受信処理
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 rpm = 600;
//各モータ回転数
int rpm1 = rpm; //Velocity Setting[rpm]
int rpm2 = rpm; //Velocity Setting[rpm]
int rpm3 = rpm; //Velocity Setting[rpm]
int rpm4 = rpm; //Velocity Setting[rpm]
//エンコーダ関係
int ActPos = 0;
int Init_Pos = 0;
//超音波センサ関係パラメータ
int max_rpm = rpm + 200;
int min_rpm = rpm -200;
int wall_distance = 0;
int dist1, dist2,dist3, dist4;
//PID制御関係
//角度調整パラメータ
int p1, i1, d1;
int error_before1 = 0;
int error_now1 = 0;
int feedback_val1;
int integral1;
int pid_sum1;
#define DELTA_T1 0.1
#define target_val1 0
#define Kp1 3
#define Ki1 0
#define Kd1 0
/*
//距離調整パラメータ
int p2, i2, d2;
int error_before2 = 0;
int error_now2 = 0;
int feedback_val2;
int integral2;
int pid_sum2;
#define DELTA_T2 0.01
#define target_val2 50
#define Kp2 2.0
#define Ki2 3.0
#define Kd2 3.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);
//-------------------------------------------
/*
sendCtrlHL(node1);
sendCtrlHL(node2);
sendCtrlHL(node3);
sendCtrlHL(node4);
//-------------送信コマンドを選択--------------
if(Serialdata == 'a'){
//左移動
sendTgtVel(node1,rpm*(-1));
sendTgtVel(node2,rpm);
sendTgtVel(node3,rpm);
sendTgtVel(node4,rpm*(-1));
for(int i=1;i<= 4;i++){
sendCtrlEN(i);
}
Serialdata = 0;
fl=1;
wait(0.5);
}
else if(Serialdata == 'd'){
//右移動
sendTgtVel(node1,rpm);
sendTgtVel(node2,rpm*(-1));
sendTgtVel(node3,rpm*(-1));
sendTgtVel(node4,rpm);
for(int i=1;i<= 4;i++){
sendCtrlEN(i);
}
Serialdata = 0;
fl=1;
wait(0.5);
}
else if(Serialdata == 'w'){
//前移動
sendTgtVel(node1,rpm*(-1));
sendTgtVel(node2,rpm*(-1));
sendTgtVel(node3,rpm);
sendTgtVel(node4,rpm);
for(int i=1;i<= 4;i++){
sendCtrlEN(i);
}
Serialdata = 0;
fl=1;
wait(0.5);
}
else if(Serialdata == 's'){
//後移動
sendTgtVel(node1,rpm);
sendTgtVel(node2,rpm);
sendTgtVel(node3,rpm*(-1));
sendTgtVel(node4,rpm*(-1));
for(int i=1;i<= 4;i++){
sendCtrlEN(i);
}
Serialdata = 0;
fl=1;
wait(0.5);
}
else if(Serialdata == 'e'){
//右旋回
sendTgtVel(node1,rpm);
sendTgtVel(node2,rpm);
sendTgtVel(node3,rpm);
sendTgtVel(node4,rpm);
for(int i=1;i<= 4;i++){
sendCtrlEN(i);
}
Serialdata = 0;
fl=1;
wait(0.1);
}
else if(Serialdata == 'q'){
//左旋回
sendTgtVel(node1,rpm*(-1));
sendTgtVel(node2,rpm*(-1));
sendTgtVel(node3,rpm*(-1));
sendTgtVel(node4,rpm*(-1));
for(int i=1;i<= 4;i++){
sendCtrlEN(i);
}
Serialdata = 0;
fl=1;
wait(0.1);
}
*/
//------------------------------------------
//人の入力待ち
/*
while(1){
readActPos(1);
ActPos=canmsgRx.data[4]+canmsgRx.data[5]*256+canmsgRx.data[6]*65536;
if(ActPos > 8388608){
ActPos -= 0x1000000;
printf("check\r\n");
}
printf("ActPos = %d\r\n",ActPos);
wait(0.1);
}
*/
while(1){
ActPos = 0;
Init_Pos = 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();
while(1){
Time = t.read();
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");
}
printf("dsit1 %d\r\n",dist1);
/*--------------------------*/
//前方向に入力があった時
if(ActPos < Init_Pos - 2000){
sendProAcc(1,1000);
sendProAcc(2,1000);
sendProAcc(3,1000);
sendProAcc(4,1000);
sendProDec(1,700);
sendProDec(2,700);
sendProDec(3,700);
sendProDec(4,700);
//速度制御モード送信
sendOPModeV(1);
sendOPModeV(2);
sendOPModeV(3);
sendOPModeV(4);
while(1){
u1.start();
u2.start();
dist1 = u1.get_dist_cm();
dist2 = u2.get_dist_cm();
//速度を指定
sendTgtVel(1,rpm*(-1));
sendTgtVel(2,rpm*(-1));
sendTgtVel(3,rpm);
sendTgtVel(4,rpm);
//指令値を送信
for(int i=1;i<= 4;i++){
sendCtrlEN(i);
}
if(dist1 < 100 && dist1 >= 70){
break;
}
printf("dsit1 %d\r\n",dist1);
}
//速度を指定
sendTgtVel(1,0);
sendTgtVel(2,0);
sendTgtVel(3,0);
sendTgtVel(4,0);
//指令値を送信
for(int i=1;i<= 4;i++){
sendCtrlEN(i);
}
//実行時間
printf("stop!!\r\n");
wait(5.0);
//printf("2\r\n");
//速度を指定
while(1){
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");
}
if(ActPos > -10000){
break;
}
sendTgtVel(1,rpm);
sendTgtVel(2,rpm);
sendTgtVel(3,rpm*(-1));
sendTgtVel(4,rpm*(-1));
//指令値を送信
for(int i=1;i<= 4;i++){
sendCtrlEN(i);
}
//printf("3\r\n");
}
//ブレーキ
sendTgtVel(1,0);
sendTgtVel(2,0);
sendTgtVel(3,0);
sendTgtVel(4,0);
for(int i=1;i<= 4;i++){
sendCtrlEN(i);
}
wait(0.5);
sendOPModeT(1);
sendOPModeT(2);
sendOPModeT(3);
sendOPModeT(4);
dist1 = 0;
wait(3.0);
break;
}
/*--------------------------*/
/*--------------------------*/
//右方向に入力があった時
if(ActPos > Init_Pos + 2000){
sendProAcc(1,1000);
sendProAcc(2,1000);
sendProAcc(3,1000);
sendProAcc(4,1000);
sendProDec(1,1000);
sendProDec(2,1000);
sendProDec(3,1000);
sendProDec(4,1000);
//速度制御モード送信
sendOPModeV(1);
sendOPModeV(2);
sendOPModeV(3);
sendOPModeV(4);
//速度を指定
sendTgtVel(1,rpm);
sendTgtVel(2,rpm*(-1));
sendTgtVel(3,rpm*(-1));
sendTgtVel(4,rpm);
//指令値を送信
for(int i=1;i<= 4;i++){
sendCtrlEN(i);
}
//実行時間
wait(2.0);
//速度を指定
sendTgtVel(1,0);
sendTgtVel(2,0);
sendTgtVel(3,0);
sendTgtVel(4,0);
//指令値を送信
for(int i=1;i<= 4;i++){
sendCtrlEN(i);
}
//実行時間
wait(5.0);
//速度を指定
sendTgtVel(1,rpm*(-1));
sendTgtVel(2,rpm);
sendTgtVel(3,rpm);
sendTgtVel(4,rpm*(-1));
//指令値を送信
for(int i=1;i<= 4;i++){
sendCtrlEN(i);
}
//実行時間
wait(2.0);
//ブレーキ
sendTgtVel(1,0);
sendTgtVel(2,0);
sendTgtVel(3,0);
sendTgtVel(4,0);
for(int i=1;i<= 4;i++){
sendCtrlEN(i);
}
wait(0.5);
sendOPModeT(1);
sendOPModeT(2);
sendOPModeT(3);
sendOPModeT(4);
wait(4.0);
break;
}
/*--------------------------*/
/*--------------------------*/
//一定時間入力がない場合
if(Time > Standby_Time){
sendProAcc(1,5000);
sendProAcc(2,5000);
sendProAcc(3,5000);
sendProAcc(4,5000);
sendProDec(1,5000);
sendProDec(2,5000);
sendProDec(3,5000);
sendProDec(4,5000);
//速度制御モード送信
sendOPModeV(1);
sendOPModeV(2);
sendOPModeV(3);
sendOPModeV(4);
//速度を指定
sendTgtVel(1,rpm);
sendTgtVel(2,rpm);
sendTgtVel(3,rpm);
sendTgtVel(4,rpm);
//指令値を送信
for(int i=1;i<= 4;i++){
sendCtrlEN(i);
}
//実行時間
wait(1.0);
//速度を指定
sendTgtVel(1,rpm*(-1));
sendTgtVel(2,rpm*(-1));
sendTgtVel(3,rpm*(-1));
sendTgtVel(4,rpm*(-1));
//指令値を送信
for(int i=1;i<= 4;i++){
sendCtrlEN(i);
}
//実行時間
wait(1.3);
//ブレーキ
sendTgtVel(1,0);
sendTgtVel(2,0);
sendTgtVel(3,0);
sendTgtVel(4,0);
for(int i=1;i<= 4;i++){
sendCtrlEN(i);
}
wait(0.5);
sendOPModeT(1);
sendOPModeT(2);
sendOPModeT(3);
sendOPModeT(4);
wait(5.0);
break;
}
/*--------------------------*/
}
wait(0.1);
}
/*
//ジグザグ
else if(Serialdata == 'j'){
int turn_count = 0;
p1 = 0;
i1 = 0;
d1 = 0;
error_before1 = 0;
error_now1 = 0;
feedback_val1 = 0;
integral1 = 0;
pid_sum1 = 0;
rpm1 = rpm;
rpm2 = rpm;
rpm3 = rpm;
rpm4 = rpm;
dist1 = 30;
dist2 = 30;
dist3 = 30;
sendProAcc(1,5000);
sendProAcc(2,5000);
sendProAcc(3,5000);
sendProAcc(4,5000);
sendProDec(1,5000);
sendProDec(2,5000);
sendProDec(3,5000);
sendProDec(4,5000);
//正面壁に近づく
while(!(dist1 <30 && dist1 >5)){
printf("phase1\r\n");
u1.start();
u2.start();
dist1 = u1.get_dist_cm();
dist2 = u2.get_dist_cm();
pc.printf("%ld %ld %ld ID1_%ld ID2_%ld ID3_%ld ID_4%ld\r\n",dist1,dist2,dist3,rpm1,rpm2,rpm3,rpm4);
//壁との角度をPID制御
feedback_val1 = (dist1 - dist2);
error_before1 = error_now1;
error_now1 = feedback_val1 - target_val1;
integral1 += (error_now1 + error_before1) / 2.0 * DELTA_T1;
p1 = Kp1 * error_now1;
i1 = Ki1 * integral1;
d1 = Kd1 * (error_now1 - error_before1) / DELTA_T1;
pid_sum1 = (p1 + i1 + d1);
//モータの回転数を更新
rpm1 -= pid_sum1;
rpm2 -= pid_sum1;
rpm3 += pid_sum1;
rpm4 += pid_sum1;
sendTgtVel(node1,rpm1);
sendTgtVel(node2,rpm2);
sendTgtVel(node3,rpm3*(-1));
sendTgtVel(node4,rpm4*(-1));
for(int i=1;i<= 4;i++){
sendCtrlEN(i);
}
//回転数フィルタ
if(rpm1 >= max_rpm){
rpm1 = max_rpm;
sendTgtVel(node1,rpm1*(-1));
sendCtrlEN(1);
}
if(rpm2 >= max_rpm){
rpm2 = max_rpm;
sendTgtVel(node2,rpm2);
sendCtrlEN(2);
}
if(rpm3 >= max_rpm){
rpm3 = max_rpm;
sendCtrlEN(3);
}
if(rpm4 >= max_rpm){
rpm4 = max_rpm;
sendTgtVel(node4,rpm4*(-1));
sendCtrlEN(4);
}
if(rpm1 <= min_rpm){
rpm1 = min_rpm;
sendTgtVel(node1,rpm1*(-1));
sendCtrlEN(1);
}
if(rpm2 <= min_rpm){
rpm2 = min_rpm;
sendTgtVel(node2,rpm2);
sendCtrlEN(2);
}
if(rpm3 <= min_rpm){
rpm3 = min_rpm;
sendTgtVel(node3,rpm3);
sendCtrlEN(3);
}
if(rpm4 <= min_rpm){
rpm4 = min_rpm;
sendTgtVel(node4,rpm4*(-1));
sendCtrlEN(4);
}
if(Serialdata == 'h'){
sendCtrlHL(node1);
sendCtrlHL(node2);
sendCtrlHL(node3);
sendCtrlHL(node4);
}
wait(0.01);
}
//u1,u2が一定以上壁から離れると終了
while(!(turn_count == 4)){
printf("phase2\r\n");
dist3 = 30;
//右端まで移動
//while(dist3 > 20){
while(!(dist3 < 30& dist3 >10)){
printf("phase3\r\n");
u1.start();
u2.start();
u3.start();
dist1 = u3.get_dist_cm();
dist2 = u3.get_dist_cm();
dist3 = u3.get_dist_cm();
pc.printf("%ld %ld %ld ID1_%ld ID2_%ld ID3_%ld ID_4%ld\r\n",dist1,dist2,dist3,rpm1,rpm2,rpm3,rpm4);
//壁との角度をPID制御
feedback_val1 = (dist1 - dist2);
error_before1 = error_now1;
error_now1 = feedback_val1 - target_val1;
integral1 += (error_now1 + error_before1) / 2.0 * DELTA_T1;
p1 = Kp1 * error_now1;
i1 = Ki1 * integral1;
d1 = Kd1 * (error_now1 - error_before1) / DELTA_T1;
pid_sum1 = (p1 + i1 + d1);
//モータの回転数を更新
rpm1 += pid_sum1;
rpm2 -= pid_sum1;
rpm3 -= pid_sum1;
rpm4 += pid_sum1;
//右移動
sendTgtVel(node1,rpm1*(-1));
sendTgtVel(node2,rpm2);
sendTgtVel(node3,rpm3);
sendTgtVel(node4,rpm4*(-1));
for(int i=1;i<= 4;i++){
sendCtrlEN(i);
}
//回転数フィルタ
if(rpm1 >= max_rpm){
rpm1 = max_rpm;
sendTgtVel(node1,rpm1*(-1));
sendCtrlEN(1);
}
if(rpm2 >= max_rpm){
rpm2 = max_rpm;
sendTgtVel(node2,rpm2);
sendCtrlEN(2);
}
if(rpm3 >= max_rpm){
rpm3 = max_rpm;
sendCtrlEN(3);
}
if(rpm4 >= max_rpm){
rpm4 = max_rpm;
sendTgtVel(node4,rpm4*(-1));
sendCtrlEN(4);
}
if(rpm1 <= min_rpm){
rpm1 = min_rpm;
sendTgtVel(node1,rpm1*(-1));
sendCtrlEN(1);
}
if(rpm2 <= min_rpm){
rpm2 = min_rpm;
sendTgtVel(node2,rpm2);
sendCtrlEN(2);
}
if(rpm3 <= min_rpm){
rpm3 = min_rpm;
sendTgtVel(node3,rpm3);
sendCtrlEN(3);
}
if(rpm4 <= min_rpm){
rpm4 = min_rpm;
sendTgtVel(node4,rpm4*(-1));
sendCtrlEN(4);
}
if(Serialdata == 'h'){
sendCtrlHL(node1);
sendCtrlHL(node2);
sendCtrlHL(node3);
sendCtrlHL(node4);
}
wait(0.01);
}
//下に少し移動
printf("phase4\r\n");
sendTgtVel(node1,rpm*(-1));
sendTgtVel(node2,rpm*(-1));
sendTgtVel(node3,rpm);
sendTgtVel(node4,rpm);
for(int i=1;i<= 4;i++){
sendCtrlEN(i);
}
turn_count += 1;
wait(0.5);
//左に移動
while(!(dist3 < 80 && dist3 >60)){
printf("phase5\r\n");
u1.start();
u2.start();
u3.start();
dist1 = u1.get_dist_cm();
dist2 = u2.get_dist_cm();
dist3 = u3.get_dist_cm();
pc.printf("%ld %ld %ld ID1_%ld ID2_%ld ID3_%ld ID_4%ld\r\n",dist1,dist2,dist3,rpm1,rpm2,rpm3,rpm4);
//壁との角度をPID制御
feedback_val1 = (dist1 - dist2);
error_before1 = error_now1;
error_now1 = feedback_val1 - target_val1;
integral1 += (error_now1 + error_before1) / 2.0 * DELTA_T1;
p1 = Kp1 * error_now1;
i1 = Ki1 * integral1;
d1 = Kd1 * (error_now1 - error_before1) / DELTA_T1;
pid_sum1 = (p1 + i1 + d1);
//モータの回転数を更新
rpm1 -= pid_sum1;
rpm2 += pid_sum1;
rpm3 += pid_sum1;
rpm4 -= pid_sum1;
//左移動
sendTgtVel(node1,rpm1);
sendTgtVel(node2,rpm2*(-1));
sendTgtVel(node3,rpm3*(-1));
sendTgtVel(node4,rpm4);
for(int i=1;i<= 4;i++){
sendCtrlEN(i);
}
//回転数フィルタ
if(rpm1 >= max_rpm){
rpm1 = max_rpm;
sendTgtVel(node1,rpm1*(-1));
sendCtrlEN(1);
}
if(rpm2 >= max_rpm){
rpm2 = max_rpm;
sendTgtVel(node2,rpm2);
sendCtrlEN(2);
}
if(rpm3 >= max_rpm){
rpm3 = max_rpm;
sendCtrlEN(3);
}
if(rpm4 >= max_rpm){
rpm4 = max_rpm;
sendTgtVel(node4,rpm4*(-1));
sendCtrlEN(4);
}
if(rpm1 <= min_rpm){
rpm1 = min_rpm;
sendTgtVel(node1,rpm1*(-1));
sendCtrlEN(1);
}
if(rpm2 <= min_rpm){
rpm2 = min_rpm;
sendTgtVel(node2,rpm2);
sendCtrlEN(2);
}
if(rpm3 <= min_rpm){
rpm3 = min_rpm;
sendTgtVel(node3,rpm3);
sendCtrlEN(3);
}
if(rpm4 <= min_rpm){
rpm4 = min_rpm;
sendTgtVel(node4,rpm4*(-1));
sendCtrlEN(4);
}
if(Serialdata == 'h'){
sendCtrlHL(node1);
sendCtrlHL(node2);
sendCtrlHL(node3);
sendCtrlHL(node4);
}
wait(0.01);
}
//下に少し移動
printf("phase6\r\n");
sendTgtVel(node1,rpm*(-1));
sendTgtVel(node2,rpm*(-1));
sendTgtVel(node3,rpm);
sendTgtVel(node4,rpm);
for(int i=1;i<= 4;i++){
sendCtrlEN(i);
}
turn_count += 1;
wait(0.5);
}
if(Serialdata == 'h'){
sendCtrlHL(node1);
sendCtrlHL(node2);
sendCtrlHL(node3);
sendCtrlHL(node4);
break;
}
wait(0.1);
}
}
*/
//-------------------------------------------
}
//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);
}