CSPがマップされている元はPosition Mode (PM) です。
https://maxonjapan.com/wp-content/uploads/manual/epos/EPOS_Command_Library.pdf
このモードの中に、
- VCS_ActivateAnalogPositionSetpoint
- VCS_DeactivateAnalogPositionSetpoint
- VCS_DisableAnalogPositionSetpoint
があります。ここでは、EPOS4に用意されている2ポートのアナログ入力を利用します。
アナログ入力を生かす
EPOS Studio 3.7を立ち上げます。WizardsのStartupを起動します。ID=5だけの設定です。
Analog Inputの1と2の両方をGenral purposeに修正して、finishします。これで、電圧入力になります。
5V電源（5番ピン）を利用し、ボリュームをつなげます。これで0～+5V入力ができたと思ったのですが、マイナス側を少し検知してしまいます。アナログ入力自体は±を扱います。
オブジェクト・ディレクトリで電圧を見ると、0V付近から5V付近まで、可変できていることがわかります。1.25Vのような表示ですが、実際に読み出せるのは百倍された整数値です。
コネクタは、EPOS4のX8コネクタにつなぎます。5VはX7コネクタからもらいました。
信号ケーブル8芯（520853）、7芯（520854）は純正を購入しました。片一方はコネクタで、他端はバラ状態です。
ディジタル入力も、使わないならNoneに設定しておきます。
プログラム
1万回のループにしています。何らかのアクシデントで止まることがあり、原因が突き止められていません。ボリュームをゆっくり動かしているだけなら、止まりません。
ボリュームの位置で回転速度が変わります。左右独立しています。
どうしてもエラーが生じたときに再開できないことがあったので、最初と最後にVCS_SendNMTService(keyHandle, nodeId, 130, &errorCode); // RESET_COMMUNICATIONを追加しています。
デフォルトの速度を上げるには、currentValueXを大きくします。
#include <iostream>
#include "Definitions.h"
#include <unistd.h>
void* keyHandle = 0;
char* deviceName = (char*)"EPOS4";
char* protocolStackName = (char*)"CANopen";
char* interfaceName = (char*)"CAN_mcp251x 0";
char* portName = (char*)"CAN0";
uint32_t errorCode = 0;
uint16_t nodeId = 5;
uint32_t ProfileVelocity = 1000;
uint32_t ProfileAcceleration = 1000;
uint32_t ProfileDeceleration = 1000;
int32_t PositionIs = 0;
long TargetPosition = 0;
uint32_t NbOfBytesWritten = 0;
uint32_t NbOfBytesRead = 0;
int32_t pPositionMust = 0;
void* p1Data = 0;
void* p2Data = 0;
uint32_t moves(uint16_t nodeId, int32_t pPositionMust){
VCS_SetPositionMust(keyHandle, nodeId, pPositionMust, &errorCode);
return errorCode;
}
uint32_t homing(uint16_t nodeId){
VCS_ActivateHomingMode(keyHandle, nodeId, &errorCode);
VCS_FindHome(keyHandle, nodeId, 37, &errorCode); // Actual position
VCS_StopHoming(keyHandle, nodeId, &errorCode);
return errorCode;
}
uint32_t printPosition(uint16_t nodeId){
VCS_GetPositionIs(keyHandle, nodeId, &PositionIs, &errorCode);
printf("\nID=%d positionIs--- %ld\n", nodeId,PositionIs);
return errorCode;
}
uint32_t Initialisation(uint16_t nodeId){
// enable_state
VCS_SendNMTService(keyHandle, nodeId, 130, &errorCode); // RESET_COMMUNICATION
VCS_ClearFault(keyHandle, nodeId, &errorCode);
VCS_SetEnableState(keyHandle, nodeId, &errorCode);
// Initialisation
VCS_SetDisableState(keyHandle, nodeId, &errorCode);
VCS_SetOperationMode(keyHandle, nodeId, 0x08, &errorCode); // 0x08 (Cyclic Synchronous Position Mode)
long Max_motor_speed = 10000; // inc
VCS_SetObject(keyHandle, nodeId, 0x6080, 0x00, &Max_motor_speed, 4, &NbOfBytesWritten, &errorCode);
// skip Max gear input speed
long Profile_deceleration = 10000; // rpm/s
VCS_SetObject(keyHandle, nodeId, 0x6084, 0x00, &Profile_deceleration, 4, &NbOfBytesWritten, &errorCode);
long Quick_stop_deceleration = 10000; // rpm/s
VCS_SetObject(keyHandle, nodeId, 0x6085, 0x00, &Quick_stop_deceleration, 4, &NbOfBytesWritten, &errorCode);
uint8_t Interpolation_time_period = 100; // 100ms
VCS_SetObject(keyHandle, nodeId, 0x60c2, 0x01, &Interpolation_time_period, 1, &NbOfBytesWritten, &errorCode);
// Nominal torque ;207 mNm µNm
// Motor Rated Torque is mota tekaku toruku
int16_t Torque_offset = 74; // read dictionary
int32_t Position_offset = 0; // inc
//int32_t Max_position_range_limit = 10000;
VCS_SetObject(keyHandle, nodeId, 0x60b2, 0x00, &Torque_offset, 2, &NbOfBytesWritten, &errorCode);
VCS_SetObject(keyHandle, nodeId, 0x60b0, 0x00, &Position_offset, 4, &NbOfBytesWritten, &errorCode);
//VCS_SetObject(keyHandle, nodeId, 0x607b, 0x02, &Max_position_range_limit, 4, &NbOfBytesWritten, &errorCode);
VCS_SetDisableState(keyHandle, nodeId, &errorCode); // Controlword (Shutdown) 0x0006
VCS_SetEnableState(keyHandle, nodeId, &errorCode); // Controlword (Switch on & Enable) 0x000F
VCS_ActivateAnalogPositionSetpoint(keyHandle, nodeId, 1, 0, 32767, &errorCode);
VCS_ActivateAnalogPositionSetpoint(keyHandle, nodeId, 2, 0, 32767, &errorCode);
VCS_EnableAnalogPositionSetpoint(keyHandle, nodeId, &errorCode);
return errorCode;
}
uint32_t Reset_state(uint16_t nodeId){
VCS_ResetDevice(keyHandle, nodeId, &errorCode);
VCS_SetDisableState(keyHandle, nodeId, &errorCode);
VCS_SendNMTService(keyHandle, nodeId, 130, &errorCode); // RESET_COMMUNICATION
return errorCode;
}
int main(){
printf("start EPOS4 CSP\n");
keyHandle = VCS_OpenDevice(deviceName, protocolStackName, interfaceName, portName, &errorCode);
if (keyHandle!=0 && errorCode == 0) {
homing(5);
homing(6);
Initialisation(5);
Initialisation(6);
int32_t oldValue1 = 0;
int32_t oldValue2 = 0;
int32_t currentValue1 = 0;
int32_t currentValue2 = 0;
int32_t addedValue1 = 0;
int32_t addedValue2 = 0;
for (int i=1; i<10000; i++){
VCS_GetObject(keyHandle, nodeId, 0x3160, 0x01, &p1Data, 2, &NbOfBytesRead,&errorCode);
VCS_GetObject(keyHandle, nodeId, 0x3160, 0x02, &p2Data, 2, &NbOfBytesRead,&errorCode);
currentValue1 = (int)p1Data/50;
currentValue2 = (int)p2Data/50;
printf(":%d: A1 currentValue1 %d oldValue1 %d addedValue1 %d\n",i,currentValue1,oldValue1,addedValue1);
printf(":%d: A1 currentValue2 %d oldValue2 %d addedValue2 %d\n",i,currentValue2,oldValue2,addedValue2);
if (currentValue1 - oldValue1 >= 0) {
moves(5,addedValue1 + currentValue1 );
}else{
moves(5,addedValue1 - currentValue1 );
}
if (currentValue2 - oldValue2 >= 0) {
moves(6,-(addedValue2 + currentValue2) );
}else{
moves(6,-(addedValue2 - currentValue2) );
}
oldValue1 = currentValue1;
oldValue2 = currentValue2;
addedValue1 = addedValue1 + currentValue1;
addedValue2 = addedValue2 + currentValue2;
printPosition(5); printPosition(6);
sleep(0.2);
}
printf("\nReset state\n");
Reset_state(5);
Reset_state(6);
}
VCS_CloseDevice(keyHandle, &errorCode);
}
実行が終わった状態です。
目次
- ① ラズパイ＋EPOS4、C++でプログラミング＜準備＞
- ② ラズパイ＋EPOS4、C++でプログラミング＜車体の用意＞
- ③ ラズパイ＋EPOS4、C++でプログラミング＜モータのチューニング＞
- ④ ラズパイ＋EPOS4、C++でプログラミング＜モータのチューニング2＞
- ⑤ ラズパイ＋EPOS4、C++でプログラミング＜ライブラリの準備＞
- ⑥ ラズパイ＋EPOS4、C++でプログラミング＜オブジェクト・ディクショナリを読み出す＞
- ⑦ ラズパイ＋EPOS4、C++でプログラミング＜モータの回転 PPM＞
- ⑧ ラズパイ＋EPOS4、C++でプログラミング＜モータの回転 CSP その1＞ベータ版
- ⑨ ラズパイ＋EPOS4、C++でプログラミング＜モータの回転 CSP その2＞ベータ版