Motor.h Motor.cpp main.cpp
Dependencies: BMX055 Motor Way
main.cpp
00001 //本命 00002 #include "mbed.h" 00003 #include "motor.h" 00004 #include "BMX055.h" 00005 #include "way.h" 00006 00007 BMX055 bmx(D0, D1); 00008 DigitalIn button(D2); 00009 DigitalOut motor_mode(D9); 00010 DigitalOut LED(D10); 00011 Motor motorL(D5, D6); 00012 Motor motorR(D3, D4); 00013 InterruptIn pg1(D12); 00014 InterruptIn pg2(D11); 00015 AnalogIn reflectorFL(A6); 00016 AnalogIn reflectorFM(A3); 00017 AnalogIn reflectorFR(A2); 00018 AnalogIn reflectorBL(A1); 00019 AnalogIn reflectorBR(A0); 00020 00021 Thread thread_trace; 00022 Thread thread_motor; 00023 Thread sensor_thread; 00024 00025 const float INTVAL_REFLECTOR(0.01);//0.01); 00026 const float INTVAL_MOTOR(0.25); 00027 00028 const float pi = 3.14159265359; 00029 const float gear_ratio = 1 / 38.2; 00030 const float tire_size = 0.057 / 2; 00031 00032 float dist = 0.0; 00033 float x = 0.0; 00034 float y = 0.0; 00035 float fast_rpm = 0; 00036 float standard_rpm = 0; 00037 float slow_rpm = 0; 00038 00039 bool lock = false; 00040 00041 void count_pg1() 00042 { 00043 motorL.count(); 00044 } 00045 00046 void count_pg2() 00047 { 00048 motorR.count(); 00049 } 00050 00051 void line_trace() 00052 { 00053 //const float fast_rpm = 0; 00054 //const float standard_rpm = 0; 00055 //const float slow_rpm = 0; 00056 static float gap_pre = 0.0; 00057 const float KP = 850; 00058 const float KD = 0; 00059 const float K = 0.01; 00060 const int L = 0; 00061 const int R = 1; 00062 00063 int flag; 00064 00065 motorL.Set_phase(L); 00066 motorR.Set_phase(R); 00067 00068 float target_azimuth = 0.0; 00069 float current_azimuth = 0.0; 00070 float curvature = 0.0; 00071 float tread = 0.097; 00072 00073 fast_rpm = 1200; 00074 standard_rpm = 1200; 00075 slow_rpm = 1200; 00076 00077 while (1) 00078 { 00079 if(reflectorFL > 0.1) 00080 { 00081 flag = L; 00082 } 00083 if(reflectorFR > 0.1) 00084 { 00085 flag = R; 00086 } 00087 if( reflectorFM<=0.4 && reflectorBL<=0.1 && reflectorBR<=0.1 )//&& reflectorFL<=0.1 && reflectorFR<=0.1 ) 00088 { 00089 if( flag == L ) 00090 { 00091 motorL.Set_phase(L); 00092 motorL.Set_target(0); 00093 motorR.Set_phase(R); 00094 motorR.Set_target(slow_rpm); 00095 //printf("turnL\n"); 00096 } 00097 else 00098 { 00099 motorL.Set_phase(L); 00100 motorL.Set_target(slow_rpm); 00101 motorR.Set_phase(R); 00102 motorR.Set_target(0); 00103 //printf("turnR\n"); 00104 } 00105 } 00106 else 00107 { 00108 if( reflectorFM > 0.4 ) 00109 { 00110 motorL.Set_phase(L); 00111 motorL.Set_target(fast_rpm); 00112 motorR.Set_phase(R); 00113 motorR.Set_target(fast_rpm); 00114 } 00115 else 00116 { 00117 if( reflectorBL.read() >= reflectorBR.read() ) 00118 { 00119 float voltage_gap = reflectorBL.read() - reflectorBR.read(); 00120 motorR.Set_phase(R); 00121 motorR.Set_target(standard_rpm + KP * voltage_gap + KD * (voltage_gap - gap_pre)); 00122 motorL.Set_phase(L); 00123 motorL.Set_target(standard_rpm - KP * voltage_gap + KD * (voltage_gap - gap_pre)); 00124 gap_pre = voltage_gap; 00125 flag = L; 00126 } 00127 else 00128 { 00129 float voltage_gap = reflectorBR.read() - reflectorBL.read(); 00130 motorR.Set_phase(R); 00131 motorR.Set_target(standard_rpm - KP * voltage_gap + KD * (voltage_gap - gap_pre)); 00132 motorL.Set_phase(L); 00133 motorL.Set_target(standard_rpm + KP * voltage_gap + KD * (voltage_gap - gap_pre)); 00134 gap_pre = voltage_gap; 00135 flag = R; 00136 } 00137 } 00138 } 00139 00140 float gap_min = 10000.0; 00141 int i_min = 0; 00142 for (int i = 0; i < 478; i++) 00143 { 00144 float dist_gap = dist - way[i][2]; 00145 if (dist_gap < gap_min) 00146 { 00147 gap_min = dist_gap; 00148 i_min = i; 00149 } 00150 } 00151 00152 float l = sqrt( pow((x-way[i_min][0]), 2) + pow((y-way[i_min][1]), 2) ); 00153 float dv = K*l; 00154 float drpm = dv*60 / ( gear_ratio * 2 * pi * tire_size ); 00155 00156 motorL.Set_target( standard_rpm + drpm ); 00157 motorR.Set_target( standard_rpm - drpm ); 00158 */ 00159 00160 printf("%f %f %f %f %f flag: %d \n", reflectorFL.read(), reflectorFM.read(), reflectorFR.read(), reflectorBL.read(), reflectorBR.read(), flag); 00161 wait(INTVAL_REFLECTOR); 00162 } 00163 } 00164 00165 void odometry() 00166 { 00167 static float v_pre = 0.0; 00168 static float x_dot_pre = 0.0; 00169 static float y_dot_pre = 0.0; 00170 static float dist_count = 0.0; 00171 00172 float vl = motorL.Get_rpm() / 60 * gear_ratio * 2 * pi * tire_size; 00173 float vr = motorR.Get_rpm() / 60 * gear_ratio * 2 * pi * tire_size; 00174 float v = (vl + vr) / 2; 00175 float x_dot = v * cos(bmx.get_azimuth_machineframe()); 00176 float y_dot = v * sin(bmx.get_azimuth_machineframe()); 00177 00178 dist += (v + v_pre) * INTVAL_MOTOR / 2; 00179 dist_count += (v + v_pre) * INTVAL_MOTOR / 2; 00180 x += (x_dot + x_dot_pre) * INTVAL_MOTOR / 2; 00181 y += (y_dot + y_dot_pre) * INTVAL_MOTOR / 2; 00182 v_pre = v; 00183 x_dot_pre = x; 00184 y_dot_pre = y; 00185 00186 if (dist<=2000) 00187 { 00188 fast_rpm = 2500; 00189 standard_rpm = 2000; 00190 slow_rpm = 1200; 00191 } 00192 else if(dist<=2400) 00193 { 00194 fast_rpm = 2000; 00195 standard_rpm = 1500; 00196 slow_rpm = 1200; 00197 } 00198 else if(dist<=4400) 00199 { 00200 fast_rpm = 2500; 00201 standard_rpm = 2000; 00202 slow_rpm = 1200; 00203 } 00204 else 00205 { 00206 fast_rpm = 1500; 00207 standard_rpm = 1200; 00208 slow_rpm = 1200; 00209 } 00210 00211 //printf("%f\n", dist); 00212 00213 if (dist_count >= 200) 00214 { 00215 LED = !LED; 00216 dist_count -= 200; 00217 } 00218 } 00219 00220 void control_motor() 00221 { 00222 while (1) 00223 { 00224 motorL.drive(); 00225 motorR.drive(); 00226 odometry(); 00227 wait(INTVAL_MOTOR); 00228 } 00229 } 00230 00231 void update() 00232 { 00233 while (true) 00234 { 00235 if (!lock) 00236 { 00237 bmx.Update_posture(); 00238 } 00239 } 00240 } 00241 00242 int main() 00243 { 00244 motor_mode = 1; 00245 LED = 1; 00246 00247 pg1.rise(&count_pg1); 00248 pg1.fall(&count_pg1); 00249 pg2.rise(&count_pg2); 00250 pg2.fall(&count_pg2); 00251 00252 while (true) 00253 { 00254 if (button == 0) 00255 { 00256 wait(1); 00257 break; 00258 } 00259 } 00260 00261 bmx.mag_calibration(button); 00262 00263 printf("準備完了\n"); 00264 while (true) 00265 { 00266 if (button == 0) 00267 { 00268 wait(1); 00269 break; 00270 } 00271 } 00272 00273 bmx.set_initial_mag(); 00274 bmx.set_initial_acc(); 00275 00276 sensor_thread.start(callback(update)); 00277 thread_trace.start(callback(line_trace)); 00278 thread_motor.start(callback(control_motor)); 00279 00280 while (true) 00281 { 00282 if (button == 0) 00283 { 00284 bmx.posture.w = 1; 00285 bmx.posture.x = 0; 00286 bmx.posture.y = 0; 00287 bmx.posture.z = 0; 00288 lock = true; 00289 wait(0.1); 00290 bmx.set_initial_mag(); 00291 bmx.set_initial_acc(); 00292 lock = false; 00293 } 00294 } 00295 }
Generated on Tue Aug 2 2022 21:19:22 by
1.7.2