Tatsuya Hiruma
gg
Dependencies: mbed MPU6050 RateLimiter test Math
Revision 8:7efca5258efb, committed 2020-03-06
- Comitter:
- 18fmr36
- Date:
- Fri Mar 06 05:58:45 2020 +0000
- Parent:
- 7:8f914ead7fc0
- Commit message:
- gg
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/BLDCmotorDriver.lib Fri Mar 06 05:58:45 2020 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/18fmr36/code/test/#ec87b823b7f8
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/RateLimiter.lib Fri Mar 06 05:58:45 2020 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/teams/TVZ-Mechatronics-Team/code/RateLimiter/#d735360f91f9
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/X_NUCLEO_IHM07M1/SPN7Driver.cpp Fri Mar 06 05:58:45 2020 +0000
@@ -0,0 +1,162 @@
+/* mbed Microcontroller Library
+* Copyright (c) 2006-2016 ARM Limited
+*
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+* http://www.apache.org/licenses/LICENSE-2.0
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+*/
+
+/**
+ ******************************************************************************
+ * @file SPN7Driver.cpp
+ * @author STMicroelectronics
+ * @brief Implementation of SPN7Driver class
+ ******************************************************************************
+ * @copy
+ *
+ * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
+ * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE
+ * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY
+ * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING
+ * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE
+ * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
+ *
+ * <h2><center>© COPYRIGHT 2016 STMicroelectronics</center></h2>
+ */
+
+// This example is based on the BLDCmotorDriver motor control library
+// by the TVZ Mechatronics Team, University of Applied Sciences Zagreb,
+// Professional Study in Mechatronics:
+// https://developer.mbed.org/teams/TVZ-Mechatronics-Team/code/BLDCmotorDriver/
+
+#include "mbed.h"
+#include "SPN7Driver.h"
+
+typedef enum {
+ ST_BLDC_LOW = 0,
+ ST_BLDC_HIGH,
+ ST_BLDC_OFF
+} st_bldc_status_t;
+
+/**************************************************************************/
+/**
+ @brief Constructor
+ * @param pIN1 Logic input pin IN1 of L6230 chip
+ * @param pIN2 Logic input pin IN2 of L6230 chip
+ * @param pIN3 Logic input pin IN3 of L6230 chip
+ * @param pEN1 Enable channel pin EN1 of L6230 chip
+ * @param pEN2 Enable channel pin EN2 of L6230 chip
+ * @param pEN3 Enable channel pin EN3 of L6230 chip
+ * @param pH1 Hall sensor pin for phase #1 (A) of X-NUCLEO-IHM07M1
+ * @param pH2 Hall sensor pin for phase #2 (B) of X-NUCLEO-IHM07M1
+ * @param pH3 Hall sensor pin for phase #3 (Z) of X-NUCLEO-IHM07M1
+ * @param pFault Fault LED pin of X-NUCLEO-IHM07M1
+*/
+/**************************************************************************/
+SPN7Driver::SPN7Driver(PinName pIN1, PinName pIN2, PinName pIN3,
+ PinName pEN1, PinName pEN2, PinName pEN3,
+ PinName pH1, PinName pH2, PinName pH3,
+ PinName pC1, PinName pC2, PinName pC3,
+ PinName pFault) :
+ BLDCmotorDriver(pIN1, pIN2, pIN3,
+ pEN1, pEN2, pEN3,
+ pH1, pH2, pH3,
+ pC1, pC2, pC3,
+ pFault)
+{
+ // The BLDCmotorDriver class was implemented for half-bridge drivers
+ // so the pin names may be misleading when referring to the L6230 chip.
+ // Get pointers to each input pin and call them IN[x] (logic input)
+ // to be consistent with the terminology used in the L6230 documentation.
+ PwmOut* IN[3] = {&GH_A, &GH_B, &GH_C};
+
+ // Set the switching period of the INx logic input pins (PWM driven)
+ for (int i = 0; i < 3; i++) {
+ IN[i]->period(switchingPeriod);
+ }
+
+ // Set the step commutation function (triggered by the Hall sensors)
+ H1.rise(this, &SPN7Driver::commutation);
+ H2.rise(this, &SPN7Driver::commutation);
+ H3.rise(this, &SPN7Driver::commutation);
+ H1.fall(this, &SPN7Driver::commutation);
+ H2.fall(this, &SPN7Driver::commutation);
+ H3.fall(this, &SPN7Driver::commutation);
+}
+
+/**************************************************************************/
+/**
+ @brief Set duty cycle for motor control
+ * @param dc duty cycle value (>0 clockwise; <0 anti-clockwise)
+*/
+/**************************************************************************/
+void SPN7Driver::setDutyCycle(float dc) {
+ if (dc >= -1 && dc <= 1) {
+ ticker.attach(this, &SPN7Driver::commutation, sampleTime);
+ tempDutyCycle = dc;
+ } else {
+ coast();
+ }
+}
+
+// six-step phase commutation
+void SPN7Driver::commutation()
+{
+ // The BLDCmotorDriver class was implemented for half-bridge drivers
+ // so the pin names may be misleading when referring to the L6230 chip.
+ // Get pointers to each input pin and call them IN[x] (logic input)
+ // to be consistent with the terminology used in the L6230 documentation.
+ PwmOut* IN[3] = {&GH_A, &GH_B, &GH_C};
+ // Get pointers to each enable pin and call them ENx (enable channel)
+ // to be consistent with the terminology used in the L6230 documentation.
+ DigitalOut* EN[3] = {&GL_A, &GL_B, &GL_C};
+
+ st_bldc_status_t tab[6][3] = {
+ {ST_BLDC_OFF, ST_BLDC_LOW, ST_BLDC_HIGH},
+ {ST_BLDC_HIGH, ST_BLDC_LOW, ST_BLDC_OFF},
+ {ST_BLDC_HIGH, ST_BLDC_OFF, ST_BLDC_LOW},
+ {ST_BLDC_OFF, ST_BLDC_HIGH, ST_BLDC_LOW},
+ {ST_BLDC_LOW, ST_BLDC_HIGH, ST_BLDC_OFF},
+ {ST_BLDC_LOW, ST_BLDC_OFF, ST_BLDC_HIGH},
+ };
+
+ dutyCycle = rl.out(tempDutyCycle);
+ int sector = getSector();
+
+ if (dutyCycle == 0) {
+ // Stop the motor
+ coast();
+ return;
+ }
+
+ // Move to next sector (i.e. commute phase)
+ if (dutyCycle > 0 ) {
+ // Clockwise spinning
+
+ // The rows of the phase status table are offset by one when spinning
+ // clockwise so we need to increment the current sector by two steps
+ sector = (sector + 2) % 6;
+
+ } else {
+ // Anti-clockwise spinning
+ sector--;
+ if(sector < 0) sector = 5;
+ }
+
+ // Get the absolute value of the duty cycle for the PWM
+ float d = (dutyCycle > 0) ? dutyCycle : -dutyCycle;
+
+ // Update the logic inputs and the enable pins
+ for (int i = 0; i < 3; i++) {
+ *EN[i] = (tab[sector][i] == ST_BLDC_OFF) ? 0 : 1;
+ *IN[i] = (tab[sector][i] == ST_BLDC_HIGH) ? d : 0;
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/X_NUCLEO_IHM07M1/SPN7Driver.h Fri Mar 06 05:58:45 2020 +0000
@@ -0,0 +1,59 @@
+/* mbed Microcontroller Library
+* Copyright (c) 2006-2016 ARM Limited
+*
+* Licensed under the Apache License, Version 2.0 (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License at
+*
+* http://www.apache.org/licenses/LICENSE-2.0
+*
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+*/
+
+/**
+ ******************************************************************************
+ * @file SPN7Driver.h
+ * @author STMicroelectronics
+ * @brief Header file for SPN7Driver based on BLDCmotorDriver class
+ ******************************************************************************
+ * @copy
+ *
+ * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS
+ * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE
+ * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY
+ * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING
+ * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE
+ * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS.
+ *
+ * <h2><center>© COPYRIGHT 2016 STMicroelectronics</center></h2>
+ */
+
+// This example is based on the BLDCmotorDriver motor control library
+// by the TVZ Mechatronics Team, University of Applied Sciences Zagreb,
+// Professional Study in Mechatronics:
+// https://developer.mbed.org/teams/TVZ-Mechatronics-Team/code/BLDCmotorDriver/
+
+#ifndef __SPN7DRIVER_H__
+#define __SPN7DRIVER_H__
+
+#include "mbed.h"
+#include "BLDCmotorDriver.h"
+
+class SPN7Driver : public BLDCmotorDriver {
+ public:
+ SPN7Driver(PinName pIN1, PinName pIN2, PinName pIN3,
+ PinName pEN1, PinName pEN2, PinName pEN3,
+ PinName pH1, PinName pH2, PinName pH3,
+ PinName pC1, PinName pC2, PinName pC3,
+ PinName pFault);
+ void setDutyCycle(float dutyCycle);
+
+ protected:
+ void commutation();
+};
+
+#endif // __SPN7DRIVER_H__
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/X_NUCLEO_IHM07M1/x_nucleo_ihm07m1_targets.h Fri Mar 06 05:58:45 2020 +0000 @@ -0,0 +1,87 @@ +/* mbed Microcontroller Library +* Copyright (c) 2006-2016 ARM Limited +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*/ + +/** + ****************************************************************************** + * @file x_nucleo_ihm07m1_targets.h + * @author STMicroelectronics + * @brief Header file with pin definitions for X-NUCLEO-IHM07M1 board + ****************************************************************************** + * @copy + * + * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS + * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE + * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY + * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING + * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE + * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. + * + * <h2><center>© COPYRIGHT 2016 STMicroelectronics</center></h2> + */ + +/* Define to prevent from recursive inclusion --------------------------------*/ +#ifndef __X_NUCLEO_IHM07M1_TARGETS_H_ +#define __X_NUCLEO_IHM07M1_TARGETS_H_ + +// Default pin configuration for X-NUCLEO-IHM07M1 with STM32 Nucleo-64 boards + +// Logic input pins +#define P_IN1 PA_8 +#define P_IN2 PA_9 +#define P_IN3 PA_10 + +// Enable channel pins +//#define P_EN1 PC_10 //IHM07 +//#define P_EN2 PC_11 +//#define P_EN3 PC_12 + +#define P_EN1 PA_7 //IHM08 +#define P_EN2 PB_0 +#define P_EN3 PB_1 + +// Hall sensors pins +#define P_HALL1 PA_15 +#define P_HALL2 PB_3 +#define P_HALL3 PB_10 + +// Temperature pin +#define P_TEMP PC_2 + +// Fault LED +#define P_FAULT PB_2 + +// Speed potentiometer +//#define P_SPEED PB_1 //IHM07 +#define P_SPEED PA_4 //IHM + +// Back EMF pins +//#define P_BEMF1 PC_3 //IHM07 +//#define P_BEMF2 PB_0 +//#define P_BEMF3 PA_7 + +#define P_BEMF1 PC_3 //IHM08 +#define P_BEMF2 PC_4 +#define P_BEMF3 PC_5 + +// Current pins +#define P_CURR1 PA_0 +#define P_CURR2 PC_1 +#define P_CURR3 PC_0 + +// Voltage bus pin +#define P_VBUS PA_1 + +#endif // __X_NUCLEO_IHM07M1_TARGETS_H_
--- a/main.cpp Sat Nov 24 00:01:13 2018 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,52 +0,0 @@
-#include "mbed.h"
-#include "MPU6050.h"
-
-DigitalOut myled(LED1);
-MPU6050 mpu(p9,p10);//(SDA,SCL)のピン配置
-
-int main() {
- float filterCoefficient = 0.9; // ローパスフィルターの係数(これは環境によって要調整。1に近づけるほど平滑化の度合いが大きくなる。
- float lowpassValue = 0;
- float highpassValue = 0;
- float speed = 0;//加速度時から算出した速度
- float oldSpeed = 0;//ひとつ前の速度
- float oldAccel = 0;//ひとつ前の加速度
- float difference=0;//変位
- float timespan=0.01;//時間差
- int accel[3];//accelを3つの配列で定義。
- while(1){
- mpu.readAccelData(accel);//加速度の値をaccel[3]に代入
- int x = accel[0]-123;//x軸方向の加速度
- int y = accel[1]+60;//y軸方向の加速度
- int z = accel[2]+1110 ;//z軸方向の加速度
- float X = x*0.000597964111328125;
- float Y = y*0.000597964111328125;
- float Z = z*0.000597964111328125;
- double a = X*X+Y*Y+Z*Z-95.982071137936;
- if (a>0){
- a = sqrt(a);
- }
- if (a<0) {
- a = abs(a);
- a = -sqrt(a);
- }
- //printf("%lf %f %f %f\r\n",a,X,Y,Z);
- // ローパスフィルター(現在の値 = 係数 * ひとつ前の値 ＋ (1 - 係数) * センサの値)
- lowpassValue = lowpassValue * filterCoefficient + a * (1 - filterCoefficient);
- // ハイパスフィルター(センサの値 - ローパスフィルターの値)//
- highpassValue = a - lowpassValue;
-
- // 速度計算(加速度を台形積分する)
- speed = ((highpassValue + oldAccel) * timespan) / 2 + speed;
- oldAccel = highpassValue;
-
- // 変位計算(速度を台形積分する)
- difference = ((speed + oldSpeed) * timespan) / 2 + difference;
- oldSpeed = speed;
-
- //printf(" speed %f difference %f\r\n",speed,difference);//速度と加速度を表示
- //printf("%f,",speed);//速度を表示
- printf("speed %f diference %f\r\n",speed,difference);//速度と変位を表示
- wait(0.01);
- }
-}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main2.cpp Fri Mar 06 05:58:45 2020 +0000
@@ -0,0 +1,718 @@
+#include "mbed.h"
+
+#include "MPU6050.h"
+#include "x_nucleo_ihm07m1_targets.h"
+#include "RateLimiter.h"
+#include "SPN7Driver.h"
+
+
+#include <math.h>
+#include <stdio.h>
+#include <time.h>
+
+
+DigitalOut led1(LED1);
+AnalogIn Curr_ui(PA_0);
+AnalogIn Curr_vi(PC_1);
+AnalogIn Curr_wi(PC_0);
+AnalogIn V_adc(PC_2);//IHM08potentiometer
+AnalogOut aout(PA_4);//IHM08DAC
+
+
+MPU6050 mpu(D14,D15);
+//Serial pc(USBTX, USBRX);
+//Timer timer;
+
+ int diffsector,sectornow,sectorbefore = 0;
+ int c = 0,s;
+ float rpm;
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Instance of the motor driver
+SPN7Driver M(
+ P_IN1, P_IN2, P_IN3, // Logic input pins
+ P_EN1, P_EN2, P_EN3, // Enable channel pins
+ P_HALL1, P_HALL2, P_HALL3, // Hall sensors pins
+ P_CURR1, P_CURR2, P_CURR3,
+ P_FAULT // Fault LED
+ );
+// Pin to check temperature on the X-NUCLEO-IHM07M1 board
+AnalogIn temperature(P_TEMP);
+
+void checkTemperature()
+{
+ if (temperature > 0.55f){
+ printf("Overheating... Turning off now\n\r");
+ M.setDutyCycle(0);
+ M.coast();
+ }
+}
+/*
+void flip()
+ {
+
+ led1 =! led1;
+ sectornow = M.getSector();
+ diffsector = sectornow - sectorbefore;
+ sectorbefore = sectornow;
+ c = c + abs(diffsector);
+ rpm = 2/3*abs(diffsector);
+
+ //printf("%f\t\r\n",rpm);
+
+
+ }
+
+*/
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////
+int main(){
+ //float g[3];
+ //int miri=1000;
+ int accel[3],gyro[3];//accelを3つの配列で定義。
+ float diff,theta,thetab=0,difftheta,thetavel;
+ double a,b;
+ double PI=3.141592;
+ float K=0.1,T=0.005;
+ //float P_CURR1;
+ float dc = 0.0f;
+ int i=0, n=1000;
+ int diffsector,sectornow,sectorbefore = 0;
+ //float t=0.01;
+ float rpm ;
+ int pole=15;
+ Timer t ;
+ Ticker flipper;
+ //float CURR;
+ float Cw,Cv,Cu;
+ float Vr_adc;
+ float th,thb=0;
+ float dt =0.01;
+
+ float LPF_i,LPF_ib=0,LPF_th,LPF_thb=0;
+ float F_thb=0;
+ float ib=0,xgb=0;
+ float e_ib=0,e_thb=0,eib=0;
+ int gam;
+ //float kp=0.05,ki=0.01;
+
+ Ticker ticker;
+ ticker.attach(checkTemperature, 1); // Periodic overheating check
+ /*
+
+ */
+ //led1 = 1;
+ //flipper.attach_us(&flip,1000000.0);
+ //flipper.attach_us(&flip,10000.0);
+ while(1) {
+ //wait(1);
+ mpu.readGyroData(gyro);
+ float xg = gyro[0];
+ float yg = gyro[1];
+ float zg = gyro[2];
+
+ mpu.readAccelData(accel);//加速度の値をaccel[3]に代入
+ //int x = accel[0]-123;//x軸方向の加速度
+ float xa = accel[0];
+ //int y = accel[1]+60;//y軸方向の加速度
+ float ya = accel[1];
+ //int z = accel[2]+1110 ;//z軸方向の加速度
+ float za = accel[2];
+
+ a = ya;
+ b = za;
+ theta = atan2(a,b)*180/PI;
+
+ diff = theta - thetab;
+
+
+
+
+
+
+ //printf("%d,%f,%d,%d,%d,%f\r\n",c,theta,sectornow,sectorbefore,diffsector,rpm);
+////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ //theta1 = fabs(theta);
+ //myled = 1;
+ //wait(0.2);
+ //myled = 0;
+ //wait(0.2);
+
+ //mpu.getGyroRawZ();
+ //mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
+ //writing current accelerometer and gyro position
+ //pc.printf("%d;%d;%d;%d;%d;%d\n",ax,ay,az,gx,gy,gz);
+
+ //printf("gyrz=%f\r\n",mpu.getAccelero());
+ //printf("gyrz=%f,accx=%f\r\n",mpu.getGyroRawZ(),mpu.getAcceleroRawX());
+ //printf("%4d,%4d,%f,%f,%f\r\n",x,y,a,b,theta);
+ //printf("%f\r\n",theta);
+ /*
+ timer.reset();
+ timer.start();
+ mpu.getGyro(g);
+ timer.stop();
+ float t=timer.read();
+ float time=t*miri;
+
+ pc.printf("time= %f ms\n\r",time);
+ */
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /*
+ if(theta < -1 && theta > -30){
+ dc = (theta/30)*0.8+0.1;
+ M.setDutyCycle(dc);
+ }else if(theta < 30 && theta > 1){
+ dc = (theta/30)*0.8+0.1;
+ M.setDutyCycle(dc);
+ }else if(theta <= 1 && theta >= -1){
+ dc = 0.0f;
+ M.setDutyCycle(dc);
+ }else{
+ dc = 0.8f;
+ M.setDutyCycle(dc);
+ }
+ */
+ ////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ //2/5
+
+ float kp_i=0.8,kp_th=0.8,ki_i=0.01,ki_th=0.01;
+ float alpha=0.8,beta=0.8;
+ float e_i,e_th;
+ float F_i,F_th;
+ float angle;
+
+ //LPF_i = alpha * LPF_ib + (1 - alpha) * Curr_vi;
+ //e_i = LPF_i - ib;
+ //ui = Fi*ei;
+ //LPF_th = beta * LPF_thb + (1- beta) * theta ;
+ th = 0.05 * (thb + yg * dt) + 0.95 * theta;
+
+ sectornow = M.getSector();
+ /*
+ diffsector = sectornow - sectorbefore;
+ if(sector=)
+
+
+ if(diffsector==5 || diffsector==-5){
+ diffsector=1;
+ }else if(diffsector==4 || diffsector==-4){
+ diffsector=2;
+ }
+
+
+ sectorbefore = sectornow;
+
+ angle = diffsector*4;
+ c = c + abs(diffsector);
+ rpm = 2/3*abs(diffsector);
+ */
+
+
+
+
+
+ //LPF_th = LPF_th;
+ //e_th = LPF_th-LPF_thb;
+ e_th = th-thb;
+
+ if(th <= -1.0 && th> -15.0){
+ dc=+abs((th/70.0)+4.0/140.0);
+ //dc=abs(th/15)+0.1;
+ }else if(th < 15.0 && th >= 1.0){
+ dc=-abs((th/70.0)+4.0/140.0);
+ //dc=-abs(th/15)-0.1;
+ }else if(th>-1 && th<1){
+ dc=0.0f;
+ //M.coast();
+ }else if(th < -15.0 ){
+ dc=0.3f;
+ }else if(th > 15.0 ){
+ dc=-0.3f;
+ }else{
+ dc=0.0f;
+ }
+
+ //dc = (th/15.0);
+ //dc = 0.3f;
+ M.setDutyCycle(dc);
+
+ Cv = Curr_vi;
+
+ //printf("theta=%f\t\t,LPF_th=%f\t,th=%f\t,e_th=%f\t,dc=%f\t\r\n",theta,LPF_th,th,e_th,dc);
+ //printf("theta=%2.4f\t,th=%2.4f\t,e_th=%2.4f\t,dc=%2.4f\t,Cv=%f\t,rpm=%f\t\r\n",theta,th,e_th,dc,Cv,rpm);
+ printf("theta=%2.4f\t,th=%2.4f\t,e_th=%2.4f\t,dc=%2.4f\t,Cv=%f\t,%d\t,%d\t,%f\t\r\n",theta,th,e_th,dc,Cv,M.getSector(),diffsector,angle);
+ sectorbefore = sectornow;
+ thb=th;
+ LPF_thb=LPF_th;
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /*
+ if(theta <= -4.0 && theta > -20.0){
+ dc = (theta/20.0)*0.5;
+ M.setDutyCycle(dc);
+
+ }else if(theta < 20.0 && theta >= 4.0){
+ dc = (theta/20.0)*0.5;
+ M.setDutyCycle(dc);
+
+ }else if(theta == 0.0){
+ dc = 0.1f;
+ M.setDutyCycle(dc);
+
+ }else if(theta > 0.05 && theta < 4.0){
+ dc = K*(1-exp(-theta1/T));
+ M.setDutyCycle(dc);
+
+ }else if(theta > -4.0 && theta < -0.05){
+ dc = -K*(1-exp(-theta1/T));
+ M.setDutyCycle(dc);
+
+ }else{
+ dc = 0.5f;
+ M.setDutyCycle(dc);
+ }
+ printf("dc=%f,%d,theta=%f\r\n",dc,M.getSector(),theta);
+ */
+////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/*
+ difftheta = theta1-theta ;
+ thetavel = difftheta/0.01;
+
+ //dc = dc + kp*(theta - theta1)+ki*theta;
+
+
+
+
+ if(theta <-0.5 && theta > -30.0){
+ //dc = (theta/30.0)*1.0;
+ dc = kp*(theta - theta1)+ki*theta;
+ M.setDutyCycle(dc);
+ //if(thetavel){}
+
+
+
+
+ }else if(theta < 30.0 && theta >0.5){
+ //dc = (theta/30.0)*1.0;
+ dc = kp*(theta - theta1)+ki*theta;
+ M.setDutyCycle(dc);
+
+ }else if(theta <= 0.5 && theta >= -0.5){
+ dc = 0.0f;
+ M.setDutyCycle(dc);
+
+ }else{
+ dc = 0.5f;
+ M.setDutyCycle(dc);
+ }
+
+ Cw = Curr_wi.read();
+ Vr_adc=V_adc.read();
+
+
+ //printf("dc=%f,%d,theta=%f\r\n",dc,M.getSector(),theta);
+ //printf("dc = %f, %f ,theta=%f\r\n",dc,Cw,theta);
+ //printf("dc = %f, %f , %f ,theta=%f\r\n",dc,V_adc,aout,theta);
+ printf("dc = %f,theta= %f,thetavel = %f\r\n",dc,theta,thetavel);
+ */
+////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ //LPF
+ /*
+ float kp_i=0.8,kp_th=0.8,ki_i=0.01,ki_th=0.01;
+ float alpha=0.8,beta=0.8;
+ float e_i,e_th;
+ float F_i,F_th;
+
+ LPF_i = alpha * LPF_ib + (1 - alpha) * Curr_vi;
+ e_i = LPF_i - ib;
+ //ui = Fi*ei;
+
+ LPF_th = beta * LPF_thb + (1- beta) * xg/16.4 ;
+ LPF_th = LPF_th;
+ e_th = LPF_th-LPF_thb;
+
+ if(theta > -2.0 && theta < 2.0){
+ gam = 0;
+ }else{
+ gam = 1;
+ }
+
+ //if()
+
+ F_i = kp_i * (e_i - e_ib) + ki_i * e_i;
+ F_th = kp_th * (e_th -e_thb) + ki_th * e_th;
+
+
+ dc = gam * F_th *(LPF_th - xgb) + (1 - gam) * F_i*(LPF_i - ib);
+ M.setDutyCycle(dc);
+
+
+ //printf("xg=%f\t,%d\r\n",xg,gam);
+ //printf("theta=%2.4f\t,dc=%1.4f\t\t\t,gam=%d\t,F_i=%f\t,F_th=%f\t,e_i=%f\t,e_th=%f\t\r\n",theta,dc,gam,F_i,F_th,e_i,e_th);
+ printf("theta=%2.4f\t,dc=%1.4f\t\t\t,gam=%d\t,F_th=%f\t,e_th=%f\t,LPF_th=%f\t,xgb=%f\t\r\n",theta,dc,gam,F_th,e_th,LPF_th,xgb);
+
+ ib = Curr_vi;
+ LPF_ib = LPF_i;
+ LPF_thb = LPF_th;
+ xgb = xg;
+ */
+////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+
+
+
+
+
+
+
+
+
+
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /*
+ //test2/1
+ float kp_i=0.8,kp_th=0.8,ki_i=0.01,ki_th=0.01;
+ float alpha=0.8,beta=0.6;
+ float e_i,e_th,e,ei,u;
+ float F_i,F_th,w_th;
+
+ th = 0.05 * (thb + yg * dt) + 0.95 * theta;
+ //LPF_th = alpha * LPF_thb + (1- alpha) * theta ;
+
+ e = 0 - LPF_th;
+ ei = eib + e * 0.01;
+ u = kp_th * e + ki_th * ei;
+
+ e_th = LPF_th-LPF_thb;
+ w_th = e_th/16.4;
+ F_th = F_thb + kp_th * (e_th -e_thb) + ki_th * e_th;
+
+ LPF_i = beta * LPF_ib + (1- beta) * Curr_vi ;
+ e_i = LPF_i-LPF_ib;
+ F_i = kp_i * (e_i -e_ib) + ki_i * e_i;
+ if(e_th < 0.05 && e_th > -0.05){
+ e_th =0;
+ }
+
+ if(th < 0.0 && th > -20.0){
+ if(e_th > -1.5 && e_th < 1.5){
+ if(e_th >0){
+ dc=th/20;
+ M.setDutyCycle(dc);
+ }else{
+ dc=-th/20;
+ M.setDutyCycle(dc);
+ }
+ }else{
+ dc = (e_th/6.0);
+ M.setDutyCycle(dc);
+ }
+ }else if(th < 20.0 && th >0.0){
+ if(e_th > -1.5 && e_th < 1.5){
+ if(e_th >0){
+ dc=th/20;
+ M.setDutyCycle(dc);
+ }else{
+ dc=-th/20;
+ M.setDutyCycle(dc);
+ }
+ }else{
+ dc = (e_th/6.0);
+ M.setDutyCycle(dc);
+ }
+ }else {
+ dc = 0.5f;
+ M.setDutyCycle(dc);
+ }
+
+ eib = ei;
+ e_thb = e_th;
+ LPF_thb = LPF_th;
+ thb=th;
+
+ Cv = Curr_vi;
+ //Vr_adc=V_adc.read();
+ //printf("theta=%f\t,e=%f\t\r\n",theta,e);
+ //printf("theta=%f\t,dc=%f\t,Cv=%f\t\r\n",theta,dc,Cv);
+ //printf("dc=%f,%d,theta=%f\r\n",dc,M.getSector(),theta);
+ printf("LPF_th = %f\t, theta = %f\t,e_th=%f\t,F_th=%f\t,dc=%f\t,u=%f\t\r\n",LPF_th,theta,e_th,F_th,dc,u);
+ //printf("dc = %f, %f , %f ,theta=%f\r\n",dc,V_adc,aout,theta);
+ //printf("dc = %f,theta= %f,thetavel = %f\r\n",dc,theta,thetavel);
+ */
+////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/*
+ //test1/31
+ float kp_i=0.8,kp_th=0.8,ki_i=0.01,ki_th=0.01;
+ float alpha=0.8,beta=0.6;
+ float e_i,e_th;
+ float F_i,F_th,w_th;
+
+ LPF_th = alpha * LPF_thb + (1- alpha) * theta ;
+ e_th = LPF_th-LPF_thb;
+ w_th = e_th/16.4;
+ F_th = F_thb + kp_th * (e_th -e_thb) + ki_th * e_th;
+
+ LPF_i = beta * LPF_ib + (1- beta) * Curr_vi ;
+ e_i = LPF_i-LPF_ib;
+ F_i = kp_i * (e_i -e_ib) + ki_i * e_i;
+
+
+ if(LPF_th < -3.0 && LPF_th > -20.0){
+ if(e_th > -0.5 && e_th < 0.5){
+ dc = (LPF_th/20.0);
+ M.setDutyCycle(dc);
+ }else{
+ dc = abs(F_th);
+ M.setDutyCycle(dc);
+ }
+ }else if(LPF_th < 20.0 && LPF_th >3.0){
+ if(e_th > -0.5 && e_th < 0.5){
+ dc = (LPF_th/20.0);
+ M.setDutyCycle(dc);
+ }else{
+ dc = -abs(F_th);
+ M.setDutyCycle(dc);
+ }
+ }else if(LPF_th <= 3.0 && LPF_th > 0.0){
+ dc = abs(F_th);
+ M.setDutyCycle(dc);
+
+ }else if(LPF_th < 0.0 && LPF_th >= -3.0){
+ dc = -abs(F_th);
+ M.setDutyCycle(dc);
+
+ }else if(LPF_th == 0){
+ dc = 0.0f;
+ M.setDutyCycle(dc);
+
+ }else{
+ dc = 0.5f;
+ M.setDutyCycle(dc);
+ }
+
+ e_thb = e_th;
+ LPF_thb = LPF_th;
+
+
+ Cv = Curr_vi;
+ //Vr_adc=V_adc.read();
+
+ //printf("theta=%f\t,dc=%f\t,Cv=%f\t\r\n",theta,dc,Cv);
+ //printf("dc=%f,%d,theta=%f\r\n",dc,M.getSector(),theta);
+ printf("LPF_th = %f\t, theta = %f\t,e_th=%f\t,F_th=%f\t,dc=%f\t,w=%f\t\r\n",LPF_th,theta,e_th,F_th,dc,w_th);
+ //printf("dc = %f, %f , %f ,theta=%f\r\n",dc,V_adc,aout,theta);
+ //printf("dc = %f,theta= %f,thetavel = %f\r\n",dc,theta,thetavel);
+*/
+////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+ /*
+ //test1/26
+ float kp_i=0.8,kp_th=0.8,ki_i=0.01,ki_th=0.1;
+ float alpha=0.8,beta=0.6;
+ float e_i,e_th;
+ float F_i,F_th;
+
+ LPF_th = alpha * LPF_thb + (1- alpha) * theta ;
+ e_th = LPF_th-LPF_thb;
+ F_th = F_thb + kp_th * (e_th -e_thb) + ki_th * e_th;
+
+ LPF_i = beta * LPF_ib + (1- beta) * Curr_vi ;
+ e_i = LPF_i-LPF_ib;
+ F_i = kp_i * (e_i -e_ib) + ki_i * e_i;
+
+ if(LPF_th < 0 && LPF_th > -20.0){
+ //if(theta <0 && theta > -20.0){
+ //dc = (LPF_th/20.0) - F_th;
+ dc = F_th/2;
+ M.setDutyCycle(dc);
+ //dc = (theta/30.0)*1.0;
+
+ }else if(LPF_th < 20.0 && LPF_th >0){
+ //dc = (LPF_th/20.0) + F_th;
+ //dc = (theta/30.0)*1.0;
+ dc = F_th/2;
+ M.setDutyCycle(dc);
+ }else if(LPF_th == 0){
+ //}else if(LPF_th <= 1.0 && LPF_th >= -1.0){
+ dc = 0.0f;
+ M.setDutyCycle(dc);
+
+ }else{
+ dc = 0.5f;
+ M.setDutyCycle(dc);
+ }
+
+
+ //}
+ else{
+ if(theta <-4.0 && theta > -20.0){
+ dc = (LPF_/20.0) + F_th-0.1;
+ M.setDutyCycle(dc);
+ //dc = (theta/30.0)*1.0;
+
+ }else if(LPF_th < 30.0 && LPF_th >4.0){
+ dc = (LPF_th/20.0) + F_th+0.1;
+ //dc = (theta/30.0)*1.0;
+ M.setDutyCycle(dc);
+
+ }else if(LPF_th <= 0.5 && LPF_th >= -0.5){
+ dc = 0.0f;
+ M.setDutyCycle(dc);
+
+ }else{
+ dc = 0.5f;
+ M.setDutyCycle(dc);
+ }
+ }
+
+
+ e_thb = e_th;
+ LPF_thb = LPF_th;
+
+
+ Cv = Curr_vi;
+ //Vr_adc=V_adc.read();
+
+ //printf("theta=%f\t,dc=%f\t,Cv=%f\t\r\n",theta,dc,Cv);
+ //printf("dc=%f,%d,theta=%f\r\n",dc,M.getSector(),theta);
+ printf("LPF_th = %f\t, theta = %f\t,e_th=%f\t,F_th=%f\t,dc=%f\t\r\n",LPF_th,theta,e_th,F_th,dc);
+ //printf("dc = %f, %f , %f ,theta=%f\r\n",dc,V_adc,aout,theta);
+ //printf("dc = %f,theta= %f,thetavel = %f\r\n",dc,theta,thetavel);
+ */
+////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /*
+ //ok
+ if(theta <-0.5 && theta > -30.0){
+ dc = (theta/30.0)*1.0;
+ M.setDutyCycle(dc);
+
+ }else if(theta < 30.0 && theta >0.5){
+ dc = (theta/30.0)*1.0;
+ M.setDutyCycle(dc);
+
+ }else if(theta <= 0.5 && theta >= -0.5){
+ dc = 0.0f;
+ M.setDutyCycle(dc);
+
+ }else{
+ dc = 0.5f;
+ M.setDutyCycle(dc);
+ }
+
+ Cv = Curr_vi;
+ //Vr_adc=V_adc.read();
+
+
+ //printf("dc=%f,%d,theta=%f\r\n",dc,M.getSector(),theta);
+ printf("dc = %f, %f ,theta=%f\r\n",dc,Cv,theta);
+ //printf("dc = %f, %f , %f ,theta=%f\r\n",dc,V_adc,aout,theta);
+ //printf("dc = %f,theta= %f,thetavel = %f\r\n",dc,theta,thetavel);
+ */
+////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /*
+ t.start();
+ sectornow = M.getSector();
+ diffsector = sectornow - sectorbefore;
+ if(diffsector > 0 || abs(diffsector) == 5){
+ //t.stop();
+ rpm=60.0/t;
+ }else{
+ //t.stop();
+ rpm=0.0;
+ }
+ printf("dc=%f,%d,time=%d,rpm=%f,%d\r\n",dc,M.getSector(),t.read_ms(),rpm,diffsector);
+ //t.reset();
+
+
+
+ //if(diff<20){
+
+ */
+////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /*//duty test1
+ char c = getchar();
+ if((c == 'w') && (dc < 0.9f)) {
+ dc += 0.1f;
+ M.setDutyCycle(dc);
+ }
+ if((c == 's') && (dc > -0.9f)) {
+ dc -= 0.1f;
+ M.setDutyCycle(dc);
+ }
+ */
+////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ //duty test2
+ /*
+ s = getsector();
+ diffsector = sectornow - sectorbefore;
+ count = count + diffsector;
+
+
+
+ sectorbefore = sectornow;
+
+ }
+
+ dc = 0.1;
+ M.setDutyCycle(dc);
+
+
+
+ rpm = 90*sector*60/t;
+
+
+ printf("dc=%f,%d,theta=%f\r\n",dc,M.getSector(),diff);
+ */
+////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /*
+ if(abs(diff)<20 && abs(diff)>0){
+
+ dc = diff/20 * 0.5;
+
+ }else{
+
+ dc = 0.0;
+
+ }
+ */
+
+
+
+
+
+
+ //printf("dc=%f,%d,theta=%f\r\n",dc,M.getSector(),diff);
+ //printf("dc=%f,%d,time=%d,rpm=%f,%d\r\n",dc,M.getSector(),t.read_us(),rpm,diffsector);
+////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+
+
+
+
+ thetab = theta;
+ //sectorbefore = sectornow;
+ i++;
+ }
+
+}
\ No newline at end of file