Amber Tang
123
Dependencies: SDFileSystem03 Servo mbed
Diff: main.cpp
- Revision:
- 0:b24f1c7238b2
diff -r 000000000000 -r b24f1c7238b2 main.cpp
--- /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