File content as of revision 0:b24f1c7238b2:

/* "PID.h"--- https://developer.mbed.org/users/aberk/code/PID/ */
/* "Servo.h"-- https://developer.mbed.org/users/andrewrussell/code/Servo/file/4c315bcd91b4/Servo.cpp */
// SDFileSystem---1. https://developer.mbed.org/users/mbed_official/code/SDFileSystem/
//                2. https://developer.mbed.org/users/simon/code/SDCardTest/
// SD care---3.3v
// Ctrl+? =>註解

//#include "PID.h"
#include "mbed.h"
#include "Servo.h"
#include "SDFileSystem.h"

//****************************************************************************/
double Command_AngularVel;
double Measure_motor_1;             //PID
double Measure_motor_1_average = 0;
double LastMeasure_motor_1_average = 0;
double Measure_motor_1_total = 0;
int index_measure = 0;
int control_Brake,control_StopRun, control_Y1_CW_CCW, control_Y2_CW_CCW;
double control_PWM_Value;
double control_angVel_Value;
double LastSectionAngle1,LastSectionAngle1_1,LastSectionAngle1_2,LastSectionAngle1_3,LastSectionAngle1_4;
double LastSectionAngle3,LastSectionAngle3_1,LastSectionAngle3_2,LastSectionAngle3_3,LastSectionAngle3_4;
//****************************Encoder declare************************************//
//DigitalIn phaseA( PA_0 ); // phase a of the quadrature encoder
//DigitalIn phaseB( PA_1 ); // phase b of the quadrature encoder
DigitalIn phaseA_1( PA_0 ); // phase a of the quadrature encoder
DigitalIn phaseB_1( PA_1 ); // phase b of the quadrature encoder
DigitalIn phaseA_3( PC_6 ); // phase a of the quadrature encoder
DigitalIn phaseB_3( PC_7 ); // phase b of the quadrature encoder
int encoderClickCount_1    = 0; // hold the signed value corresponding to the number of clicks left or right since last sample
int previousEncoderState_1 = 0; // keep a record of the last actioned sample state of the Qb+Qa outputs for comparison on each interrupt
int encoderClickCount_3    = 0; // hold the signed value corresponding to the number of clicks left or right since last sample
int previousEncoderState_3 = 0; // keep a record of the last actioned sample state of the Qb+Qa outputs for comparison on each interrupt
double angle1 =0,LastAngle1 =0;
double angle3 =0,LastAngle3 =0;
double t=7,t1=0.03;
double SectionTime =0;
double Average_SectionAngle1, Average_SectionAngle3;
double Now_angularVelocity1, Now_angularVelocity3;
double SectionAngle1 =0,SectionAngle3 =0;
double NOWTime=0, LastTime = 0;
double RunTime = 0,lastTime =0;
//****************************Encoder declare************************************//

//DigitalOut StopRun(D15);
//DigitalOut Brake(D14);
//DigitalOut AR(D9);
//DigitalOut Y1_CW_CCW(PC_8);
//DigitalOut Y2_CW_CCW(PC_9);
//Servo Y_PWM(D8);
//DigitalOut stanley_Y(D7);
DigitalOut StopRun(PA_12);
DigitalOut Brake(PA_11);
DigitalOut AR(PB_12);
DigitalOut Y1_CW_CCW(PC_12);
DigitalOut Y2_CW_CCW(PD_2);
Servo Y_PWM(PB_2);
DigitalOut stanley_Y(PB_1);
DigitalOut myled(LED1);
//SDFileSystem sd(D11, D12, D13, D10, "sd"); //  mosi, miso, sclk, cs
SDFileSystem sd( D4, D5, D3, D6, "sd"); //  mosi, miso, sclk, cs
DigitalIn mybutton(USER_BUTTON);

Timer NowTime;
Timer segTime;
int Motor_run(double control_value_PWM ,int control_brake , int control_stopRun, int control_Y1_CW_CCW, int control_Y2_CW_CCW)
{
    //StopRun = control_stopRun;
//    Brake = control_brake;
    StopRun.write(control_stopRun);
    Brake.write(control_brake);
    Y1_CW_CCW.write(control_Y1_CW_CCW);
    Y2_CW_CCW.write(control_Y2_CW_CCW);
    //double Control_value_PWM = 1-control_value_PWM;
    //control_PWM_Value = 0.05;
    //PWM.write(control_PWM_Value);
    Y_PWM.write(control_value_PWM);
    //PWM.write(0.3);

    //wait_ms(1);
    return control_value_PWM;
}

//****************************  1  ************************************//
void quadratureDecoder_1( void )
{
  int  currentEncoderState_1 = (phaseB_1.read() << 1) + phaseA_1.read(); 
  
    if( currentEncoderState_1 == previousEncoderState_1 )
    {
        return;
    }
    
    switch( previousEncoderState_1 )
    {
        case 0:
            if( currentEncoderState_1 == 2 )
            {
                encoderClickCount_1--;
                
            }
            else if( currentEncoderState_1 == 1 )
            {
                encoderClickCount_1++;
                
            }
            break;
            
        case 1:
            if( currentEncoderState_1 == 0 )
            {        
                encoderClickCount_1--;
                
            }
            else if( currentEncoderState_1 == 3 )
            {
                encoderClickCount_1++;
                
            }
            break;
            
        case 2:
            if( currentEncoderState_1 == 3 )
            {
                encoderClickCount_1--;
                
            }
            else if( currentEncoderState_1 == 0 )
            {
                encoderClickCount_1++;
                
            }
            break;
            
        case 3:
            if( currentEncoderState_1 == 1 )
            {
                encoderClickCount_1--;
                
            }
            else if( currentEncoderState_1 == 2 )
            {
                encoderClickCount_1++;
                
            }
            break;

        default:
            break;
    }
    previousEncoderState_1 = currentEncoderState_1;

}
//****************************  3  ************************************//
void quadratureDecoder_3( void )
{
  int  currentEncoderState_3 = (phaseB_3.read() << 1) + phaseA_3.read(); 
  
    if( currentEncoderState_3 == previousEncoderState_3 )
    {
        return;
    }
    
    switch( previousEncoderState_3 )
    {
        case 0:
            if( currentEncoderState_3 == 2 )
            {
                encoderClickCount_3--;
                
            }
            else if( currentEncoderState_3 == 1 )
            {
                encoderClickCount_3++;
                
            }
            break;
            
        case 1:
            if( currentEncoderState_3 == 0 )
            {        
                encoderClickCount_3--;
                
            }
            else if( currentEncoderState_3 == 3 )
            {
                encoderClickCount_3++;
                
            }
            break;
            
        case 2:
            if( currentEncoderState_3 == 3 )
            {
                encoderClickCount_3--;
                
            }
            else if( currentEncoderState_3 == 0 )
            {
                encoderClickCount_3++;
                
            }
            break;
            
        case 3:
            if( currentEncoderState_3 == 1 )
            {
                encoderClickCount_3--;
                
            }
            else if( currentEncoderState_3 == 2 )
            {
                encoderClickCount_3++;
                
            }
            break;

        default:
            break;
    }
    previousEncoderState_3 = currentEncoderState_3;

}
void getAngular( void )
{
    angle1 = (encoderClickCount_1*0.1499)/5; 
    angle3 = (encoderClickCount_3*0.1499)/5; 
    NOWTime = NowTime.read();
    SectionTime = NOWTime - LastTime;
    SectionAngle1 = angle1 - LastAngle1;
    SectionAngle3 = angle3 - LastAngle3;
    Average_SectionAngle1 = (SectionAngle1*0.3 + LastSectionAngle1*0.3 + LastSectionAngle1_1*0.1 + LastSectionAngle1_2*0.1 + LastSectionAngle1_3*0.1 + LastSectionAngle1_4*0.1)/1;
    Average_SectionAngle3 = (SectionAngle3*0.3 + LastSectionAngle3*0.3 + LastSectionAngle3_1*0.1 + LastSectionAngle3_2*0.1 + LastSectionAngle3_3*0.1 + LastSectionAngle3_4*0.1)/1;
    Now_angularVelocity1 = Average_SectionAngle1/(t1);
    Now_angularVelocity3 = Average_SectionAngle3/(t1);
    LastTime = NOWTime;
    LastAngle1 = angle1;
    LastAngle3 = angle3;
    LastSectionAngle1_4 = LastSectionAngle1_3;
    LastSectionAngle1_3 = LastSectionAngle1_2;
    LastSectionAngle1_2 = LastSectionAngle1_1;
    LastSectionAngle1_1 = LastSectionAngle1;
    LastSectionAngle1 = SectionAngle1;
    LastSectionAngle3_4 = LastSectionAngle3_3;
    LastSectionAngle3_3 = LastSectionAngle3_2;
    LastSectionAngle3_2 = LastSectionAngle3_1;
    LastSectionAngle3_1 = LastSectionAngle3;
    LastSectionAngle3 = SectionAngle3;
}
double PWM_commmand_transformation( double  Control_AngVel_Value )
{
    double Control_PWM_Value = 0;
    if( Control_AngVel_Value > 0 )
    {
//        control_Y1_CW_CCW = 0;                     // clockwise:0    counterclockwise:1
//        control_Y2_CW_CCW = 1;
        Control_AngVel_Value = Control_AngVel_Value;
    }
    else if( Control_AngVel_Value < 0 )
    {
//        control_Y1_CW_CCW = 1;                     // clockwise:0    counterclockwise:1
//        control_Y2_CW_CCW = 0;
        Control_AngVel_Value = -Control_AngVel_Value;
    }
    if( Control_AngVel_Value > 190)
    {
        if( Control_AngVel_Value < 270 )
        {
            if( Control_AngVel_Value < 232 )
            {
                Control_PWM_Value =  Control_AngVel_Value/386   ;   //0.5~0.6
            }
            else 
            {
                Control_PWM_Value =  Control_AngVel_Value/386.9;   //0.6~0.7
            }
        }
        else
        {
            if( Control_AngVel_Value < 309 )
            {
               Control_PWM_Value =  Control_AngVel_Value/386.6; //0.7~0.8
            }
            else
            {
                if( Control_AngVel_Value < 347 )
                {
                  Control_PWM_Value =  Control_AngVel_Value/385.8;  //0.8~0.9
                }
                else
                {
                  Control_PWM_Value =  Control_AngVel_Value/386.3;  //0.9~1
                }
            }
        }
    }
    else if( Control_AngVel_Value < 190)
    {
        if( Control_AngVel_Value < 41 )
        {
           Control_PWM_Value =  Control_AngVel_Value/413.3; //0~0.1
        }
        else
        {
            if( Control_AngVel_Value < 60 )
            {
               Control_PWM_Value =  Control_AngVel_Value/400; //0.1~0.15
            }
            else
            {
                if( Control_AngVel_Value < 80 )
                {
                  Control_PWM_Value =  Control_AngVel_Value/395.7;  //0.15~0.2
                }
                else
                {
                    if( Control_AngVel_Value < 118 )
                    {
                       Control_PWM_Value =  Control_AngVel_Value/393; //0.2~0.3
                    }
                    else
                    {
                        if( Control_AngVel_Value < 155 )
                        {
                            Control_PWM_Value =  Control_AngVel_Value/388.5; //0.3~0.4
                        }
                        else
                        {
                            Control_PWM_Value =  Control_AngVel_Value/386.6; //0.4~0.5
                        }
                    }
                }
            }
        }
    }
    return Control_PWM_Value;
}
int main() 
{
    Serial pc( USBTX, USBRX );
    //pc.baud(921600);
    pc.baud(115200);
    //ExtInt = 0;            //number high, voltage high.
    AR.write(1);
//    stanley_Y=0;
    Y_PWM.calibrate(0.0001, 0*0.0001, 1*0.0001);  //10kHZ
    //PWM.calibrate(0.02, 0.001, 0.002);                //50HZ 
    //PWM.calibrate(0.02, 0.02*0.02, 1*0.02);              //50HZ
    //PWM.calibrate(0.0001, 0*0.0001, 1*0.0001);  //10kHZ
    //PWM.calibrate(0.00001, 0.08*0.00001, 1*0.00001);  //100kHZ
    //PWM.calibrate(0.00001, 0.2*0.00001, 0.8*0.00001);  //100kHZ
    //PWM.calibrate(0.00005, 0.08*0.00005, 1*0.00005);     //20kHZ
    //PWM.calibrate(0.000025, 0*0.000025, 1*0.000025);  //40kHZ
    //PWM.calibrate(0.000005, 0*0.000025, 1*0.000025);  //200kHZ
    
    mkdir("/sd/Amber20170725b", 0777);  //SD裡面的資料夾叫Amber77,在此做宣告
//    FILE *fp = fopen("/sd/Amber20170725b/PWMtoAngVel_20170725_forward.csv", "a");  
    FILE *fp = fopen("/sd/Amber20170725b/PWMtoAngVel_20170725_reverse.csv", "a");    
    //將檔案存進SD的資料夾Amber77裡面,並取名為PWM_AngVel_PWM.xls/.xlsx/.csv
    fprintf(fp,"RunTime,control_PWM_Value, Now_angularVelocity1, Now_angularVelocity3\n");   
    
    
    
    phaseA_1.mode( PullUp );                              // phaseA is set with a built in pull-up resistor
    phaseB_1.mode( PullUp );                              // phaseB is set with a built in pull-up resistor    
    phaseA_3.mode( PullUp );                              // phaseA is set with a built in pull-up resistor
    phaseB_3.mode( PullUp );                              // phaseB is set with a built in pull-up resistor
    Ticker sampleTicker_encoder_1;                                // create a timer to sample the encoder
    Ticker sampleTicker_encoder_3;                                // create a timer to sample the encoder
    Ticker MeasureAngularVelocity;
    sampleTicker_encoder_1.attach_us( &quadratureDecoder_1, t ); // make the quadrature decoder function check the knob once every 1000us = 1ms
    sampleTicker_encoder_3.attach_us( &quadratureDecoder_3, t ); // make the quadrature decoder function check the knob once every 1000us = 1ms
    
    MeasureAngularVelocity.attach( &getAngular, t1 ); // make the quadrature decoder function check the knob once every 1000us = 1ms
    NowTime.start();
    segTime.start();
    while( 1 )
    { 
    
        RunTime = segTime.read();
            
        if( RunTime>=5 && RunTime<=15 )
        {
            //control_angVel_Value = 386;  //最高角速度只能給386
            control_Brake = 1;
            control_StopRun = 0;
//            control_Y1_CW_CCW = 1;  //y往負的方向轉，Robot往正的方向移動
//            control_Y2_CW_CCW = 0;
            control_Y1_CW_CCW = 0;     //y往正的方向轉，Robot往負的方向移動
            control_Y2_CW_CCW = 1;
            control_PWM_Value=0.1;
        }
        else if(RunTime > 15 && RunTime <= 25)
        {
            control_Brake = 1;
            control_StopRun = 0;
//            control_Y1_CW_CCW = 1;  //y往負的方向轉，Robot往正的方向移動
//            control_Y2_CW_CCW = 0;
            control_Y1_CW_CCW = 0;
            control_Y2_CW_CCW = 1;
//            control_angVel_Value = 186;
            control_PWM_Value=0.2;
        }
        else if(RunTime > 25 && RunTime <= 35)
        {
            control_Brake = 1;
            control_StopRun = 0;
//            control_Y1_CW_CCW = 1;  //y往負的方向轉，Robot往正的方向移動
//            control_Y2_CW_CCW = 0;
            control_Y1_CW_CCW = 0;
            control_Y2_CW_CCW = 1;
//            control_angVel_Value = 150;
            control_PWM_Value=0.3;
        }
        else if(RunTime > 35 && RunTime <= 45)
        {
            control_Brake = 1;
            control_StopRun = 0;
//            control_Y1_CW_CCW = 1;  //y往負的方向轉，Robot往正的方向移動
//            control_Y2_CW_CCW = 0;
            control_Y1_CW_CCW = 0;
            control_Y2_CW_CCW = 1;
//            control_angVel_Value = 150;
            control_PWM_Value=0.4;
        }
        else if(RunTime > 45 && RunTime <= 55)
        {
            control_Brake = 1;
            control_StopRun = 0;
//            control_Y1_CW_CCW = 1;  //y往負的方向轉，Robot往正的方向移動
//            control_Y2_CW_CCW = 0;
            control_Y1_CW_CCW = 0;
            control_Y2_CW_CCW = 1;
//            control_angVel_Value = 150;
            control_PWM_Value=0.5;
        }
        else if(RunTime > 55 && RunTime <= 65)
        {
            control_Brake = 1;
            control_StopRun = 0;
//            control_Y1_CW_CCW = 1;  //y往負的方向轉，Robot往正的方向移動
//            control_Y2_CW_CCW = 0;
            control_Y1_CW_CCW = 0;
            control_Y2_CW_CCW = 1;
//            control_angVel_Value = 150;
            control_PWM_Value=0.6;
        }
        else if(RunTime > 65 && RunTime <= 75)
        {
            control_Brake = 1;
            control_StopRun = 0;
//            control_Y1_CW_CCW = 1;  //y往負的方向轉，Robot往正的方向移動
//            control_Y2_CW_CCW = 0;
            control_Y1_CW_CCW = 0;
            control_Y2_CW_CCW = 1;
//            control_angVel_Value = 150;
            control_PWM_Value=0.7;
        }
        else if(RunTime > 75 && RunTime <= 85)
        {
            control_Brake = 1;
            control_StopRun = 0;
//            control_Y1_CW_CCW = 1;  //y往負的方向轉，Robot往正的方向移動
//            control_Y2_CW_CCW = 0;
            control_Y1_CW_CCW = 0;
            control_Y2_CW_CCW = 1;
//            control_angVel_Value = 150;
            control_PWM_Value=0.8;
        }
        else if(RunTime > 85 && RunTime <= 95)
        {
            control_Brake = 1;
            control_StopRun = 0;
//            control_Y1_CW_CCW = 1;  //y往負的方向轉，Robot往正的方向移動
//            control_Y2_CW_CCW = 0;
            control_Y1_CW_CCW = 0;
            control_Y2_CW_CCW = 1;
//            control_angVel_Value = 150;
            control_PWM_Value=0.9;
        }
        else if(RunTime > 95 && RunTime <= 105)
        {
            control_Brake = 1;
            control_StopRun = 0;
//            control_Y1_CW_CCW = 1;  //y往負的方向轉，Robot往正的方向移動
//            control_Y2_CW_CCW = 0;
            control_Y1_CW_CCW = 0;
            control_Y2_CW_CCW = 1;
//            control_angVel_Value = 150;
            control_PWM_Value=1;
        }
        else if(RunTime > 105 && RunTime <= 115)
        {
            control_Brake = 1;
            control_StopRun = 0;
//            control_Y1_CW_CCW = 1;  //y往負的方向轉，Robot往正的方向移動
//            control_Y2_CW_CCW = 0;
            control_Y1_CW_CCW = 0;
            control_Y2_CW_CCW = 1;
//            control_angVel_Value = 150;
            control_PWM_Value=0.5;
        }
        else if(RunTime > 115 && RunTime <= 125)
        {
            control_Brake = 1;
            control_StopRun = 0;
//            control_Y1_CW_CCW = 1;  //y往負的方向轉，Robot往正的方向移動
//            control_Y2_CW_CCW = 0;
            control_Y1_CW_CCW = 0;
            control_Y2_CW_CCW = 1;
//            control_angVel_Value = 150;
            control_PWM_Value=0.2;
        }
        else if(RunTime > 125)
        {
            control_Brake = 1;
            control_StopRun = 0;
//            control_Y1_CW_CCW = 1;  //y往負的方向轉，Robot往正的方向移動
//            control_Y2_CW_CCW = 0;
            control_Y1_CW_CCW = 0;
            control_Y2_CW_CCW = 1;
//            control_angVel_Value = 150;
            control_PWM_Value=0;
        }
            
//        control_PWM_Value = PWM_commmand_transformation(control_angVel_Value);
            
        if( (RunTime-lastTime) > 0.1  )
        {
            index_measure++;
            lastTime = RunTime;
        }
        if( index_measure >= 1 )
        {
            //Measure_motor_1_average = Measure_motor_1_total/(index_measure);
            pc.printf("RunTime : %.3f ", RunTime );            
            pc.printf(", control_PWM_Value : %.3f\n", control_PWM_Value);
            pc.printf("Now_angularVelocity1 : %.3f",Now_angularVelocity1);
            pc.printf(", Now_angularVelocity3 : %.3f\n",Now_angularVelocity3);
            index_measure = 0;
            Measure_motor_1_total = 0;
            if(fp == NULL) 
            {
                    error("Could not open file for write\n");
            }
//            fprintf(fp,"%.3f,%.3f,%.3f\n", RunTime,Now_angularVelocity,control_PWM_Value);  
            fprintf(fp,"%.3f,%.3f,%.3f,%.3f\n", RunTime, control_PWM_Value, Now_angularVelocity1, Now_angularVelocity3);
        }
        
        Motor_run(control_PWM_Value,control_Brake,control_StopRun, control_Y1_CW_CCW, control_Y2_CW_CCW);
        if(!mybutton)
        {
            StopRun.write(1);
            Brake.write(1);
            Y1_CW_CCW.write(1);                   // clockwise:0    counterclockwise:1
            Y2_CW_CCW.write(0);  
            Y_PWM.write(0);                   // clockwise:0    counterclockwise:1
        }
    }
    fclose(fp);
}