Motor.h Motor.cpp main.cpp

Dependencies:   BMX055 Motor Way

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

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 }