Amber Tang / Mbed 2 deprecated y-axis_2motor_control_20170721_OK

123

Dependencies:   SDFileSystem03 Servo mbed

main.cpp@0:b24f1c7238b2, 2017-09-20 (annotated)

Committer:
Amber77
Date:
Wed Sep 20 20:34:09 2017 +0000
Revision:
0:b24f1c7238b2
20170921

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Amber77 0:b24f1c7238b2 1 /* "PID.h"--- https://developer.mbed.org/users/aberk/code/PID/ */
Amber77 0:b24f1c7238b2 2 /* "Servo.h"-- https://developer.mbed.org/users/andrewrussell/code/Servo/file/4c315bcd91b4/Servo.cpp */
Amber77 0:b24f1c7238b2 3 // SDFileSystem---1. https://developer.mbed.org/users/mbed_official/code/SDFileSystem/
Amber77 0:b24f1c7238b2 4 // 2. https://developer.mbed.org/users/simon/code/SDCardTest/
Amber77 0:b24f1c7238b2 5 // SD care---3.3v
Amber77 0:b24f1c7238b2 6 // Ctrl+? =>註解
Amber77 0:b24f1c7238b2 7
Amber77 0:b24f1c7238b2 8 //#include "PID.h"
Amber77 0:b24f1c7238b2 9 #include "mbed.h"
Amber77 0:b24f1c7238b2 10 #include "Servo.h"
Amber77 0:b24f1c7238b2 11 #include "SDFileSystem.h"
Amber77 0:b24f1c7238b2 12
Amber77 0:b24f1c7238b2 13 //****************************************************************************/
Amber77 0:b24f1c7238b2 14 double Command_AngularVel;
Amber77 0:b24f1c7238b2 15 double Measure_motor_1; //PID
Amber77 0:b24f1c7238b2 16 double Measure_motor_1_average = 0;
Amber77 0:b24f1c7238b2 17 double LastMeasure_motor_1_average = 0;
Amber77 0:b24f1c7238b2 18 double Measure_motor_1_total = 0;
Amber77 0:b24f1c7238b2 19 int index_measure = 0;
Amber77 0:b24f1c7238b2 20 int control_Brake,control_StopRun, control_Y1_CW_CCW, control_Y2_CW_CCW;
Amber77 0:b24f1c7238b2 21 double control_PWM_Value;
Amber77 0:b24f1c7238b2 22 double control_angVel_Value;
Amber77 0:b24f1c7238b2 23 double LastSectionAngle1,LastSectionAngle1_1,LastSectionAngle1_2,LastSectionAngle1_3,LastSectionAngle1_4;
Amber77 0:b24f1c7238b2 24 double LastSectionAngle3,LastSectionAngle3_1,LastSectionAngle3_2,LastSectionAngle3_3,LastSectionAngle3_4;
Amber77 0:b24f1c7238b2 25 //****************************Encoder declare************************************//
Amber77 0:b24f1c7238b2 26 //DigitalIn phaseA( PA_0 ); // phase a of the quadrature encoder
Amber77 0:b24f1c7238b2 27 //DigitalIn phaseB( PA_1 ); // phase b of the quadrature encoder
Amber77 0:b24f1c7238b2 28 DigitalIn phaseA_1( PA_0 ); // phase a of the quadrature encoder
Amber77 0:b24f1c7238b2 29 DigitalIn phaseB_1( PA_1 ); // phase b of the quadrature encoder
Amber77 0:b24f1c7238b2 30 DigitalIn phaseA_3( PC_6 ); // phase a of the quadrature encoder
Amber77 0:b24f1c7238b2 31 DigitalIn phaseB_3( PC_7 ); // phase b of the quadrature encoder
Amber77 0:b24f1c7238b2 32 int encoderClickCount_1 = 0; // hold the signed value corresponding to the number of clicks left or right since last sample
Amber77 0:b24f1c7238b2 33 int previousEncoderState_1 = 0; // keep a record of the last actioned sample state of the Qb+Qa outputs for comparison on each interrupt
Amber77 0:b24f1c7238b2 34 int encoderClickCount_3 = 0; // hold the signed value corresponding to the number of clicks left or right since last sample
Amber77 0:b24f1c7238b2 35 int previousEncoderState_3 = 0; // keep a record of the last actioned sample state of the Qb+Qa outputs for comparison on each interrupt
Amber77 0:b24f1c7238b2 36 double angle1 =0,LastAngle1 =0;
Amber77 0:b24f1c7238b2 37 double angle3 =0,LastAngle3 =0;
Amber77 0:b24f1c7238b2 38 double t=7,t1=0.03;
Amber77 0:b24f1c7238b2 39 double SectionTime =0;
Amber77 0:b24f1c7238b2 40 double Average_SectionAngle1, Average_SectionAngle3;
Amber77 0:b24f1c7238b2 41 double Now_angularVelocity1, Now_angularVelocity3;
Amber77 0:b24f1c7238b2 42 double SectionAngle1 =0,SectionAngle3 =0;
Amber77 0:b24f1c7238b2 43 double NOWTime=0, LastTime = 0;
Amber77 0:b24f1c7238b2 44 double RunTime = 0,lastTime =0;
Amber77 0:b24f1c7238b2 45 //****************************Encoder declare************************************//
Amber77 0:b24f1c7238b2 46
Amber77 0:b24f1c7238b2 47 //DigitalOut StopRun(D15);
Amber77 0:b24f1c7238b2 48 //DigitalOut Brake(D14);
Amber77 0:b24f1c7238b2 49 //DigitalOut AR(D9);
Amber77 0:b24f1c7238b2 50 //DigitalOut Y1_CW_CCW(PC_8);
Amber77 0:b24f1c7238b2 51 //DigitalOut Y2_CW_CCW(PC_9);
Amber77 0:b24f1c7238b2 52 //Servo Y_PWM(D8);
Amber77 0:b24f1c7238b2 53 //DigitalOut stanley_Y(D7);
Amber77 0:b24f1c7238b2 54 DigitalOut StopRun(PA_12);
Amber77 0:b24f1c7238b2 55 DigitalOut Brake(PA_11);
Amber77 0:b24f1c7238b2 56 DigitalOut AR(PB_12);
Amber77 0:b24f1c7238b2 57 DigitalOut Y1_CW_CCW(PC_12);
Amber77 0:b24f1c7238b2 58 DigitalOut Y2_CW_CCW(PD_2);
Amber77 0:b24f1c7238b2 59 Servo Y_PWM(PB_2);
Amber77 0:b24f1c7238b2 60 DigitalOut stanley_Y(PB_1);
Amber77 0:b24f1c7238b2 61 DigitalOut myled(LED1);
Amber77 0:b24f1c7238b2 62 //SDFileSystem sd(D11, D12, D13, D10, "sd"); // mosi, miso, sclk, cs
Amber77 0:b24f1c7238b2 63 SDFileSystem sd( D4, D5, D3, D6, "sd"); // mosi, miso, sclk, cs
Amber77 0:b24f1c7238b2 64 DigitalIn mybutton(USER_BUTTON);
Amber77 0:b24f1c7238b2 65
Amber77 0:b24f1c7238b2 66 Timer NowTime;
Amber77 0:b24f1c7238b2 67 Timer segTime;
Amber77 0:b24f1c7238b2 68 int Motor_run(double control_value_PWM ,int control_brake , int control_stopRun, int control_Y1_CW_CCW, int control_Y2_CW_CCW)
Amber77 0:b24f1c7238b2 69 {
Amber77 0:b24f1c7238b2 70 //StopRun = control_stopRun;
Amber77 0:b24f1c7238b2 71 // Brake = control_brake;
Amber77 0:b24f1c7238b2 72 StopRun.write(control_stopRun);
Amber77 0:b24f1c7238b2 73 Brake.write(control_brake);
Amber77 0:b24f1c7238b2 74 Y1_CW_CCW.write(control_Y1_CW_CCW);
Amber77 0:b24f1c7238b2 75 Y2_CW_CCW.write(control_Y2_CW_CCW);
Amber77 0:b24f1c7238b2 76 //double Control_value_PWM = 1-control_value_PWM;
Amber77 0:b24f1c7238b2 77 //control_PWM_Value = 0.05;
Amber77 0:b24f1c7238b2 78 //PWM.write(control_PWM_Value);
Amber77 0:b24f1c7238b2 79 Y_PWM.write(control_value_PWM);
Amber77 0:b24f1c7238b2 80 //PWM.write(0.3);
Amber77 0:b24f1c7238b2 81
Amber77 0:b24f1c7238b2 82 //wait_ms(1);
Amber77 0:b24f1c7238b2 83 return control_value_PWM;
Amber77 0:b24f1c7238b2 84 }
Amber77 0:b24f1c7238b2 85
Amber77 0:b24f1c7238b2 86 //**************************** 1 ************************************//
Amber77 0:b24f1c7238b2 87 void quadratureDecoder_1( void )
Amber77 0:b24f1c7238b2 88 {
Amber77 0:b24f1c7238b2 89 int currentEncoderState_1 = (phaseB_1.read() << 1) + phaseA_1.read();
Amber77 0:b24f1c7238b2 90
Amber77 0:b24f1c7238b2 91 if( currentEncoderState_1 == previousEncoderState_1 )
Amber77 0:b24f1c7238b2 92 {
Amber77 0:b24f1c7238b2 93 return;
Amber77 0:b24f1c7238b2 94 }
Amber77 0:b24f1c7238b2 95
Amber77 0:b24f1c7238b2 96 switch( previousEncoderState_1 )
Amber77 0:b24f1c7238b2 97 {
Amber77 0:b24f1c7238b2 98 case 0:
Amber77 0:b24f1c7238b2 99 if( currentEncoderState_1 == 2 )
Amber77 0:b24f1c7238b2 100 {
Amber77 0:b24f1c7238b2 101 encoderClickCount_1--;
Amber77 0:b24f1c7238b2 102
Amber77 0:b24f1c7238b2 103 }
Amber77 0:b24f1c7238b2 104 else if( currentEncoderState_1 == 1 )
Amber77 0:b24f1c7238b2 105 {
Amber77 0:b24f1c7238b2 106 encoderClickCount_1++;
Amber77 0:b24f1c7238b2 107
Amber77 0:b24f1c7238b2 108 }
Amber77 0:b24f1c7238b2 109 break;
Amber77 0:b24f1c7238b2 110
Amber77 0:b24f1c7238b2 111 case 1:
Amber77 0:b24f1c7238b2 112 if( currentEncoderState_1 == 0 )
Amber77 0:b24f1c7238b2 113 {
Amber77 0:b24f1c7238b2 114 encoderClickCount_1--;
Amber77 0:b24f1c7238b2 115
Amber77 0:b24f1c7238b2 116 }
Amber77 0:b24f1c7238b2 117 else if( currentEncoderState_1 == 3 )
Amber77 0:b24f1c7238b2 118 {
Amber77 0:b24f1c7238b2 119 encoderClickCount_1++;
Amber77 0:b24f1c7238b2 120
Amber77 0:b24f1c7238b2 121 }
Amber77 0:b24f1c7238b2 122 break;
Amber77 0:b24f1c7238b2 123
Amber77 0:b24f1c7238b2 124 case 2:
Amber77 0:b24f1c7238b2 125 if( currentEncoderState_1 == 3 )
Amber77 0:b24f1c7238b2 126 {
Amber77 0:b24f1c7238b2 127 encoderClickCount_1--;
Amber77 0:b24f1c7238b2 128
Amber77 0:b24f1c7238b2 129 }
Amber77 0:b24f1c7238b2 130 else if( currentEncoderState_1 == 0 )
Amber77 0:b24f1c7238b2 131 {
Amber77 0:b24f1c7238b2 132 encoderClickCount_1++;
Amber77 0:b24f1c7238b2 133
Amber77 0:b24f1c7238b2 134 }
Amber77 0:b24f1c7238b2 135 break;
Amber77 0:b24f1c7238b2 136
Amber77 0:b24f1c7238b2 137 case 3:
Amber77 0:b24f1c7238b2 138 if( currentEncoderState_1 == 1 )
Amber77 0:b24f1c7238b2 139 {
Amber77 0:b24f1c7238b2 140 encoderClickCount_1--;
Amber77 0:b24f1c7238b2 141
Amber77 0:b24f1c7238b2 142 }
Amber77 0:b24f1c7238b2 143 else if( currentEncoderState_1 == 2 )
Amber77 0:b24f1c7238b2 144 {
Amber77 0:b24f1c7238b2 145 encoderClickCount_1++;
Amber77 0:b24f1c7238b2 146
Amber77 0:b24f1c7238b2 147 }
Amber77 0:b24f1c7238b2 148 break;
Amber77 0:b24f1c7238b2 149
Amber77 0:b24f1c7238b2 150 default:
Amber77 0:b24f1c7238b2 151 break;
Amber77 0:b24f1c7238b2 152 }
Amber77 0:b24f1c7238b2 153 previousEncoderState_1 = currentEncoderState_1;
Amber77 0:b24f1c7238b2 154
Amber77 0:b24f1c7238b2 155 }
Amber77 0:b24f1c7238b2 156 //**************************** 3 ************************************//
Amber77 0:b24f1c7238b2 157 void quadratureDecoder_3( void )
Amber77 0:b24f1c7238b2 158 {
Amber77 0:b24f1c7238b2 159 int currentEncoderState_3 = (phaseB_3.read() << 1) + phaseA_3.read();
Amber77 0:b24f1c7238b2 160
Amber77 0:b24f1c7238b2 161 if( currentEncoderState_3 == previousEncoderState_3 )
Amber77 0:b24f1c7238b2 162 {
Amber77 0:b24f1c7238b2 163 return;
Amber77 0:b24f1c7238b2 164 }
Amber77 0:b24f1c7238b2 165
Amber77 0:b24f1c7238b2 166 switch( previousEncoderState_3 )
Amber77 0:b24f1c7238b2 167 {
Amber77 0:b24f1c7238b2 168 case 0:
Amber77 0:b24f1c7238b2 169 if( currentEncoderState_3 == 2 )
Amber77 0:b24f1c7238b2 170 {
Amber77 0:b24f1c7238b2 171 encoderClickCount_3--;
Amber77 0:b24f1c7238b2 172
Amber77 0:b24f1c7238b2 173 }
Amber77 0:b24f1c7238b2 174 else if( currentEncoderState_3 == 1 )
Amber77 0:b24f1c7238b2 175 {
Amber77 0:b24f1c7238b2 176 encoderClickCount_3++;
Amber77 0:b24f1c7238b2 177
Amber77 0:b24f1c7238b2 178 }
Amber77 0:b24f1c7238b2 179 break;
Amber77 0:b24f1c7238b2 180
Amber77 0:b24f1c7238b2 181 case 1:
Amber77 0:b24f1c7238b2 182 if( currentEncoderState_3 == 0 )
Amber77 0:b24f1c7238b2 183 {
Amber77 0:b24f1c7238b2 184 encoderClickCount_3--;
Amber77 0:b24f1c7238b2 185
Amber77 0:b24f1c7238b2 186 }
Amber77 0:b24f1c7238b2 187 else if( currentEncoderState_3 == 3 )
Amber77 0:b24f1c7238b2 188 {
Amber77 0:b24f1c7238b2 189 encoderClickCount_3++;
Amber77 0:b24f1c7238b2 190
Amber77 0:b24f1c7238b2 191 }
Amber77 0:b24f1c7238b2 192 break;
Amber77 0:b24f1c7238b2 193
Amber77 0:b24f1c7238b2 194 case 2:
Amber77 0:b24f1c7238b2 195 if( currentEncoderState_3 == 3 )
Amber77 0:b24f1c7238b2 196 {
Amber77 0:b24f1c7238b2 197 encoderClickCount_3--;
Amber77 0:b24f1c7238b2 198
Amber77 0:b24f1c7238b2 199 }
Amber77 0:b24f1c7238b2 200 else if( currentEncoderState_3 == 0 )
Amber77 0:b24f1c7238b2 201 {
Amber77 0:b24f1c7238b2 202 encoderClickCount_3++;
Amber77 0:b24f1c7238b2 203
Amber77 0:b24f1c7238b2 204 }
Amber77 0:b24f1c7238b2 205 break;
Amber77 0:b24f1c7238b2 206
Amber77 0:b24f1c7238b2 207 case 3:
Amber77 0:b24f1c7238b2 208 if( currentEncoderState_3 == 1 )
Amber77 0:b24f1c7238b2 209 {
Amber77 0:b24f1c7238b2 210 encoderClickCount_3--;
Amber77 0:b24f1c7238b2 211
Amber77 0:b24f1c7238b2 212 }
Amber77 0:b24f1c7238b2 213 else if( currentEncoderState_3 == 2 )
Amber77 0:b24f1c7238b2 214 {
Amber77 0:b24f1c7238b2 215 encoderClickCount_3++;
Amber77 0:b24f1c7238b2 216
Amber77 0:b24f1c7238b2 217 }
Amber77 0:b24f1c7238b2 218 break;
Amber77 0:b24f1c7238b2 219
Amber77 0:b24f1c7238b2 220 default:
Amber77 0:b24f1c7238b2 221 break;
Amber77 0:b24f1c7238b2 222 }
Amber77 0:b24f1c7238b2 223 previousEncoderState_3 = currentEncoderState_3;
Amber77 0:b24f1c7238b2 224
Amber77 0:b24f1c7238b2 225 }
Amber77 0:b24f1c7238b2 226 void getAngular( void )
Amber77 0:b24f1c7238b2 227 {
Amber77 0:b24f1c7238b2 228 angle1 = (encoderClickCount_1*0.1499)/5;
Amber77 0:b24f1c7238b2 229 angle3 = (encoderClickCount_3*0.1499)/5;
Amber77 0:b24f1c7238b2 230 NOWTime = NowTime.read();
Amber77 0:b24f1c7238b2 231 SectionTime = NOWTime - LastTime;
Amber77 0:b24f1c7238b2 232 SectionAngle1 = angle1 - LastAngle1;
Amber77 0:b24f1c7238b2 233 SectionAngle3 = angle3 - LastAngle3;
Amber77 0:b24f1c7238b2 234 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;
Amber77 0:b24f1c7238b2 235 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;
Amber77 0:b24f1c7238b2 236 Now_angularVelocity1 = Average_SectionAngle1/(t1);
Amber77 0:b24f1c7238b2 237 Now_angularVelocity3 = Average_SectionAngle3/(t1);
Amber77 0:b24f1c7238b2 238 LastTime = NOWTime;
Amber77 0:b24f1c7238b2 239 LastAngle1 = angle1;
Amber77 0:b24f1c7238b2 240 LastAngle3 = angle3;
Amber77 0:b24f1c7238b2 241 LastSectionAngle1_4 = LastSectionAngle1_3;
Amber77 0:b24f1c7238b2 242 LastSectionAngle1_3 = LastSectionAngle1_2;
Amber77 0:b24f1c7238b2 243 LastSectionAngle1_2 = LastSectionAngle1_1;
Amber77 0:b24f1c7238b2 244 LastSectionAngle1_1 = LastSectionAngle1;
Amber77 0:b24f1c7238b2 245 LastSectionAngle1 = SectionAngle1;
Amber77 0:b24f1c7238b2 246 LastSectionAngle3_4 = LastSectionAngle3_3;
Amber77 0:b24f1c7238b2 247 LastSectionAngle3_3 = LastSectionAngle3_2;
Amber77 0:b24f1c7238b2 248 LastSectionAngle3_2 = LastSectionAngle3_1;
Amber77 0:b24f1c7238b2 249 LastSectionAngle3_1 = LastSectionAngle3;
Amber77 0:b24f1c7238b2 250 LastSectionAngle3 = SectionAngle3;
Amber77 0:b24f1c7238b2 251 }
Amber77 0:b24f1c7238b2 252 double PWM_commmand_transformation( double Control_AngVel_Value )
Amber77 0:b24f1c7238b2 253 {
Amber77 0:b24f1c7238b2 254 double Control_PWM_Value = 0;
Amber77 0:b24f1c7238b2 255 if( Control_AngVel_Value > 0 )
Amber77 0:b24f1c7238b2 256 {
Amber77 0:b24f1c7238b2 257 // control_Y1_CW_CCW = 0; // clockwise:0 counterclockwise:1
Amber77 0:b24f1c7238b2 258 // control_Y2_CW_CCW = 1;
Amber77 0:b24f1c7238b2 259 Control_AngVel_Value = Control_AngVel_Value;
Amber77 0:b24f1c7238b2 260 }
Amber77 0:b24f1c7238b2 261 else if( Control_AngVel_Value < 0 )
Amber77 0:b24f1c7238b2 262 {
Amber77 0:b24f1c7238b2 263 // control_Y1_CW_CCW = 1; // clockwise:0 counterclockwise:1
Amber77 0:b24f1c7238b2 264 // control_Y2_CW_CCW = 0;
Amber77 0:b24f1c7238b2 265 Control_AngVel_Value = -Control_AngVel_Value;
Amber77 0:b24f1c7238b2 266 }
Amber77 0:b24f1c7238b2 267 if( Control_AngVel_Value > 190)
Amber77 0:b24f1c7238b2 268 {
Amber77 0:b24f1c7238b2 269 if( Control_AngVel_Value < 270 )
Amber77 0:b24f1c7238b2 270 {
Amber77 0:b24f1c7238b2 271 if( Control_AngVel_Value < 232 )
Amber77 0:b24f1c7238b2 272 {
Amber77 0:b24f1c7238b2 273 Control_PWM_Value = Control_AngVel_Value/386 ; //0.5~0.6
Amber77 0:b24f1c7238b2 274 }
Amber77 0:b24f1c7238b2 275 else
Amber77 0:b24f1c7238b2 276 {
Amber77 0:b24f1c7238b2 277 Control_PWM_Value = Control_AngVel_Value/386.9; //0.6~0.7
Amber77 0:b24f1c7238b2 278 }
Amber77 0:b24f1c7238b2 279 }
Amber77 0:b24f1c7238b2 280 else
Amber77 0:b24f1c7238b2 281 {
Amber77 0:b24f1c7238b2 282 if( Control_AngVel_Value < 309 )
Amber77 0:b24f1c7238b2 283 {
Amber77 0:b24f1c7238b2 284 Control_PWM_Value = Control_AngVel_Value/386.6; //0.7~0.8
Amber77 0:b24f1c7238b2 285 }
Amber77 0:b24f1c7238b2 286 else
Amber77 0:b24f1c7238b2 287 {
Amber77 0:b24f1c7238b2 288 if( Control_AngVel_Value < 347 )
Amber77 0:b24f1c7238b2 289 {
Amber77 0:b24f1c7238b2 290 Control_PWM_Value = Control_AngVel_Value/385.8; //0.8~0.9
Amber77 0:b24f1c7238b2 291 }
Amber77 0:b24f1c7238b2 292 else
Amber77 0:b24f1c7238b2 293 {
Amber77 0:b24f1c7238b2 294 Control_PWM_Value = Control_AngVel_Value/386.3; //0.9~1
Amber77 0:b24f1c7238b2 295 }
Amber77 0:b24f1c7238b2 296 }
Amber77 0:b24f1c7238b2 297 }
Amber77 0:b24f1c7238b2 298 }
Amber77 0:b24f1c7238b2 299 else if( Control_AngVel_Value < 190)
Amber77 0:b24f1c7238b2 300 {
Amber77 0:b24f1c7238b2 301 if( Control_AngVel_Value < 41 )
Amber77 0:b24f1c7238b2 302 {
Amber77 0:b24f1c7238b2 303 Control_PWM_Value = Control_AngVel_Value/413.3; //0~0.1
Amber77 0:b24f1c7238b2 304 }
Amber77 0:b24f1c7238b2 305 else
Amber77 0:b24f1c7238b2 306 {
Amber77 0:b24f1c7238b2 307 if( Control_AngVel_Value < 60 )
Amber77 0:b24f1c7238b2 308 {
Amber77 0:b24f1c7238b2 309 Control_PWM_Value = Control_AngVel_Value/400; //0.1~0.15
Amber77 0:b24f1c7238b2 310 }
Amber77 0:b24f1c7238b2 311 else
Amber77 0:b24f1c7238b2 312 {
Amber77 0:b24f1c7238b2 313 if( Control_AngVel_Value < 80 )
Amber77 0:b24f1c7238b2 314 {
Amber77 0:b24f1c7238b2 315 Control_PWM_Value = Control_AngVel_Value/395.7; //0.15~0.2
Amber77 0:b24f1c7238b2 316 }
Amber77 0:b24f1c7238b2 317 else
Amber77 0:b24f1c7238b2 318 {
Amber77 0:b24f1c7238b2 319 if( Control_AngVel_Value < 118 )
Amber77 0:b24f1c7238b2 320 {
Amber77 0:b24f1c7238b2 321 Control_PWM_Value = Control_AngVel_Value/393; //0.2~0.3
Amber77 0:b24f1c7238b2 322 }
Amber77 0:b24f1c7238b2 323 else
Amber77 0:b24f1c7238b2 324 {
Amber77 0:b24f1c7238b2 325 if( Control_AngVel_Value < 155 )
Amber77 0:b24f1c7238b2 326 {
Amber77 0:b24f1c7238b2 327 Control_PWM_Value = Control_AngVel_Value/388.5; //0.3~0.4
Amber77 0:b24f1c7238b2 328 }
Amber77 0:b24f1c7238b2 329 else
Amber77 0:b24f1c7238b2 330 {
Amber77 0:b24f1c7238b2 331 Control_PWM_Value = Control_AngVel_Value/386.6; //0.4~0.5
Amber77 0:b24f1c7238b2 332 }
Amber77 0:b24f1c7238b2 333 }
Amber77 0:b24f1c7238b2 334 }
Amber77 0:b24f1c7238b2 335 }
Amber77 0:b24f1c7238b2 336 }
Amber77 0:b24f1c7238b2 337 }
Amber77 0:b24f1c7238b2 338 return Control_PWM_Value;
Amber77 0:b24f1c7238b2 339 }
Amber77 0:b24f1c7238b2 340 int main()
Amber77 0:b24f1c7238b2 341 {
Amber77 0:b24f1c7238b2 342 Serial pc( USBTX, USBRX );
Amber77 0:b24f1c7238b2 343 //pc.baud(921600);
Amber77 0:b24f1c7238b2 344 pc.baud(115200);
Amber77 0:b24f1c7238b2 345 //ExtInt = 0; //number high, voltage high.
Amber77 0:b24f1c7238b2 346 AR.write(1);
Amber77 0:b24f1c7238b2 347 // stanley_Y=0;
Amber77 0:b24f1c7238b2 348 Y_PWM.calibrate(0.0001, 0*0.0001, 1*0.0001); //10kHZ
Amber77 0:b24f1c7238b2 349 //PWM.calibrate(0.02, 0.001, 0.002); //50HZ
Amber77 0:b24f1c7238b2 350 //PWM.calibrate(0.02, 0.02*0.02, 1*0.02); //50HZ
Amber77 0:b24f1c7238b2 351 //PWM.calibrate(0.0001, 0*0.0001, 1*0.0001); //10kHZ
Amber77 0:b24f1c7238b2 352 //PWM.calibrate(0.00001, 0.08*0.00001, 1*0.00001); //100kHZ
Amber77 0:b24f1c7238b2 353 //PWM.calibrate(0.00001, 0.2*0.00001, 0.8*0.00001); //100kHZ
Amber77 0:b24f1c7238b2 354 //PWM.calibrate(0.00005, 0.08*0.00005, 1*0.00005); //20kHZ
Amber77 0:b24f1c7238b2 355 //PWM.calibrate(0.000025, 0*0.000025, 1*0.000025); //40kHZ
Amber77 0:b24f1c7238b2 356 //PWM.calibrate(0.000005, 0*0.000025, 1*0.000025); //200kHZ
Amber77 0:b24f1c7238b2 357
Amber77 0:b24f1c7238b2 358 mkdir("/sd/Amber20170725b", 0777); //SD裡面的資料夾叫Amber77,在此做宣告
Amber77 0:b24f1c7238b2 359 // FILE *fp = fopen("/sd/Amber20170725b/PWMtoAngVel_20170725_forward.csv", "a");
Amber77 0:b24f1c7238b2 360 FILE *fp = fopen("/sd/Amber20170725b/PWMtoAngVel_20170725_reverse.csv", "a");
Amber77 0:b24f1c7238b2 361 //將檔案存進SD的資料夾Amber77裡面,並取名為PWM_AngVel_PWM.xls/.xlsx/.csv
Amber77 0:b24f1c7238b2 362 fprintf(fp,"RunTime,control_PWM_Value, Now_angularVelocity1, Now_angularVelocity3\n");
Amber77 0:b24f1c7238b2 363
Amber77 0:b24f1c7238b2 364
Amber77 0:b24f1c7238b2 365
Amber77 0:b24f1c7238b2 366 phaseA_1.mode( PullUp ); // phaseA is set with a built in pull-up resistor
Amber77 0:b24f1c7238b2 367 phaseB_1.mode( PullUp ); // phaseB is set with a built in pull-up resistor
Amber77 0:b24f1c7238b2 368 phaseA_3.mode( PullUp ); // phaseA is set with a built in pull-up resistor
Amber77 0:b24f1c7238b2 369 phaseB_3.mode( PullUp ); // phaseB is set with a built in pull-up resistor
Amber77 0:b24f1c7238b2 370 Ticker sampleTicker_encoder_1; // create a timer to sample the encoder
Amber77 0:b24f1c7238b2 371 Ticker sampleTicker_encoder_3; // create a timer to sample the encoder
Amber77 0:b24f1c7238b2 372 Ticker MeasureAngularVelocity;
Amber77 0:b24f1c7238b2 373 sampleTicker_encoder_1.attach_us( &quadratureDecoder_1, t ); // make the quadrature decoder function check the knob once every 1000us = 1ms
Amber77 0:b24f1c7238b2 374 sampleTicker_encoder_3.attach_us( &quadratureDecoder_3, t ); // make the quadrature decoder function check the knob once every 1000us = 1ms
Amber77 0:b24f1c7238b2 375
Amber77 0:b24f1c7238b2 376 MeasureAngularVelocity.attach( &getAngular, t1 ); // make the quadrature decoder function check the knob once every 1000us = 1ms
Amber77 0:b24f1c7238b2 377 NowTime.start();
Amber77 0:b24f1c7238b2 378 segTime.start();
Amber77 0:b24f1c7238b2 379 while( 1 )
Amber77 0:b24f1c7238b2 380 {
Amber77 0:b24f1c7238b2 381
Amber77 0:b24f1c7238b2 382 RunTime = segTime.read();
Amber77 0:b24f1c7238b2 383
Amber77 0:b24f1c7238b2 384 if( RunTime>=5 && RunTime<=15 )
Amber77 0:b24f1c7238b2 385 {
Amber77 0:b24f1c7238b2 386 //control_angVel_Value = 386; //最高角速度只能給386
Amber77 0:b24f1c7238b2 387 control_Brake = 1;
Amber77 0:b24f1c7238b2 388 control_StopRun = 0;
Amber77 0:b24f1c7238b2 389 // control_Y1_CW_CCW = 1; //y往負的方向轉，Robot往正的方向移動
Amber77 0:b24f1c7238b2 390 // control_Y2_CW_CCW = 0;
Amber77 0:b24f1c7238b2 391 control_Y1_CW_CCW = 0; //y往正的方向轉，Robot往負的方向移動
Amber77 0:b24f1c7238b2 392 control_Y2_CW_CCW = 1;
Amber77 0:b24f1c7238b2 393 control_PWM_Value=0.1;
Amber77 0:b24f1c7238b2 394 }
Amber77 0:b24f1c7238b2 395 else if(RunTime > 15 && RunTime <= 25)
Amber77 0:b24f1c7238b2 396 {
Amber77 0:b24f1c7238b2 397 control_Brake = 1;
Amber77 0:b24f1c7238b2 398 control_StopRun = 0;
Amber77 0:b24f1c7238b2 399 // control_Y1_CW_CCW = 1; //y往負的方向轉，Robot往正的方向移動
Amber77 0:b24f1c7238b2 400 // control_Y2_CW_CCW = 0;
Amber77 0:b24f1c7238b2 401 control_Y1_CW_CCW = 0;
Amber77 0:b24f1c7238b2 402 control_Y2_CW_CCW = 1;
Amber77 0:b24f1c7238b2 403 // control_angVel_Value = 186;
Amber77 0:b24f1c7238b2 404 control_PWM_Value=0.2;
Amber77 0:b24f1c7238b2 405 }
Amber77 0:b24f1c7238b2 406 else if(RunTime > 25 && RunTime <= 35)
Amber77 0:b24f1c7238b2 407 {
Amber77 0:b24f1c7238b2 408 control_Brake = 1;
Amber77 0:b24f1c7238b2 409 control_StopRun = 0;
Amber77 0:b24f1c7238b2 410 // control_Y1_CW_CCW = 1; //y往負的方向轉，Robot往正的方向移動
Amber77 0:b24f1c7238b2 411 // control_Y2_CW_CCW = 0;
Amber77 0:b24f1c7238b2 412 control_Y1_CW_CCW = 0;
Amber77 0:b24f1c7238b2 413 control_Y2_CW_CCW = 1;
Amber77 0:b24f1c7238b2 414 // control_angVel_Value = 150;
Amber77 0:b24f1c7238b2 415 control_PWM_Value=0.3;
Amber77 0:b24f1c7238b2 416 }
Amber77 0:b24f1c7238b2 417 else if(RunTime > 35 && RunTime <= 45)
Amber77 0:b24f1c7238b2 418 {
Amber77 0:b24f1c7238b2 419 control_Brake = 1;
Amber77 0:b24f1c7238b2 420 control_StopRun = 0;
Amber77 0:b24f1c7238b2 421 // control_Y1_CW_CCW = 1; //y往負的方向轉，Robot往正的方向移動
Amber77 0:b24f1c7238b2 422 // control_Y2_CW_CCW = 0;
Amber77 0:b24f1c7238b2 423 control_Y1_CW_CCW = 0;
Amber77 0:b24f1c7238b2 424 control_Y2_CW_CCW = 1;
Amber77 0:b24f1c7238b2 425 // control_angVel_Value = 150;
Amber77 0:b24f1c7238b2 426 control_PWM_Value=0.4;
Amber77 0:b24f1c7238b2 427 }
Amber77 0:b24f1c7238b2 428 else if(RunTime > 45 && RunTime <= 55)
Amber77 0:b24f1c7238b2 429 {
Amber77 0:b24f1c7238b2 430 control_Brake = 1;
Amber77 0:b24f1c7238b2 431 control_StopRun = 0;
Amber77 0:b24f1c7238b2 432 // control_Y1_CW_CCW = 1; //y往負的方向轉，Robot往正的方向移動
Amber77 0:b24f1c7238b2 433 // control_Y2_CW_CCW = 0;
Amber77 0:b24f1c7238b2 434 control_Y1_CW_CCW = 0;
Amber77 0:b24f1c7238b2 435 control_Y2_CW_CCW = 1;
Amber77 0:b24f1c7238b2 436 // control_angVel_Value = 150;
Amber77 0:b24f1c7238b2 437 control_PWM_Value=0.5;
Amber77 0:b24f1c7238b2 438 }
Amber77 0:b24f1c7238b2 439 else if(RunTime > 55 && RunTime <= 65)
Amber77 0:b24f1c7238b2 440 {
Amber77 0:b24f1c7238b2 441 control_Brake = 1;
Amber77 0:b24f1c7238b2 442 control_StopRun = 0;
Amber77 0:b24f1c7238b2 443 // control_Y1_CW_CCW = 1; //y往負的方向轉，Robot往正的方向移動
Amber77 0:b24f1c7238b2 444 // control_Y2_CW_CCW = 0;
Amber77 0:b24f1c7238b2 445 control_Y1_CW_CCW = 0;
Amber77 0:b24f1c7238b2 446 control_Y2_CW_CCW = 1;
Amber77 0:b24f1c7238b2 447 // control_angVel_Value = 150;
Amber77 0:b24f1c7238b2 448 control_PWM_Value=0.6;
Amber77 0:b24f1c7238b2 449 }
Amber77 0:b24f1c7238b2 450 else if(RunTime > 65 && RunTime <= 75)
Amber77 0:b24f1c7238b2 451 {
Amber77 0:b24f1c7238b2 452 control_Brake = 1;
Amber77 0:b24f1c7238b2 453 control_StopRun = 0;
Amber77 0:b24f1c7238b2 454 // control_Y1_CW_CCW = 1; //y往負的方向轉，Robot往正的方向移動
Amber77 0:b24f1c7238b2 455 // control_Y2_CW_CCW = 0;
Amber77 0:b24f1c7238b2 456 control_Y1_CW_CCW = 0;
Amber77 0:b24f1c7238b2 457 control_Y2_CW_CCW = 1;
Amber77 0:b24f1c7238b2 458 // control_angVel_Value = 150;
Amber77 0:b24f1c7238b2 459 control_PWM_Value=0.7;
Amber77 0:b24f1c7238b2 460 }
Amber77 0:b24f1c7238b2 461 else if(RunTime > 75 && RunTime <= 85)
Amber77 0:b24f1c7238b2 462 {
Amber77 0:b24f1c7238b2 463 control_Brake = 1;
Amber77 0:b24f1c7238b2 464 control_StopRun = 0;
Amber77 0:b24f1c7238b2 465 // control_Y1_CW_CCW = 1; //y往負的方向轉，Robot往正的方向移動
Amber77 0:b24f1c7238b2 466 // control_Y2_CW_CCW = 0;
Amber77 0:b24f1c7238b2 467 control_Y1_CW_CCW = 0;
Amber77 0:b24f1c7238b2 468 control_Y2_CW_CCW = 1;
Amber77 0:b24f1c7238b2 469 // control_angVel_Value = 150;
Amber77 0:b24f1c7238b2 470 control_PWM_Value=0.8;
Amber77 0:b24f1c7238b2 471 }
Amber77 0:b24f1c7238b2 472 else if(RunTime > 85 && RunTime <= 95)
Amber77 0:b24f1c7238b2 473 {
Amber77 0:b24f1c7238b2 474 control_Brake = 1;
Amber77 0:b24f1c7238b2 475 control_StopRun = 0;
Amber77 0:b24f1c7238b2 476 // control_Y1_CW_CCW = 1; //y往負的方向轉，Robot往正的方向移動
Amber77 0:b24f1c7238b2 477 // control_Y2_CW_CCW = 0;
Amber77 0:b24f1c7238b2 478 control_Y1_CW_CCW = 0;
Amber77 0:b24f1c7238b2 479 control_Y2_CW_CCW = 1;
Amber77 0:b24f1c7238b2 480 // control_angVel_Value = 150;
Amber77 0:b24f1c7238b2 481 control_PWM_Value=0.9;
Amber77 0:b24f1c7238b2 482 }
Amber77 0:b24f1c7238b2 483 else if(RunTime > 95 && RunTime <= 105)
Amber77 0:b24f1c7238b2 484 {
Amber77 0:b24f1c7238b2 485 control_Brake = 1;
Amber77 0:b24f1c7238b2 486 control_StopRun = 0;
Amber77 0:b24f1c7238b2 487 // control_Y1_CW_CCW = 1; //y往負的方向轉，Robot往正的方向移動
Amber77 0:b24f1c7238b2 488 // control_Y2_CW_CCW = 0;
Amber77 0:b24f1c7238b2 489 control_Y1_CW_CCW = 0;
Amber77 0:b24f1c7238b2 490 control_Y2_CW_CCW = 1;
Amber77 0:b24f1c7238b2 491 // control_angVel_Value = 150;
Amber77 0:b24f1c7238b2 492 control_PWM_Value=1;
Amber77 0:b24f1c7238b2 493 }
Amber77 0:b24f1c7238b2 494 else if(RunTime > 105 && RunTime <= 115)
Amber77 0:b24f1c7238b2 495 {
Amber77 0:b24f1c7238b2 496 control_Brake = 1;
Amber77 0:b24f1c7238b2 497 control_StopRun = 0;
Amber77 0:b24f1c7238b2 498 // control_Y1_CW_CCW = 1; //y往負的方向轉，Robot往正的方向移動
Amber77 0:b24f1c7238b2 499 // control_Y2_CW_CCW = 0;
Amber77 0:b24f1c7238b2 500 control_Y1_CW_CCW = 0;
Amber77 0:b24f1c7238b2 501 control_Y2_CW_CCW = 1;
Amber77 0:b24f1c7238b2 502 // control_angVel_Value = 150;
Amber77 0:b24f1c7238b2 503 control_PWM_Value=0.5;
Amber77 0:b24f1c7238b2 504 }
Amber77 0:b24f1c7238b2 505 else if(RunTime > 115 && RunTime <= 125)
Amber77 0:b24f1c7238b2 506 {
Amber77 0:b24f1c7238b2 507 control_Brake = 1;
Amber77 0:b24f1c7238b2 508 control_StopRun = 0;
Amber77 0:b24f1c7238b2 509 // control_Y1_CW_CCW = 1; //y往負的方向轉，Robot往正的方向移動
Amber77 0:b24f1c7238b2 510 // control_Y2_CW_CCW = 0;
Amber77 0:b24f1c7238b2 511 control_Y1_CW_CCW = 0;
Amber77 0:b24f1c7238b2 512 control_Y2_CW_CCW = 1;
Amber77 0:b24f1c7238b2 513 // control_angVel_Value = 150;
Amber77 0:b24f1c7238b2 514 control_PWM_Value=0.2;
Amber77 0:b24f1c7238b2 515 }
Amber77 0:b24f1c7238b2 516 else if(RunTime > 125)
Amber77 0:b24f1c7238b2 517 {
Amber77 0:b24f1c7238b2 518 control_Brake = 1;
Amber77 0:b24f1c7238b2 519 control_StopRun = 0;
Amber77 0:b24f1c7238b2 520 // control_Y1_CW_CCW = 1; //y往負的方向轉，Robot往正的方向移動
Amber77 0:b24f1c7238b2 521 // control_Y2_CW_CCW = 0;
Amber77 0:b24f1c7238b2 522 control_Y1_CW_CCW = 0;
Amber77 0:b24f1c7238b2 523 control_Y2_CW_CCW = 1;
Amber77 0:b24f1c7238b2 524 // control_angVel_Value = 150;
Amber77 0:b24f1c7238b2 525 control_PWM_Value=0;
Amber77 0:b24f1c7238b2 526 }
Amber77 0:b24f1c7238b2 527
Amber77 0:b24f1c7238b2 528 // control_PWM_Value = PWM_commmand_transformation(control_angVel_Value);
Amber77 0:b24f1c7238b2 529
Amber77 0:b24f1c7238b2 530 if( (RunTime-lastTime) > 0.1 )
Amber77 0:b24f1c7238b2 531 {
Amber77 0:b24f1c7238b2 532 index_measure++;
Amber77 0:b24f1c7238b2 533 lastTime = RunTime;
Amber77 0:b24f1c7238b2 534 }
Amber77 0:b24f1c7238b2 535 if( index_measure >= 1 )
Amber77 0:b24f1c7238b2 536 {
Amber77 0:b24f1c7238b2 537 //Measure_motor_1_average = Measure_motor_1_total/(index_measure);
Amber77 0:b24f1c7238b2 538 pc.printf("RunTime : %.3f ", RunTime );
Amber77 0:b24f1c7238b2 539 pc.printf(", control_PWM_Value : %.3f\n", control_PWM_Value);
Amber77 0:b24f1c7238b2 540 pc.printf("Now_angularVelocity1 : %.3f",Now_angularVelocity1);
Amber77 0:b24f1c7238b2 541 pc.printf(", Now_angularVelocity3 : %.3f\n",Now_angularVelocity3);
Amber77 0:b24f1c7238b2 542 index_measure = 0;
Amber77 0:b24f1c7238b2 543 Measure_motor_1_total = 0;
Amber77 0:b24f1c7238b2 544 if(fp == NULL)
Amber77 0:b24f1c7238b2 545 {
Amber77 0:b24f1c7238b2 546 error("Could not open file for write\n");
Amber77 0:b24f1c7238b2 547 }
Amber77 0:b24f1c7238b2 548 // fprintf(fp,"%.3f,%.3f,%.3f\n", RunTime,Now_angularVelocity,control_PWM_Value);
Amber77 0:b24f1c7238b2 549 fprintf(fp,"%.3f,%.3f,%.3f,%.3f\n", RunTime, control_PWM_Value, Now_angularVelocity1, Now_angularVelocity3);
Amber77 0:b24f1c7238b2 550 }
Amber77 0:b24f1c7238b2 551
Amber77 0:b24f1c7238b2 552 Motor_run(control_PWM_Value,control_Brake,control_StopRun, control_Y1_CW_CCW, control_Y2_CW_CCW);
Amber77 0:b24f1c7238b2 553 if(!mybutton)
Amber77 0:b24f1c7238b2 554 {
Amber77 0:b24f1c7238b2 555 StopRun.write(1);
Amber77 0:b24f1c7238b2 556 Brake.write(1);
Amber77 0:b24f1c7238b2 557 Y1_CW_CCW.write(1); // clockwise:0 counterclockwise:1
Amber77 0:b24f1c7238b2 558 Y2_CW_CCW.write(0);
Amber77 0:b24f1c7238b2 559 Y_PWM.write(0); // clockwise:0 counterclockwise:1
Amber77 0:b24f1c7238b2 560 }
Amber77 0:b24f1c7238b2 561 }
Amber77 0:b24f1c7238b2 562 fclose(fp);
Amber77 0:b24f1c7238b2 563 }