Revision 0:b24f1c7238b2, committed 2017-09-20

Comitter:

Amber77

Date:

Wed Sep 20 20:34:09 2017 +0000

Commit message:

20170921

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SDFileSystem.lib	Wed Sep 20 20:34:09 2017 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/Amber77/code/SDFileSystem03/#fbe97c1cb264

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Servo.lib	Wed Sep 20 20:34:09 2017 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/andrewrussell/code/Servo/#4c315bcd91b4

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Sep 20 20:34:09 2017 +0000
@@ -0,0 +1,563 @@
+/* "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);
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Wed Sep 20 20:34:09 2017 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/176b8275d35d
\ No newline at end of file