モータードライバーver4.1 角度フィードバック(クーロン摩擦補償) 角速度フィードバック
Dependencies: mbed QEI PID DBG
MotorDriver_ver4-1.cpp
- Committer:
- shundai
- Date:
- 2019-08-26
- Revision:
- 6:cc51f9e7ef18
- Parent:
- 3:141dc1666df2
File content as of revision 6:cc51f9e7ef18:
/* 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.3f #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 1 #define CCW 0 #define MOTOR_FREUENCY 20000 //[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, 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(); float buf = 0.0f; 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 = 1.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) { /* pc.printf("buf: %f \r\n", buf); if(buf > 0.0f) { SR = ABLE; PHASE = (float)CW; PWM1 = buf; PWM2 = 1.0f; } else if(buf <= 0.0f) { SR = ABLE; PHASE = (float)CCW; PWM1 = -1*buf; PWM2 = 1.0f; } */ //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); //MotorDrive(THETA_FEEDBACK, Motor.target); MotorDrive(OMEGA_FEEDBACK, 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 'p': //motor.setSetPoint(1.5f); //PID target value buf += 0.15f; Motor.target += 30.0f; pc.printf("get 'p' \r\n"); break; case 'l': //motor.setSetPoint(0.5f); //PID target value buf -= 0.15f; Motor.target -= 30.0f; pc.printf("get 'l' \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, 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(target >= 0) { if(abs(Motor.omega) < 0.01f && abs(Motor.theta - target) > 6.0E-1) //モータの回転が停止中 { SR = ABLE; PHASE = (float)CW; } if(abs(Motor.theta - target) > 6.0E-1)//モータ回転中で誤差が0.1以上 { MotorDrive(OMEGA_FEEDBACK, motor2.compute()); } else //誤差の範囲内ならモータのPWMを停止させる { MotorDrive(OMEGA_FEEDBACK, 0.0); } //MotorDrive(OMEGA_FEEDBACK, CCW, abs(motor2.compute())); } else { if(abs(Motor.omega) < 0.01f && abs(Motor.theta - target) > 6.0E-1) //モータの回転が停止時 { SR = ABLE; PHASE = (float)CCW; PWM1 = 0.3f; PWM2 = 1.0f; } if(abs(Motor.theta - target) > 6.0E-1)//モータ回転中で誤差が0.1以上 { MotorDrive(OMEGA_FEEDBACK, motor2.compute()); } else //誤差の範囲内ならモータのPWMを停止させる { MotorDrive(OMEGA_FEEDBACK, 0.0); PWM1 = 0.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: /* if(direction != old_direction) //Prevent sudden turn { motor.setProcessValue(0.0f); if(motor.compute() < 0.05f) { old_direction = direction; } } */ //pc.printf("OMEGA_FEEDBACK "); if(target >= 0.0f) { motor.setSetPoint(abs(target)); motor.setProcessValue(abs(Motor.omega)); //ここにtargetの正負によって回転方向を帰るプログラムを書く SR = ABLE; PHASE = (float)CW; PWM1 = motor.compute(); PWM2 = 1.0f; //pc.printf("%f \r\n", (float)direction); } else { motor.setSetPoint(abs(target)); motor.setProcessValue(abs(Motor.omega)); //ここにtargetの正負によって回転方向を帰るプログラムを書く SR = ABLE; PHASE = (float)CCW; PWM1 = motor.compute(); PWM2 = 1.0f; } 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() */