shundai miyawaki
モータードライバーver4.1 角度フィードバック（クーロン摩擦補償） 角速度フィードバック
Dependencies: mbed QEI PID DBG
Diff: MotorDriver_ver4-1.cpp
- Revision:
- 4:40a764acc4ec
- Parent:
- 3:141dc1666df2
- Child:
- 5:1155a15f978c
--- a/MotorDriver_ver4-1.cpp Mon Jul 08 16:44:35 2019 +0000
+++ b/MotorDriver_ver4-1.cpp Sun Jul 14 08:38:04 2019 +0000
@@ -1,8 +1,11 @@
-/*
+/************************************************
MotorDriver ver4.1 farmware
author : shundai miyawaki
2019/06/14 ~
-*/
+
+Copyright shundai miyawaki. All rights reserved.
+
+************************************************/
#if !defined(TARGET_STM32F303K8)
#error This code is only for NUCLEO-F303K8 or STM32F303K8T6. Reselect a Platform.
@@ -11,30 +14,26 @@
#define DEBUG //debug ON
//#undef DEBUG //debug OFF
-
#include "dbg.h"
#include "mbed.h"
//#include <CANformat.h>
-
-
-
#include "PID.h"
#include "QEI.h"
-#define Kp2 1.3f
+#define Kp2 0.9f
#define Ki2 0.0f
-#define Kd2 0.002f
+#define Kd2 0.004f
+
+#define ANGLE_THRESHOLD 2.0f //under 2.0 degree is decreaced
#define Kp 0.4f
#define Ki 0.8f
#define Kd 0.0f
-#define THETA_FEEDBACK 0
-#define OMEGA_FEEDBACK 1
-#define CURRENT_FEEDBACK 2
-
-//#define hostID 0
+#define THETA_FEEDBACK 1
+#define OMEGA_FEEDBACK 2
+#define CURRENT_FEEDBACK 3
#define SERIAL_BAUDLATE 115200
#define ENCODER_PULSE_PER_REVOLUTION 4000 //(1000*4)
@@ -47,7 +46,8 @@
#define CW 1
#define CCW 0
-#define MOTOR_FREUENCY 8000 //[Hz]
+#define MOTOR_FREUENCY 10000 //[Hz]
+#define CAN_FREQUENCY 500 //[kHz]
#define MOTOR_TARGET_OMEGA_THRESHOLD 0.5f //[deg/s]
#define MOTOR_TARGET_THETA_THRESHOLD 0.5f //[rad]
@@ -64,7 +64,15 @@
//current sensor define
#define CURRENT_SENSOR_VCC 3.3 //[V]
#define CURRENT_SENSOR_RANGE 105 //[mv/A]
+#define CURRENT_LIMIT 1.5 //[A]
+//turning by toukai robocon
+#define MOTOR1_ID 393246
+#define MOTOR2_ID -2147418100
+#define MOTOR3_ID 196631
+#define MOTOR4_ID 0000000004
+#define MOTOR5_ID 0000000005
+#define MOTOR6_ID 0000000006
/*
readしたらCAN.readDATAを0にする
@@ -75,8 +83,6 @@
#define ENCODER_PPR 100.0f
*/
-
-
//standerd define
Serial pc(PB_6, PB_7, SERIAL_BAUDLATE);
DigitalOut myled(LED1); //PB_3
@@ -87,8 +93,8 @@
PwmOut PWM2(PA_6);
PwmOut PHASE(PB_0);
DigitalOut SR(PA_7);
-PID motor(Kp, Ki, Kd, PID_CONTROLL_PERIOD*0.001f);
-PID motor2(Kp2, Ki2, Kd2, PID_CONTROLL_PERIOD*0.001f);
+PID thetaPID(Kp, Ki, Kd, PID_CONTROLL_PERIOD*0.001f);
+PID omegaPID(Kp2, Ki2, Kd2, PID_CONTROLL_PERIOD*0.001f);
QEI encoder(PA_3, PA_1, PA_0, ENCODER_PULSE_PER_REVOLUTION, QEI::X4_ENCODING);
AnalogIn Current(PB_1);
@@ -99,7 +105,7 @@
struct CANdata
{
//int timeout;
- //int deviceID;
+ int deviceID;
char recvDATA[8];
char sendDATA[8];
};
@@ -111,21 +117,22 @@
//void CANtimeout();
//void CANtest();
-
-
+/*
+int mode : select THETA_FEEDBACK(=1) or OMEGA_FEEDBACK(=2)
+double target : enter the input value corresponding to the mode
+*/
void MotorDrive(int mode, double target);
//void CurrentFunction();
-
InterruptIn userButton(PB_5);
-
Ticker Encoder;
-
Timer ENCODER_timer;
struct state
{
- double target;
+ double ThetaTarget;
+ double OmegaTarget;
+ double CurrentTarget;
double theta;
double omega;
double current;
@@ -140,63 +147,99 @@
void ButtonFunction();
void SerialFunction();
void CANread();
+void CurrentFunction();
-
+// 電流センサーをタイマー割込みにして，規定量を超えたら角速度の目標値を０にする．
int main()
{
// MPU setup
- pc.printf("MotorDriver_ver4.1 start! \r\n");
- pc.printf("/** MPU INFO **/ \r\n");
- pc.printf("Target PlatForm: STM32F303K8T6 \r\n");
- pc.printf("System Frequency: %d Hz \r\n", SystemCoreClock);
- pc.printf("Program File: %s \r\n", __FILE__);
- pc.printf("Compile Date: %s \r\n", __DATE__);
- pc.printf("Compile Time: %s \r\n", __TIME__);
+ DBG("MotorDriver_ver4.1 start! \r\n");
+ DBG("/** MPU INFO **/ \r\n");
+ DBG("Target PlatForm: STM32F303K8T6 \r\n");
+ DBG("System Frequency: %d Hz \r\n", SystemCoreClock);
+ DBG("Program File: %s \r\n", __FILE__);
+ DBG("Compile Date: %s \r\n", __DATE__);
+ DBG("Compile Time: %s \r\n", __TIME__);
//Unique ID starting addresses for most of STM32 microcontrollers.
unsigned long *id = (unsigned long *)0x1FFFF7AC;
- pc.printf("%d, %d, %d \r\n", id[0], id[1], id[2]);
+ DBG("%d, %d, %d \r\n", id[0], id[1], id[2]);
+
+ switch (id[0])
+ {
+ case MOTOR1_ID:
+ MDCAN.deviceID = 11;
+ DBG("\r\n ==MOTOR1== \r\n\r\n");
+ break;
+
+ case MOTOR2_ID:
+ MDCAN.deviceID = 12;
+ DBG("\r\n ==MOTOR2== \r\n\r\n");
+ break;
+
+ case MOTOR3_ID:
+ MDCAN.deviceID = 13;
+ DBG("\r\n ==MOTOR3== \r\n\r\n");
+ break;
+ case MOTOR4_ID:
+ MDCAN.deviceID = 14;
+ DBG("\r\n ==MOTOR4== \r\n\r\n");
+ break;
+
+ case MOTOR5_ID:
+ MDCAN.deviceID = 15;
+ DBG("\r\n ==MOTOR5== \r\n\r\n");
+ break;
+
+ case MOTOR6_ID:
+ MDCAN.deviceID = 16;
+ DBG("\r\n ==MOTOR6== \r\n\r\n");
+ break;
+
+ default:
+
+ while(1)
+ {
+ DBG("DeviceID none define\r\n");
+ }
+ }
+ DBG("deviceID : %d \r\n", MDCAN.deviceID);
+
myled = 1;
wait(1.0);
myled = 0;
- //load Flash Data
- int deviceID = 10;
-
// CAN setup
- can.frequency(1000000); //ROS default bps:100kbps -> 500kbps
- //can.filter(deviceID, 0b11111111, CANAny, 0);
- can.reset(); //delete receive buffer
- //can.monitor();
- //can.mode();
+ can.frequency(500000); //ROS default bps:100kbps -> 500kbps
+ can.mode(CAN::Normal);
+ //can.filter(MDCAN.deviceID, 0x0000000F, CANStandard, 14);
+ //can.reset(); //delete receive buffer
+
// motor signal setup
PWM1.period(1.0f/MOTOR_FREUENCY);
PWM2.period(1.0f/MOTOR_FREUENCY);
- PHASE.period(1.0f/MOTOR_FREUENCY);
-
+ PHASE.period(1.0f/MOTOR_FREUENCY);
PWM1 = 0.0f;
PWM2 = 0.0f;
PHASE = 0.0f;
SR = 0;
+
+ // PID position setup
+ omegaPID.setInputLimits(-360.0f, 360.0f);
+ omegaPID.setOutputLimits(-3.68f, 3.68f);
+ omegaPID.setBias(0.0);
+ omegaPID.setMode(AUTO_MODE);
// PID omega setup
- motor.setInputLimits(0.0f, 3.68f); // 0.0rps(PWM:0%) ~ 3.68.rps(Vcc=10.0V)
- motor.setOutputLimits(0.0f, 1.0f);
- motor.setBias(0.0);
- motor.setMode(AUTO_MODE);
- //motor.setSetPoint(1.0f); //PID target value
-
- // PID position setup
- motor2.setInputLimits(-360.0f, 360.0f); // 0.0rps(PWM:0%) ~ 3.68.rps(Vcc=10.0V)
- motor2.setOutputLimits(-3.68f, 3.68f);
- motor2.setBias(0.0);
- motor2.setMode(AUTO_MODE);
- //motor.setSetPoint(1.0f); //PID target value
+ thetaPID.setInputLimits(0.0f, 3.68f);
+ thetaPID.setOutputLimits(0.0f, 1.0f);
+ thetaPID.setBias(0.0);
+ thetaPID.setMode(AUTO_MODE);
// current sensor setup
for(int i = 0 ; i < CURRENT_SENSOR_OFFSET_COUNT ; i++)
@@ -205,8 +248,7 @@
}
current_bias = current_bias/(double)CURRENT_SENSOR_OFFSET_COUNT;
- pc.printf("current_bias_average: %f \r\n", current_bias);
-
+ DBG("current_bias_average: %f \r\n", current_bias);
// interrupt start function
@@ -218,20 +260,21 @@
Encoder.attach(Velocity, ENCODER_CONTROLL_PERIOD*0.001f); //Measure rotation speed every 30ms
//NVIC_SetPriority(TIMER3_IRQn, 1);
- Motor.target = 250.0f; //[deg]
- //Motor.target = 1.0f; //[rps]
-
+ //initial target
+ Motor.ThetaTarget = 90.0f; //initial theta[deg]
+ Motor.OmegaTarget = 0.0f; //initial omega[rps]
+
while(1)
{
//Motor.current = ((Current.read()-current_bias)*((float)CURRENT_SENSOR_VCC/2.0f))/(CURRENT_SENSOR_RANGE*0.001f);
- //pc.printf("motor2.compute : %6.3f motor.compute : %6.3f theta : %6.3f omega[rps] :%6.3f \r\n", motor2.compute(), motor.compute(), Motor.theta, Motor.omega);
- pc.printf("theta:%6.3f omega:%6.3f\r\n", Motor.theta, Motor.omega);
+ //DBG("omegaPID.compute : %6.3f thetaPID.compute : %6.3f theta : %6.3f omega[rps] :%6.3f \r\n", omegaPID.compute(), thetaPID.compute(), Motor.theta, Motor.omega);
+ pc.printf("%6.3f,%6.3f\r\n", Motor.theta, Motor.omega);
- MotorDrive(THETA_FEEDBACK, Motor.target);
- //MotorDrive(OMEGA_FEEDBACK, Motor.target);
+ //MotorDrive(THETA_FEEDBACK, Motor.ThetaTarget);
+ MotorDrive(OMEGA_FEEDBACK, Motor.OmegaTarget);
- //pc.printf("current:%f omega:%f pwm:%f \r\n",current.read(), omega ,motor.compute());
+ //DBG("current:%f omega:%f pwm:%f \r\n",current.read(), omega ,thetaPID.compute());
//CurrentFunction();
}
}
@@ -241,60 +284,49 @@
if(pc.readable() == 1)
{
char data = pc.getc();
- pc.printf("get data:%c \r\n", data);
-
+ DBG("get data:%c \r\n", data);
switch (data)
{
case 'q':
-
- //motor.setSetPoint(1.5f); //PID target value
-
- Motor.target += 0.5f;
- pc.printf("get 'q' \r\n");
+
+ Motor.OmegaTarget += 0.5f;
+ DBG("get 'q' \r\n");
break;
case 'a':
-
- //motor.setSetPoint(0.5f); //PID target value
-
- Motor.target -= 0.5f;
- pc.printf("get 'a' \r\n");
+
+ Motor.OmegaTarget -= 0.5f;
+ DBG("get 'a' \r\n");
break;
case 'p':
- //motor.setSetPoint(1.5f); //PID target value
-
- Motor.target += 30.0f;
- pc.printf("get 'p' \r\n");
+ Motor.ThetaTarget += 30.0f;
+ DBG("get 'p' \r\n");
break;
case 'l':
- //motor.setSetPoint(0.5f); //PID target value
-
- Motor.target -= 30.0f;
- pc.printf("get 'l' \r\n");
+ Motor.ThetaTarget -= 30.0f;
+ DBG("get 'l' \r\n");
break;
case 'r':
-
-
- pc.printf("\r\nSystem Reset! \r\n");
+ DBG("\r\nSystem Reset! \r\n");
NVIC_SystemReset();
break;
default:
- pc.printf("default \r\n");
+ DBG("The others key recieve \r\n");
break;
}
@@ -303,13 +335,11 @@
void ButtonFunction()
{
- motor.setProcessValue(0); //target value = 0
- //int cmd = 0;
-
- pc.printf("setup open \r\n");
-
- pc.printf("1:change the CAN device ID \r\n");
- pc.printf("2:change the CAN device ID \r\n");
+ Motor.OmegaTarget = 0.0f; //motor stop
+
+ DBG("setup open \r\n");
+ DBG("1:change the CAN device ID \r\n");
+ DBG("2:change the CAN device ID \r\n");
//CANtest(); //CAN通信のテスト信号を簡易的に出す関数(通常は使わない)303K8_sendと同じ機能
@@ -324,8 +354,8 @@
else
{
- pc.printf("old device ID:\r\n");
- pc.printf("new device ID:\r\n");
+ DBG("old device ID:\r\n");
+ DBG("new device ID:\r\n");
while(1)
{
@@ -337,62 +367,67 @@
void CANread()
{
- __disable_irq(); //All Interrupt disabled
- Encoder.detach();
+ //__disable_irq(); //All Interrupt disabled
+ //Encoder.detach();
- if(can.read(msg) != 0)
+ if(can.read(msg) != 0
+ && MDCAN.deviceID == msg.id
+ && msg.format == 0)
{
- pc.printf("CAN recieve \r\n");
+ DBG("CAN recieve \r\n");
myled = !myled;
+
+ DBG("recv ID : %d", msg.id);
for(int i = 0 ; i < 8 ; i++)
{
MDCAN.recvDATA[i] = msg.data[i];
- pc.printf("[%c] ", MDCAN.recvDATA[i]);
+ DBG("[%d] ", MDCAN.recvDATA[i]);
}
- pc.printf("\r\n");
+ DBG("\r\n");
- switch(MDCAN.recvDATA[1]) //check PID
+ switch(MDCAN.recvDATA[0]) //check PID
{
- case 0: //MD data send
-
- MDCAN.sendDATA[0] = 1; //PID
- MDCAN.sendDATA[1] = 0; //dataA
- MDCAN.sendDATA[2] = 0; //dataB
- MDCAN.sendDATA[3] = 0; //dataC
- MDCAN.sendDATA[4] = 0; //dataD
- MDCAN.sendDATA[5] = 0; //dataE
- MDCAN.sendDATA[6] = 0; //dataF
- MDCAN.sendDATA[7] = 15;//dataG
-
- //CANsend(hostID);
-
- //pc.printf("reply to host(PID=%d)\r\n",PID);
+ case THETA_FEEDBACK: //MD data send
+
+ /*
+ int sum = 0;
+
+ for(int i = 0 ; i < 8 ; i++)
+ {
+ sum += MDCAN.recvDATA[i];
+ }
+ */
+
+ //現在の分解能は 1 deg
+ Motor.ThetaTarget = (double)MDCAN.recvDATA[1];
+ DBG("theta FB can \r\n");
break;
- case 9: //MD drive
+ case OMEGA_FEEDBACK:
- ///MD4.Drive();
-
+ //現在の分解能は 1 rps
+ Motor.OmegaTarget = (double)MDCAN.recvDATA[1];
+ DBG("omega FB can \r\n");
break;
default:
+
+ DBG("recv CAN PID error \r\n");
- //CAN.recvDATA[1] = 0b00000000;
-
break;
}
}
- motor.setSetPoint((double)MDCAN.recvDATA[1]); //motor target value velocity
- __enable_irq();
- Encoder.attach(Velocity, 0.01); //Measure rotation speed every 10ms
- pc.attach(SerialFunction, Serial::RxIrq);
- userButton.fall(ButtonFunction);
- can.attach(CANread, CAN::RxIrq);
+ //thetaPID.setSetPoint((double)MDCAN.recvDATA[1]); //motor target value velocity
+ //__enable_irq();
+ //Encoder.attach(Velocity, ENCODER_CONTROLL_PERIOD); //Measure rotation speed every 10ms
+ //pc.attach(SerialFunction, Serial::RxIrq);
+ //userButton.fall(ButtonFunction);
+ //can.attach(CANread, CAN::RxIrq);
}
void Velocity()
@@ -405,61 +440,51 @@
Motor.theta = (encoder.getPulses()/(double)ENCODER_PULSE_PER_REVOLUTION)*360.0f;
ENCODER_timer.reset(); //timer reset:t=0s
+ /*
+ CANsend(OMEGA_FEEDBACK, );
- //pc.printf("omega[rps]:%lf pulse:%d \r\n", Motor.omega, pulse);
+ MDCAN.sendDATA[0] = OMEGA_FEEDBACK;
+ MDCAN.sendDATA[1] = ;
+ MDCAN.sendDATA[2] = ;
+ MDCAN.sendDATA[3] = ;
+ MDCAN.sendDATA[4] = ;
+ MDCAN.sendDATA[5] = ;
+ MDCAN.sendDATA[6] = ;
+ MDCAN.sendDATA[7] = ;
+ */
+ //DBG("omega[rps]:%lf pulse:%d \r\n", Motor.omega, pulse);
}
-void CANsend(int id)
+void CANsend(int id, int PID)
{
- __disable_irq();
- Encoder.detach();
+ //__disable_irq();
+ //Encoder.detach();
+ //data -> Can data
+
+ //余りを計算して，端数は削除するとCANデータに変換できる
if(!can.write(CANMessage(id, MDCAN.sendDATA, 8, CANData, CANExtended)))
{
- pc.printf("CAN send error! \r\n");
+ DBG("CAN send error! \r\n");
}
- __enable_irq();
- Encoder.attach(Velocity, 0.03); //Measure rotation speed every 30ms
- pc.attach(SerialFunction, Serial::RxIrq);
- userButton.fall(ButtonFunction);
- can.attach(CANread, CAN::RxIrq);
-}
-
-/*
-void CANtimeout()
-{
- if(MDCAN.recvDATA[2] == 0)
- {
- pc.printf("CAN CONNECTION TIMEOUT \r\n");
- //MotorDrive();
- }
+ //__enable_irq();
+ //Encoder.attach(Velocity, ENCODER_CONTROLL_PERIOD); //Measure rotation speed every 30ms
+ //pc.attach(SerialFunction, Serial::RxIrq);
+ //userButton.fall(ButtonFunction);
+ //can.attach(CANread, CAN::RxIrq);
}
void CurrentFunction()
{
- static int noneDrive_count;
-
-
- pc.printf("current(raw) : %f \r\n",Current.read());
-
- Motor.current = (Current.read() - current_bias);
-
- if(noneDrive_count >= 2)
- {
- CANsend(); //Power capacity lack. check power supply output current
- }
-
- //if(MAX_LIMIT_CURRENT)
-
+
}
-*/
void MotorDrive(int mode, double target)
-{
- //theta feebback(mode 0)
- //omega feedback(mode 1)
- //current feedback(mode 2)
+{
+ //theta feebback(mode 1)
+ //omega feedback(mode 2)
+ //current feedback(mode 3)
//static int old_direction;
static double old_target;
@@ -468,36 +493,38 @@
{
case THETA_FEEDBACK:
- //pc.printf("THETA_FEEDBACK!! \r\n");
+ DBG("THETA_FEEDBACK!! \r\n");
- motor2.setSetPoint(target);
- motor2.setProcessValue(Motor.theta);
+ omegaPID.setSetPoint(target);
+ omegaPID.setProcessValue(Motor.theta);
if(target >= 0)
{
- if(abs(Motor.omega) < 0.01f && abs(Motor.theta - target) > 6.0E-1) //モータの回転が停止中
+ if(abs(Motor.omega) < 0.01f && abs(Motor.theta - target) > ANGLE_THRESHOLD) //モータの回転が停止中
{
SR = ABLE;
PHASE = (float)CW;
+ PWM1 = 0.3f;
+ PWM2 = 1.0f;
}
- if(abs(Motor.theta - target) > 6.0E-1)//モータ回転中で誤差が0.1以上
+ if(abs(Motor.theta - target) > ANGLE_THRESHOLD)//モータ回転中で誤差が0.1以上
{
- MotorDrive(OMEGA_FEEDBACK, motor2.compute());
+ MotorDrive(OMEGA_FEEDBACK, omegaPID.compute());
}
else //誤差の範囲内ならモータのPWMを停止させる
{
MotorDrive(OMEGA_FEEDBACK, 0.0);
- }
-
- //MotorDrive(OMEGA_FEEDBACK, CCW, abs(motor2.compute()));
+ PWM1 = 0.0f;
+ PWM2 = 1.0f;
+ }
}
else
{
- if(abs(Motor.omega) < 0.01f && abs(Motor.theta - target) > 6.0E-1) //モータの回転が停止時
+ if(abs(Motor.omega) < 0.01f && abs(Motor.theta - target) > ANGLE_THRESHOLD) //モータの回転が停止時
{
SR = ABLE;
PHASE = (float)CCW;
@@ -505,27 +532,19 @@
PWM2 = 1.0f;
}
- if(abs(Motor.theta - target) > 6.0E-1)//モータ回転中で誤差が0.1以上
+ if(abs(Motor.theta - target) > ANGLE_THRESHOLD)//モータ回転中で誤差が0.1以上
{
- MotorDrive(OMEGA_FEEDBACK, motor2.compute());
+ MotorDrive(OMEGA_FEEDBACK, omegaPID.compute());
}
else //誤差の範囲内ならモータのPWMを停止させる
{
- MotorDrive(OMEGA_FEEDBACK, 0.0);
+ MotorDrive(OMEGA_FEEDBACK, 0.0);
PWM1 = 0.0f;
- PWM2 = 1.0f;
+ PWM2 = 1.0f;
}
-
- //MotorDrive(OMEGA_FEEDBACK, CW, abs(motor2.compute()));
}
- /*
- SR = ABLE;
- PHASE = (float)direction;
- PWM1 = motor.compute();
- PWM2 = 1.0f;
- */
break;
case OMEGA_FEEDBACK:
@@ -533,9 +552,9 @@
/*
if(direction != old_direction) //Prevent sudden turn
{
- motor.setProcessValue(0.0f);
+ thetaPID.setProcessValue(0.0f);
- if(motor.compute() < 0.05f)
+ if(thetaPID.compute() < 0.05f)
{
old_direction = direction;
}
@@ -543,29 +562,29 @@
*/
- //pc.printf("OMEGA_FEEDBACK ");
+ DBG("OMEGA_FEEDBACK ");
if(target >= 0.0f)
{
- motor.setSetPoint(abs(target));
- motor.setProcessValue(abs(Motor.omega));
+ thetaPID.setSetPoint(abs(target));
+ thetaPID.setProcessValue(abs(Motor.omega));
//ここにtargetの正負によって回転方向を帰るプログラムを書く
SR = ABLE;
PHASE = (float)CW;
- PWM1 = motor.compute();
+ PWM1 = thetaPID.compute();
PWM2 = 1.0f;
- //pc.printf("%f \r\n", (float)direction);
+ //DBG("%f \r\n", (float)direction);
}
else
{
- motor.setSetPoint(abs(target));
- motor.setProcessValue(abs(Motor.omega));
+ thetaPID.setSetPoint(abs(target));
+ thetaPID.setProcessValue(abs(Motor.omega));
//ここにtargetの正負によって回転方向を帰るプログラムを書く
SR = ABLE;
PHASE = (float)CCW;
- PWM1 = motor.compute();
+ PWM1 = thetaPID.compute();
PWM2 = 1.0f;
}
@@ -573,27 +592,17 @@
case CURRENT_FEEDBACK:
- pc.printf("CURRENT_FEEDBACK UNSELECTABLE!! \r\n");
+ DBG("CURRENT_FEEDBACK UNSELECTABLE!! \r\n");
break;
default:
- pc.printf("Function:MotorDriver mode error! \r\n");
+ DBG("Function:MotorDriver mode error! \r\n");
break;
}
-
- //motor.setProcessValue(omega);
-
//old_direction = direction;
old_target = target;
}
-
-
-/*
-void CAN2DATA()
-
-void DATA2CAN()
-*/
\ No newline at end of file