gg

Dependencies:   mbed MPU6050 RateLimiter test Math

Files at this revision

API Documentation at this revision

Comitter:
18fmr36
Date:
Fri Mar 06 05:58:45 2020 +0000
Parent:
7:8f914ead7fc0
Commit message:
gg

Changed in this revision

BLDCmotorDriver.lib Show annotated file Show diff for this revision Revisions of this file
RateLimiter.lib Show annotated file Show diff for this revision Revisions of this file
X_NUCLEO_IHM07M1/SPN7Driver.cpp Show annotated file Show diff for this revision Revisions of this file
X_NUCLEO_IHM07M1/SPN7Driver.h Show annotated file Show diff for this revision Revisions of this file
X_NUCLEO_IHM07M1/x_nucleo_ihm07m1_targets.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show diff for this revision Revisions of this file
main2.cpp Show annotated file Show diff for this revision Revisions of this file
diff -r 8f914ead7fc0 -r 7efca5258efb BLDCmotorDriver.lib
--- /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
diff -r 8f914ead7fc0 -r 7efca5258efb RateLimiter.lib
--- /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
diff -r 8f914ead7fc0 -r 7efca5258efb X_NUCLEO_IHM07M1/SPN7Driver.cpp
--- /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>&copy; 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;
+    }    
+}
diff -r 8f914ead7fc0 -r 7efca5258efb X_NUCLEO_IHM07M1/SPN7Driver.h
--- /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>&copy; 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__
diff -r 8f914ead7fc0 -r 7efca5258efb X_NUCLEO_IHM07M1/x_nucleo_ihm07m1_targets.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>&copy; 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_
diff -r 8f914ead7fc0 -r 7efca5258efb main.cpp
--- 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);
-    }
-}
diff -r 8f914ead7fc0 -r 7efca5258efb main2.cpp
--- /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