File content as of revision 8:7efca5258efb:

#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++;
    }
    
}