shundai miyawaki
モータードライバーver4.1 角度フィードバック（クーロン摩擦補償） 角速度フィードバック
Dependencies: mbed QEI PID DBG
Diff: MotorDriver_ver4-1.cpp
- Revision:
- 5:1155a15f978c
- Parent:
- 4:40a764acc4ec
- Child:
- 7:d71bef2faad7
- Child:
- 8:d36996d8b4a0
--- a/MotorDriver_ver4-1.cpp Sun Jul 14 08:38:04 2019 +0000
+++ b/MotorDriver_ver4-1.cpp Sun Aug 25 15:39:22 2019 +0000
@@ -7,81 +7,103 @@
************************************************/
+/*
+
+// target board select checker of stm32f042k6t6
+//未検証
+#if !defined(TARGET_STM32F042K6)
+#error This code is only for NUCLEO-F042K6 or STM32F042K6T6. Reselect a Platform.
+#endif
+*/
+
+// target board select checker of stm32f303k8t6
+//検証済
#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
+// debug switch
+//#define DEBUG //debug ON
+#undef DEBUG //debug OFF
+// preproseccer
#include "dbg.h"
#include "mbed.h"
+#include "PID.h"
+#include "QEI.h"
//#include <CANformat.h>
-#include "PID.h"
-#include "QEI.h"
-#define Kp2 0.9f
-#define Ki2 0.0f
-#define Kd2 0.004f
+// ゲインはモータードライバー固有のパラメータなのでflashに保存する，
+// 初期値としてデフォルトの値をdefineで定義する
+// theta feedback PID gain
+#define theta_Kp 9.0f
+#define theta_Ki 0.0f
+#define theta_Kd 0.00001f
+
+// omega feedback PID gain
+#define omega_Kp 0.05f
+#define omega_Ki 0.05f
+#define omega_Kd 0.0f
+
#define ANGLE_THRESHOLD 2.0f //under 2.0 degree is decreaced
-#define Kp 0.4f
-#define Ki 0.8f
-#define Kd 0.0f
+// CAN PID define
-#define THETA_FEEDBACK 1
-#define OMEGA_FEEDBACK 2
-#define CURRENT_FEEDBACK 3
+#define THETA_FEEDBACK_CCW 10
+#define THETA_FEEDBACK_CW 11
+#define OMEGA_FEEDBACK_CCW 20
+#define OMEGA_FEEDBACK_CW 21
+#define CURRENT_FEEDBACK_CCW 30
+#define CURRENT_FEEDBACK_CW 31
-#define SERIAL_BAUDLATE 115200
-#define ENCODER_PULSE_PER_REVOLUTION 4000 //(1000*4)
-#define CURRENT_SENSOR_OFFSET_COUNT 1000
+#define MOTOR_STOP 0
+#define THETA_FEEDBACK 10
+#define OMEGA_FEEDBACK 20
+#define CURRENT_FEEDBACK 30
+// math value define
#define M_PI 3.141593f
+// constant value
#define ABLE 0
#define DISABLE 1
-
+#define CCW 0
#define CW 1
-#define CCW 0
-#define MOTOR_FREUENCY 10000 //[Hz]
-#define CAN_FREQUENCY 500 //[kHz]
+// threshold value define
#define MOTOR_TARGET_OMEGA_THRESHOLD 0.5f //[deg/s]
-#define MOTOR_TARGET_THETA_THRESHOLD 0.5f //[rad]
+#define MOTOR_TARGET_THETA_THRESHOLD 0.5f //[rpm]
#define MOTOR_TARGET_CURRENT_THRESHOLD 0.1f //[A]
+// stm32f303k8t6 unique serial address
#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]
+// controll value define
+#define PID_CONTROLL_PERIOD 8 //[ms]
#define ENCODER_CONTROLL_PERIOD 8 //[ms]
+#define CAN_SEND_PERIOD 100 //[ms]
+#define MOTOR_FREUENCY 8000 //[Hz]
+#define CAN_FREQUENCY 100 //[kHz]
+#define SERIAL_BAUDLATE 115200 //[bps]
+#define CURRENT_SENSOR_OFFSET_COUNT 1000 // [count]
-//current sensor define
+// 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
+// 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
+#define MOTOR4_ID 786469
+#define MOTOR5_ID 1966107
+#define MOTOR6_ID 1638418
-/*
-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);
@@ -93,9 +115,30 @@
PwmOut PWM2(PA_6);
PwmOut PHASE(PB_0);
DigitalOut SR(PA_7);
-PID thetaPID(Kp, Ki, Kd, PID_CONTROLL_PERIOD*0.001f);
-PID omegaPID(Kp2, Ki2, Kd2, PID_CONTROLL_PERIOD*0.001f);
+PID thetaPID(theta_Kp, theta_Ki, theta_Kd, PID_CONTROLL_PERIOD*0.001f);
+PID omegaPID(omega_Kp, omega_Ki, omega_Kd, PID_CONTROLL_PERIOD*0.001f);
+
+//use encoder define
+
+/*
+ pulse per revolution : 1000
+ phase : A, B, Z
+ voltage : 10.8V - 20.0V
+*/
+//#define ENCODER_TRD_S1000B
+#define ENCODER_E6A2_CW5C
+
+#if defined ENCODER_TRD_S1000B
+#define ENCODER_PULSE_PER_REVOLUTION 4000 //(1000*4)
QEI encoder(PA_3, PA_1, PA_0, ENCODER_PULSE_PER_REVOLUTION, QEI::X4_ENCODING);
+#endif
+
+#if defined ENCODER_E6A2_CW5C
+#define ENCODER_PULSE_PER_REVOLUTION 2000 //(500*4)
+QEI encoder(PA_3, PA_1, NC, ENCODER_PULSE_PER_REVOLUTION, QEI::X4_ENCODING);
+#endif
+
+//AnalogIn CPUtemp(ADC_TEMP);
AnalogIn Current(PB_1);
//CAN define
@@ -113,7 +156,7 @@
struct CANdata MDCAN;
-void CANsend();
+
//void CANtimeout();
//void CANtest();
@@ -126,10 +169,12 @@
//void CurrentFunction();
InterruptIn userButton(PB_5);
Ticker Encoder;
+Ticker SEND;
Timer ENCODER_timer;
struct state
{
+ int PID;
double ThetaTarget;
double OmegaTarget;
double CurrentTarget;
@@ -147,10 +192,20 @@
void ButtonFunction();
void SerialFunction();
void CANread();
+void CANsend();
void CurrentFunction();
+char SerialGet();
// 電流センサーをタイマー割込みにして，規定量を超えたら角速度の目標値を０にする．
+
+
+void loadFlash(uint32_t address, uint8_t *data, uint32_t size);
+void writeFlash(uint32_t address, uint8_t *data, uint32_t size);
+void eraseFlash(uint32_t address);
+
+float buf = 0.0f;
+
int main()
{
// MPU setup
@@ -163,7 +218,7 @@
DBG("Compile Time: %s \r\n", __TIME__);
//Unique ID starting addresses for most of STM32 microcontrollers.
- unsigned long *id = (unsigned long *)0x1FFFF7AC;
+ long *id = (long *)0x1FFFF7AC;
DBG("%d, %d, %d \r\n", id[0], id[1], id[2]);
@@ -203,23 +258,23 @@
while(1)
{
- DBG("DeviceID none define\r\n");
+ DBG("deviceID none define. It does not work!\r\n");
}
}
DBG("deviceID : %d \r\n", MDCAN.deviceID);
-
+
+ // user LED Indicator
myled = 1;
wait(1.0);
myled = 0;
// CAN setup
- can.frequency(500000); //ROS default bps:100kbps -> 500kbps
+ can.frequency(CAN_FREQUENCY*1000); //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);
@@ -230,17 +285,17 @@
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
- thetaPID.setInputLimits(0.0f, 3.68f);
- thetaPID.setOutputLimits(0.0f, 1.0f);
+ thetaPID.setInputLimits(-360.0f*100.0f, 360.0f*100.0f); //[deg]
+ thetaPID.setOutputLimits(-240.0f, 240.0f); //[rpm]
thetaPID.setBias(0.0);
thetaPID.setMode(AUTO_MODE);
+ // PID omega setup
+ omegaPID.setInputLimits(0.0, 240.0f*0.9f); //[rpm]
+ omegaPID.setOutputLimits(0.0f, 1.0f); //[duty]
+ omegaPID.setBias(0.0);
+ omegaPID.setMode(AUTO_MODE);
+
// current sensor setup
for(int i = 0 ; i < CURRENT_SENSOR_OFFSET_COUNT ; i++)
{
@@ -249,33 +304,79 @@
current_bias = current_bias/(double)CURRENT_SENSOR_OFFSET_COUNT;
DBG("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);
+ SEND.attach(CANsend, CAN_SEND_PERIOD*0.001f);
ENCODER_timer.start();
Encoder.attach(Velocity, ENCODER_CONTROLL_PERIOD*0.001f); //Measure rotation speed every 30ms
//NVIC_SetPriority(TIMER3_IRQn, 1);
//initial target
- Motor.ThetaTarget = 90.0f; //initial theta[deg]
- Motor.OmegaTarget = 0.0f; //initial omega[rps]
+ Motor.PID = OMEGA_FEEDBACK; // initial drive mode is stop.
+ Motor.ThetaTarget = 0.0f; // initial theta[deg]
+ Motor.OmegaTarget = 0.0f; // initial omega[rpm]
while(1)
{
+ pc.printf("%f \r\n", buf);
+
+ if(buf > 0.0f)
+ {
+ PWM1 = 0.0f;
+ PWM2 = buf;
+ }
+
+ else if(buf <= 0.0f)
+ {
+ PWM1 = buf;
+ PWM2 = 0.0f;
+ }
+
//Motor.current = ((Current.read()-current_bias)*((float)CURRENT_SENSOR_VCC/2.0f))/(CURRENT_SENSOR_RANGE*0.001f);
//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.ThetaTarget);
- MotorDrive(OMEGA_FEEDBACK, Motor.OmegaTarget);
+ switch(Motor.PID)
+ {
+ case THETA_FEEDBACK:
+
+ //DBG("THETA_FEEDBACK \r\n");
+ MotorDrive(THETA_FEEDBACK, Motor.ThetaTarget);
+ break;
+
+ case OMEGA_FEEDBACK:
+
+ //DBG("OMEGA_FEEDBACK \r\n");
+ MotorDrive(OMEGA_FEEDBACK, Motor.OmegaTarget);
+ break;
+
+ case CURRENT_FEEDBACK:
+
+ //DBG("CURRENT_FEEDBACK \r\n");
+ MotorDrive(CURRENT_FEEDBACK, Motor.CurrentTarget);
+
+ case MOTOR_STOP:
+
+ //DBG("MOTOR_STOP \r\n");
+ MotorDrive(OMEGA_FEEDBACK, 0.0f);
+ break;
+
+ default: // the others PID is stop.
+
+ MotorDrive(OMEGA_FEEDBACK, 0.0f);
+ break;
+ }
+
+ //pc.printf("PID[%d], TT[%6.3f deg], OT[%6.3f rpm], T[%6.3f deg], O[%6.3f rpm] \r\n", Motor.PID, Motor.ThetaTarget, Motor.OmegaTarget, Motor.theta, Motor.omega);
+ pc.printf("T[%6.3f deg], O[%6.3f rpm] \r\n", Motor.theta, Motor.omega);
//DBG("current:%f omega:%f pwm:%f \r\n",current.read(), omega ,thetaPID.compute());
//CurrentFunction();
+
}
}
@@ -285,45 +386,59 @@
{
char data = pc.getc();
DBG("get data:%c \r\n", data);
-
+
switch (data)
{
- case 'q':
-
- Motor.OmegaTarget += 0.5f;
- DBG("get 'q' \r\n");
-
- break;
-
- case 'a':
-
- Motor.OmegaTarget -= 0.5f;
- DBG("get 'a' \r\n");
-
- break;
-
case 'p':
-
- Motor.ThetaTarget += 30.0f;
+ //Motor.PID = THETA_FEEDBACK;
+ //Motor.ThetaTarget += 30.0f;
+ buf += 0.05f;
DBG("get 'p' \r\n");
break;
case 'l':
-
- Motor.ThetaTarget -= 30.0f;
+ //Motor.PID = THETA_FEEDBACK;
+ //Motor.ThetaTarget -= 30.0f;
+ buf -= 0.05f;
DBG("get 'l' \r\n");
break;
+
+ case 'o':
+ Motor.PID = OMEGA_FEEDBACK;
+ Motor.OmegaTarget += 10.0f;
+ DBG("get 'q' \r\n");
+
+ break;
+ case 'k':
+ Motor.PID = OMEGA_FEEDBACK;
+ Motor.OmegaTarget -= 10.0f;
+ DBG("get 'a' \r\n");
+ break;
+
case 'r':
DBG("\r\nSystem Reset! \r\n");
NVIC_SystemReset();
break;
-
+
+ case 'f': //PID free (PWM only)
+
+
+ /*
+ while(1)
+ {
+ SR = ABLE;
+ PHASE = (float)CW;
+ PWM1 = 0.05f;
+ PWM2 = 1.0f;
+ }
+ */
+
default:
DBG("The others key recieve \r\n");
@@ -335,45 +450,57 @@
void ButtonFunction()
{
- Motor.OmegaTarget = 0.0f; //motor stop
-
+ //serial menu printf
DBG("setup open \r\n");
- DBG("1:change the CAN device ID \r\n");
- DBG("2:change the CAN device ID \r\n");
+ DBG("1: Theta Controll\r\n");
+ DBG("2: Omega Controll\r\n");
+ DBG("3: Exit setup menu...\r\n");
+
+ DBG("Send to MD11 test CAN data\r\n");
+
+ MDCAN.sendDATA[0] = 21; // omega FB CCw
+ MDCAN.sendDATA[1] = 60; // 60 rpm
+ MDCAN.sendDATA[2] = 0;
+ MDCAN.sendDATA[3] = 0;
+ MDCAN.sendDATA[4] = 0;
+ MDCAN.sendDATA[5] = 0;
+ MDCAN.sendDATA[6] = 0;
+ MDCAN.sendDATA[7] = 0;
+
+ if(!can.write(CANMessage(12, MDCAN.sendDATA, 8, CANData, CANStandard)))
+ {
+ DBG("CAN send error! \r\n");
+ }
//CANtest(); //CAN通信のテスト信号を簡易的に出す関数(通常は使わない)303K8_sendと同じ機能
//ユーザーボタンを押したらモータを台形加速させるプログラムを作る
+ //ボタン情報を割込みで入力させる関数を用意してボタン情報を入力する．
+
+ //モータが駆動し始める電流を測定する．（パルスが増えるまでＰＷＭを微小増加させる）
+ /*
+ Motor.OmegaTarget = 60.0f; //[rpm]
+
while(1)
{
- if(!pc.readable())
- {
- // pc.scanf("%d", cmd);
- }
-
- else
- {
- DBG("old device ID:\r\n");
- DBG("new device ID:\r\n");
-
- while(1)
- {
-
- }
- }
+ // motor stop
+ PWM1 = 0.0f;
+ PWM2 = 0.0f;
}
+ */
}
void CANread()
{
- //__disable_irq(); //All Interrupt disabled
- //Encoder.detach();
-
-
- if(can.read(msg) != 0
- && MDCAN.deviceID == msg.id
- && msg.format == 0)
+ // 1. when data received.
+ // 2. when CAN data ID equal my ID
+ // 3. when CAN data format is Standerd
+ // 4. Not the same as the one received in the past
+ // 同じものは受信しないプログラムを消したら通信できた446re
+ if(can.read(msg) != 0 &&
+ MDCAN.deviceID == msg.id &&
+ msg.format == 0 )
{
DBG("CAN recieve \r\n");
myled = !myled;
@@ -390,28 +517,35 @@
switch(MDCAN.recvDATA[0]) //check PID
{
- case THETA_FEEDBACK: //MD data send
-
- /*
- int sum = 0;
+ //送られてくるデータはかならず正なので１バイト目のCCW，CWじょうほうを目標値に付け加える．
+ case THETA_FEEDBACK_CW: //MD data send
+
+ Motor.PID = THETA_FEEDBACK;
+ Motor.ThetaTarget = (double)MDCAN.recvDATA[1];
+ DBG("theta CW FB CAN \r\n");
+ break;
+
+ case THETA_FEEDBACK_CCW: //MD data send
- 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");
-
+ Motor.PID = THETA_FEEDBACK;
+ Motor.ThetaTarget = -1.0f*(double)MDCAN.recvDATA[1];
+ DBG("theta CCW FB CAN \r\n");
break;
- case OMEGA_FEEDBACK:
+ case OMEGA_FEEDBACK_CW:
- //現在の分解能は 1 rps
+ // 1 rpm(0.0166 rps)
+ Motor.PID = OMEGA_FEEDBACK;
Motor.OmegaTarget = (double)MDCAN.recvDATA[1];
- DBG("omega FB can \r\n");
+ DBG("omega CW FB CAN \r\n");
+ break;
+
+ case OMEGA_FEEDBACK_CCW:
+
+ // 1 rpm(0.0166 rps)
+ Motor.PID = OMEGA_FEEDBACK;
+ Motor.OmegaTarget = -1.0f*(double)MDCAN.recvDATA[1];
+ DBG("omega CCW FB CAN \r\n");
break;
default:
@@ -421,52 +555,83 @@
break;
}
}
-
- //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()
{
static int pulse; //old pulse
- Motor.omega = ((encoder.getPulses() - pulse)/(double)ENCODER_PULSE_PER_REVOLUTION)/ENCODER_timer.read();
+ Motor.omega = 60.0f*((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
- /*
- CANsend(OMEGA_FEEDBACK, );
+ if(Motor.omega > 0)
+ {
+ MDCAN.sendDATA[0] = OMEGA_FEEDBACK_CW;
+ }
- 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] = ;
+ else
+ {
+ MDCAN.sendDATA[0] = OMEGA_FEEDBACK_CCW;
+ }
+ //pc.printf("%d \r\n", abs(int(Motor.omega)));
+ //MDCAN.sendDATA[1] = abs(int(Motor.omega));
+ //MDCAN.sendDATA[2] = 0;
+ //MDCAN.sendDATA[3] = 0;
+ //MDCAN.sendDATA[4] = 0;
+ //MDCAN.sendDATA[5] = 0;
+ //MDCAN.sendDATA[6] = 0;
+ //MDCAN.sendDATA[7] = 0;
+
+ /*
+ if(!can.write(CANMessage(MDCAN.deviceID*10, MDCAN.sendDATA, 8, CANData, CANExtended)))
+ {
+ DBG("CAN send error! \r\n");
+ }
*/
+
//DBG("omega[rps]:%lf pulse:%d \r\n", Motor.omega, pulse);
}
-void CANsend(int id, int PID)
+void CANsend()
{
+ //CANSenｄがデータ集めて送信（50mstoka）
//__disable_irq();
//Encoder.detach();
//data -> Can data
+ switch (MDCAN.sendDATA[0])
+ {
+ case THETA_FEEDBACK:
+ MDCAN.sendDATA[1] = abs(int(Motor.theta));
+ break;
+
+ case OMEGA_FEEDBACK:
+ MDCAN.sendDATA[1] = abs(int(Motor.omega));
+ break;
+ }
+
+
+ MDCAN.sendDATA[2] = 0;
+ MDCAN.sendDATA[3] = 0;
+ MDCAN.sendDATA[4] = 0;
+ MDCAN.sendDATA[5] = 0;
+ MDCAN.sendDATA[6] = 0;
+ MDCAN.sendDATA[7] = 0;
+
//余りを計算して，端数は削除するとCANデータに変換できる
- if(!can.write(CANMessage(id, MDCAN.sendDATA, 8, CANData, CANExtended)))
+ if(!can.write(CANMessage((char)((int)MDCAN.deviceID-10)*10+100, MDCAN.sendDATA, 8, CANData, CANStandard)))
{
DBG("CAN send error! \r\n");
}
+
+ else
+ {
+ pc.printf("send !!");
+ }
//__enable_irq();
//Encoder.attach(Velocity, ENCODER_CONTROLL_PERIOD); //Measure rotation speed every 30ms
@@ -487,30 +652,31 @@
//current feedback(mode 3)
//static int old_direction;
- static double old_target;
+ //static double old_target;
switch (mode)
{
case THETA_FEEDBACK:
- DBG("THETA_FEEDBACK!! \r\n");
+ //DBG("THETA_FEEDBACK!! \r\n");
- omegaPID.setSetPoint(target);
- omegaPID.setProcessValue(Motor.theta);
+ thetaPID.setSetPoint(target);
+ thetaPID.setProcessValue(Motor.theta);
if(target >= 0)
{
+ // Friction compensation
if(abs(Motor.omega) < 0.01f && abs(Motor.theta - target) > ANGLE_THRESHOLD) //モータの回転が停止中
{
- SR = ABLE;
- PHASE = (float)CW;
- PWM1 = 0.3f;
- PWM2 = 1.0f;
+ //SR = ABLE;
+ //PHASE = (float)CW;
+ //PWM1 = 0.9f;
+ //PWM2 = 1.0f;
}
if(abs(Motor.theta - target) > ANGLE_THRESHOLD)//モータ回転中で誤差が0.1以上
{
- MotorDrive(OMEGA_FEEDBACK, omegaPID.compute());
+ MotorDrive(OMEGA_FEEDBACK, thetaPID.compute());
}
else //誤差の範囲内ならモータのPWMを停止させる
@@ -522,19 +688,19 @@
}
else
- {
-
+ {
+ // Friction compensation
if(abs(Motor.omega) < 0.01f && abs(Motor.theta - target) > ANGLE_THRESHOLD) //モータの回転が停止時
{
- SR = ABLE;
- PHASE = (float)CCW;
- PWM1 = 0.3f;
- PWM2 = 1.0f;
+ //SR = ABLE;
+ //PHASE = (float)CCW;
+ //PWM1 = 0.9f;
+ //PWM2 = 1.0f;
}
if(abs(Motor.theta - target) > ANGLE_THRESHOLD)//モータ回転中で誤差が0.1以上
{
- MotorDrive(OMEGA_FEEDBACK, omegaPID.compute());
+ MotorDrive(OMEGA_FEEDBACK, thetaPID.compute());
}
else //誤差の範囲内ならモータのPWMを停止させる
@@ -549,29 +715,16 @@
case OMEGA_FEEDBACK:
- /*
- if(direction != old_direction) //Prevent sudden turn
- {
- thetaPID.setProcessValue(0.0f);
-
- if(thetaPID.compute() < 0.05f)
- {
- old_direction = direction;
- }
- }
- */
-
-
- DBG("OMEGA_FEEDBACK ");
+ //DBG("OMEGA_FEEDBACK ");
if(target >= 0.0f)
{
- thetaPID.setSetPoint(abs(target));
- thetaPID.setProcessValue(abs(Motor.omega));
+ omegaPID.setSetPoint(abs(target)); //目標値入力
+ omegaPID.setProcessValue(abs(Motor.omega));//センサー入力(絶対値いる？)
//ここにtargetの正負によって回転方向を帰るプログラムを書く
SR = ABLE;
PHASE = (float)CW;
- PWM1 = thetaPID.compute();
+ PWM1 = omegaPID.compute();
PWM2 = 1.0f;
//DBG("%f \r\n", (float)direction);
@@ -579,12 +732,12 @@
else
{
- thetaPID.setSetPoint(abs(target));
- thetaPID.setProcessValue(abs(Motor.omega));
- //ここにtargetの正負によって回転方向を帰るプログラムを書く
+ omegaPID.setSetPoint(abs(target));
+ omegaPID.setProcessValue(abs(Motor.omega));
+ //ここにtargetの正負によって回転方向を帰るプログラムを書く
SR = ABLE;
PHASE = (float)CCW;
- PWM1 = thetaPID.compute();
+ PWM1 = omegaPID.compute();
PWM2 = 1.0f;
}
@@ -604,5 +757,36 @@
}
//old_direction = direction;
- old_target = target;
+ //old_target = target;
+}
+
+void eraseFlash(uint32_t address)
+{
+ FLASH_EraseInitTypeDef erase;
+ erase.TypeErase = FLASH_TYPEERASE_PAGES; /* ページの消去を選択 */
+ erase.PageAddress = address; /* ページの先頭アドレスを指定 */
+ erase.NbPages = 1; /* 消すページの数．今回は1つだけ */
+
+ uint32_t pageError = 0;
+
+ HAL_FLASHEx_Erase(&erase, &pageError); /* HAL_FLASHExの関数で消去 */
}
+
+void writeFlash(uint32_t address, uint8_t *data, uint32_t size)
+{
+ uint32_t *dataWord = (uint32_t*)data; /* 書き込むデータへのポインタ(4Byteごと) */
+ uint32_t sizeWord = (size+3)/4; /* データのサイズ(4Byteで1) */
+
+ HAL_FLASH_Unlock(); /* フラッシュをアンロック */
+ eraseFlash(address); /* 指定したアドレスのページを消去 */
+ do {
+ /* 4Byte(Word)ずつフラッシュに書き込む */
+ HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, address, *dataWord);
+ } while (address+=4, ++dataWord, --sizeWord);
+ HAL_FLASH_Lock(); /* フラッシュをロック */
+}
+
+void loadFlash(uint32_t address, uint8_t *data, uint32_t size)
+{
+ memcpy(data, (uint8_t*)address, size);
+}
\ No newline at end of file