shundai miyawaki
モータードライバーver4.1 角度フィードバック（クーロン摩擦補償） 角速度フィードバック
Dependencies: mbed QEI PID DBG
Diff: MotorDriver_ver4-1.cpp
- Revision:
- 1:844acc585d3d
- Child:
- 2:50e50ee8e08a
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/MotorDriver_ver4-1.cpp Mon Jul 08 15:13:43 2019 +0000
@@ -0,0 +1,572 @@
+/*
+MotorDriver ver4.1 farmware
+author : shundai miyawaki
+2019/06/14 ~
+*/
+
+#if !defined(TARGET_STM32F303K8)
+#error This code is only for NUCLEO-F303K8 or STM32F303K8T6. Reselect a Platform.
+#endif
+
+#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.5f
+#define Ki2 0.0f
+#define Kd2 0.002f
+
+#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 SERIAL_BAUDLATE 115200
+#define ENCODER_PULSE_PER_REVOLUTION 4000 //(1000*4)
+#define CURRENT_SENSOR_OFFSET_COUNT 1000
+
+#define M_PI 3.141593f
+
+#define ABLE 0
+#define DISABLE 1
+
+#define CW 0
+#define CCW 1
+#define MOTOR_FREUENCY 8000 //[Hz]
+
+#define MOTOR_TARGET_OMEGA_THRESHOLD 0.5f //[deg/s]
+#define MOTOR_TARGET_THETA_THRESHOLD 0.5f //[rad]
+#define MOTOR_TARGET_CURRENT_THRESHOLD 0.1f //[A]
+
+#define uniqueSerialAdd_kl_freescale (unsigned long *)0x40048058
+#define uniqueSerialAddr_stm32 (unsigned long *)0x1FFFF7E8
+#define uniqueSerialAddr uniqueSerialAddr_stm32
+
+//Controll Period
+#define PID_CONTROLL_PERIOD 10 //[ms]
+#define ENCODER_CONTROLL_PERIOD 8 //[ms]
+
+//current sensor define
+#define CURRENT_SENSOR_VCC 3.3 //[V]
+#define CURRENT_SENSOR_RANGE 105 //[mv/A]
+
+
+/*
+readしたらCAN.readDATAを0にする
+read時は割込み禁止で，CAN timeoutの割込みで0のままなら，すべて停止
+*/
+/*
+#define CAN_TIMEOUT 3000 //[ms]
+#define ENCODER_PPR 100.0f
+*/
+
+
+
+//standerd define
+Serial pc(PB_6, PB_7, SERIAL_BAUDLATE);
+DigitalOut myled(LED1); //PB_3
+DigitalIn userSW(PB_5);
+
+//motor define
+PwmOut PWM1(PA_4);
+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);
+QEI encoder(PA_3, PA_1, PA_0, ENCODER_PULSE_PER_REVOLUTION, QEI::X4_ENCODING);
+AnalogIn Current(PB_1);
+
+//CAN define
+CAN can(PA_11, PA_12); //RD(PA_11,D10),TD(PA_12,D2)
+CANMessage msg;
+
+struct CANdata
+{
+ //int timeout;
+ //int deviceID;
+ char recvDATA[8];
+ char sendDATA[8];
+};
+
+struct CANdata MDCAN;
+
+
+void CANsend();
+//void CANtimeout();
+//void CANtest();
+
+
+
+void MotorDrive(int mode, int direction, double target);
+
+//void CurrentFunction();
+
+InterruptIn userButton(PB_5);
+
+Ticker Encoder;
+
+Timer ENCODER_timer;
+
+struct state
+{
+ double target;
+ double theta;
+ double omega;
+ double current;
+};
+
+struct state Motor;
+
+//Current Sensor setup
+double current_bias = 0.0;
+
+void Velocity();
+void ButtonFunction();
+void SerialFunction();
+void CANread();
+
+
+
+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__);
+
+ //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]);
+
+
+ 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();
+
+ // motor signal setup
+ PWM1.period(1.0f/MOTOR_FREUENCY);
+ PWM2.period(1.0f/MOTOR_FREUENCY);
+ PHASE.period(1.0f/MOTOR_FREUENCY);
+
+ PWM1 = 0.0f;
+ PWM2 = 0.0f;
+ PHASE = 0.0f;
+ SR = 0;
+
+ // 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
+
+ // current sensor setup
+ for(int i = 0 ; i < CURRENT_SENSOR_OFFSET_COUNT ; i++)
+ {
+ current_bias += Current.read();
+ }
+
+ current_bias = current_bias/(double)CURRENT_SENSOR_OFFSET_COUNT;
+ pc.printf("current_bias_average: %f \r\n", current_bias);
+
+
+
+ // interrupt start function
+ userButton.mode(PullUp);
+ userButton.fall(ButtonFunction);
+ pc.attach(SerialFunction, Serial::RxIrq);
+ can.attach(CANread, CAN::RxIrq);
+ ENCODER_timer.start();
+ 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]
+
+ 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);
+
+ MotorDrive(THETA_FEEDBACK, CCW, Motor.target);
+
+ /*
+ if(Motor.target >= 0)
+ {
+ MotorDrive(OMEGA_FEEDBACK, CCW, Motor.target);
+ }
+
+ else
+ {
+ MotorDrive(OMEGA_FEEDBACK, CW, Motor.target);
+ }
+ */
+ //pc.printf("current:%f omega:%f pwm:%f \r\n",current.read(), omega ,motor.compute());
+ //CurrentFunction();
+ }
+}
+
+void SerialFunction()
+{
+ if(pc.readable() == 1)
+ {
+ char data = pc.getc();
+ pc.printf("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");
+
+ break;
+
+ case 'a':
+
+ //motor.setSetPoint(0.5f); //PID target value
+
+ Motor.target -= 0.5f;
+ pc.printf("get 'a' \r\n");
+
+ break;
+
+ case 'r':
+
+
+
+ pc.printf("\r\nSystem Reset! \r\n");
+ NVIC_SystemReset();
+
+ break;
+
+ default:
+
+ pc.printf("default \r\n");
+
+ break;
+ }
+ }
+}
+
+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");
+
+ //CANtest(); //CAN通信のテスト信号を簡易的に出す関数(通常は使わない)303K8_sendと同じ機能
+
+ //ユーザーボタンを押したらモータを台形加速させるプログラムを作る
+
+ while(1)
+ {
+ if(!pc.readable())
+ {
+ // pc.scanf("%d", cmd);
+ }
+
+ else
+ {
+ pc.printf("old device ID:\r\n");
+ pc.printf("new device ID:\r\n");
+
+ while(1)
+ {
+
+ }
+ }
+ }
+}
+
+void CANread()
+{
+ __disable_irq(); //All Interrupt disabled
+ Encoder.detach();
+
+
+ if(can.read(msg) != 0)
+ {
+ pc.printf("CAN recieve \r\n");
+ myled = !myled;
+
+ for(int i = 0 ; i < 8 ; i++)
+ {
+ MDCAN.recvDATA[i] = msg.data[i];
+ pc.printf("[%c] ", MDCAN.recvDATA[i]);
+ }
+
+ pc.printf("\r\n");
+
+ switch(MDCAN.recvDATA[1]) //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);
+
+ break;
+
+ case 9: //MD drive
+
+ ///MD4.Drive();
+
+ break;
+
+ default:
+
+ //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);
+}
+
+void Velocity()
+{
+ static int pulse; //old pulse
+
+ Motor.omega = ((encoder.getPulses() - pulse)/(double)ENCODER_PULSE_PER_REVOLUTION)/ENCODER_timer.read();
+ pulse = encoder.getPulses(); //pulse update
+
+ Motor.theta = (encoder.getPulses()/(double)ENCODER_PULSE_PER_REVOLUTION)*360.0f;
+
+ ENCODER_timer.reset(); //timer reset:t=0s
+
+ //pc.printf("omega[rps]:%lf pulse:%d \r\n", Motor.omega, pulse);
+}
+
+void CANsend(int id)
+{
+ __disable_irq();
+ Encoder.detach();
+
+ if(!can.write(CANMessage(id, MDCAN.sendDATA, 8, CANData, CANExtended)))
+ {
+ pc.printf("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();
+ }
+}
+
+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, int direction, double target)
+{
+ //theta feebback(mode 0)
+ //omega feedback(mode 1)
+ //current feedback(mode 2)
+
+ //static int old_direction;
+ static double old_target;
+
+ switch (mode)
+ {
+ case THETA_FEEDBACK:
+
+ pc.printf("THETA_FEEDBACK!! \r\n");
+
+ motor2.setSetPoint(target);
+ motor2.setProcessValue(Motor.theta);
+
+ if(motor2.compute() >= 0)
+ {
+
+ if(abs(Motor.omega) < 0.1f) //モータの回転が停止時
+ {
+ SR = ABLE;
+ PHASE = (float)direction;
+ PWM1 = 0.4f;
+ PWM2 = 1.0f;
+ //MotorDrive(CURRENT_FEEDBACK, CCW, 1.2f);
+ }
+
+
+ else
+ {
+ MotorDrive(OMEGA_FEEDBACK, direction, abs(motor2.compute()));
+ }
+
+ //MotorDrive(OMEGA_FEEDBACK, CCW, abs(motor2.compute()));
+ }
+
+ else
+ {
+
+ if(abs(Motor.omega) < 0.1f) //モータの回転が停止時
+ {
+ SR = ABLE;
+ PHASE = (float)CW;
+ PWM1 = 0.4f;
+ PWM2 = 1.0f;
+ //MotorDrive(CURRENT_FEEDBACK, CW, 1.2f);
+ }
+
+ else
+ {
+ MotorDrive(OMEGA_FEEDBACK, CW, abs(motor2.compute()));
+ }
+
+ //MotorDrive(OMEGA_FEEDBACK, CW, abs(motor2.compute()));
+ }
+
+ /*
+ SR = ABLE;
+ PHASE = (float)direction;
+ PWM1 = motor.compute();
+ PWM2 = 1.0f;
+ */
+ break;
+
+ case OMEGA_FEEDBACK:
+
+
+ pc.printf("OMEGA_FEEDBACK ");
+ /*
+ if(direction != old_direction) //Prevent sudden turn
+ {
+ motor.setProcessValue(0.0f);
+
+ if(motor.compute() < 0.05f)
+ {
+ old_direction = direction;
+ }
+ }
+
+ else
+ {
+ */
+ motor.setSetPoint(abs(target));
+ motor.setProcessValue(abs(Motor.omega));
+ //ここにtargetの正負によって回転方向を帰るプログラムを書く
+ SR = ABLE;
+ PHASE = (float)direction;
+ PWM1 = motor.compute();
+ PWM2 = 1.0f;
+
+ //pc.printf("%f \r\n", (float)direction);
+
+ break;
+
+ case CURRENT_FEEDBACK:
+
+ pc.printf("CURRENT_FEEDBACK UNSELECTABLE!! \r\n");
+
+ break;
+
+ default:
+
+ pc.printf("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