main.cpp

00001 #include "mbed.h"
00002 #include "ColorSensor.h"
00003 #include "TextLCD.h"
00004 #include "Servo.h"
00005 #include "HMC6352.h"
00006 
00007 #include "main.h"
00008 
00009 //センサの数
00010 #define COLOR_NUM 3
00011 
00012 //閾値
00013 #define R_THR 65
00014 #define G_THR 65
00015 #define B_THR 65
00016 #define PINR_THR 1000
00017 
00018 enum{
00019     GO,
00020     STOP
00021 };
00022 
00023 //TextLCD lcd(p30, p29, p28, p27, p26, p25, TextLCD::LCD20x4); // rs, e, d4-d7
00024 
00025 ColorSensor color0(p20, p17, p18, p19, 5);
00026 ColorSensor color1(p16, p13, p14, p15, 5);
00027 ColorSensor color2(p12, p9, p10, p11, 5);
00028 
00029 Servo servoR(p23);
00030 Servo servoL(p24);
00031 
00032 //ColorSensor color = ColorSensor();
00033 //Serial pc(USBTX, USBRX); // tx, rx
00034 
00035 DigitalOut led[4] = {LED1,LED2,LED3,LED4};
00036 DigitalOut air[2] = {p21,p22};
00037 
00038 Timer color_t[3];
00039 Timer ping_t;
00040 Ticker interrupt0;
00041 
00042 double proportional = 0;
00043 uint16_t com_val = 0;
00044     unsigned redp[COLOR_NUM], greenp[COLOR_NUM], bluep[COLOR_NUM];
00045 
00046 #define Convert_dekaruto(a) ((a+100.0)/2.0/100.0)
00047 
00048 #define STRAIGHT 0.6;
00049 #define SPIN 0.4;
00050 
00051 //#define STRAIGHT 0.0;
00052 //#define SPIN 1.0;
00053 
00054 
00055 
00056 void tic_sensor()
00057 {
00058     Ultrasonic();
00059     
00060     color0.checkRGB(redp[0],greenp[0],bluep[0]);
00061     color1.checkRGB(redp[1],greenp[1],bluep[1]);
00062     color2.checkRGB(redp[2],greenp[2],bluep[2]);
00063     
00064     //pc.printf("R:%d G:%d B:%d\n", redp[0], greenp[0], bluep[0]);
00065 }
00066 
00067 
00068 void move(int vl,int vs){
00069     double fut_R,fut_L,true_vs;
00070     
00071     true_vs = abs(vs)/SPIN;
00072     
00073     if(true_vs > 40){
00074         vl = 0;
00075         vs = 100*(vs/abs(vs));
00076     }
00077     
00078     fut_R = Convert_dekaruto((vl + vs));
00079     fut_L = Convert_dekaruto((vl - vs)/**1.4*/);
00080     
00081     fut_R = Convert_dekaruto(-50);
00082     fut_L = Convert_dekaruto(-50);
00083  
00084     servoR = fut_R;
00085     servoL = fut_L;
00086        
00087     //printf("R:%lf   L:%lf\n",fut_R,fut_L);
00088     //printf("R:%d   L:%d\n",(vl + vs),-(vl - vs));
00089 }
00090 
00091 /*void PidUpdate()
00092 {   
00093     inputPID = (((int)(compass.sample() - ((207.0) * 10.0) + 5400.0) % 3600) / 10.0);
00094     //printf("%lf\n",inputPID);      
00095 }*/
00096 
00097 double vsOut(){
00098     double vs;
00099     vs = ((inputPID / 360 - 0.5) * 2 * 100) * -1;
00100     vs = vs * 8;
00101     
00102     if(vs/abs(vs) < 0){
00103         //vs *= 1.3;   
00104     }
00105     
00106     if(abs(vs) > 90)vs = 90*(vs/abs(vs));
00107     if(abs(vs) < 25) vs = 25*(vs/abs(vs));
00108     
00109     return vs;
00110 }
00111 
00112 
00113 int ping_button(int ping,int button){
00114     static int continue_flag = 0;
00115     static int change_flag = 0;
00116 
00117     if(continue_flag == 0){
00118         if(ping <= PINR_THR){
00119             ping_t.start();
00120             continue_flag = 1;
00121         }
00122     }    
00123     
00124     if(continue_flag == 1){
00125         //agatterutoki
00126         if(ping <= PINR_THR){
00127             if(change_flag == 0){
00128                 if(ping_t.read_ms() >= 300){
00129                     button = !button;
00130                     change_flag = 1;
00131                 }
00132             }
00133         }
00134         //tatisagari
00135         if(ping >= (PINR_THR+200)){
00136             ping_t.stop();
00137             ping_t.reset();
00138             continue_flag = 0;
00139             change_flag = 0;
00140         }       
00141     }
00142     return button;    
00143 }
00144 
00145 
00146 //青から赤に反応したらジャンプ
00147 //pidUpdataのせいでカラーセンサが動かない!!
00148 int main()
00149 {
00150     //wait(3);
00151     
00152     timer2.start();
00153     ping_t.start();
00154     
00155         
00156     unsigned R, G, B;
00157     unsigned threshold, t[3] = {0} ;
00158     double color_sum;
00159     int vl;
00160     double vs;
00161     uint8_t button, state=0;
00162     
00163     //pc.baud(115200);
00164     air[0] = 0; air[1] = 1;
00165   
00166     
00167     compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
00168     //pidUpdata.attach(&PidUpdate, PID_CYCLE);
00169     interrupt0.attach(&tic_sensor, 0.05/*sec*/);//0.04sec以上じゃないとmain動かない
00170   
00171   int e=0;
00172   while(1)
00173   {
00174     
00175     
00176     //lcd.cls();
00177     //pc.printf("R:%d G:%d B:%d\n", redp[0], greenp[0], bluep[0]);
00178     /*
00179     lcd.locate(0,0);
00180     lcd.printf("R:%d G:%d B:%d", redp[0][0], greenp[0][0], bluep[0][0]);
00181     lcd.locate(0,1);
00182     lcd.printf("R:%d G:%d B:%d", redp[1][0], greenp[1][0], bluep[1][0]);
00183      */
00184     pc.printf("%d\n", ultrasonicVal[0]);
00185     
00186     
00187     threshold = 0;
00188     for(int i=0; i<3; i++){
00189         if(bluep[i] >= B_THR){
00190             color_t[i].reset();
00191             color_t[i].start();
00192             t[i] = 0;
00193         }else if(redp[i] >= R_THR){
00194             t[i] = color_t[i].read_ms();
00195         }else{
00196             t[i] = 0;
00197         }
00198  
00199         if((t[i] <= 500) && (t[i] != 0)){
00200             threshold++;
00201         }
00202     }
00203     
00204     
00205     
00206     if(threshold >= 1){
00207             led[0] = 1; led[1] = 1; led[2] = 1; led[3] = 1;
00208             air[0] = 1;  air[1] = 0;
00209             wait(0.4);
00210             led[0] = 0; led[1] = 0; led[2] = 0; led[3] = 0;
00211             air[0] = 0;  air[1] = 1;
00212             wait(0.5);
00213     }else{
00214             led[0] = 0; led[1] = 0; led[2] = 0; led[3] = 0;
00215     }
00216     
00217     
00218     
00219     button = ping_button(ultrasonicVal[0],button);
00220         
00221     if(button){
00222         state = GO;
00223     }else{
00224         state = STOP;
00225     }
00226     
00227     
00228     
00229     
00230     
00231     if(state == GO){
00232         vl = -90;//led[0] = 1; led[1] = 1;
00233     }else if(state == STOP){
00234         vl = 0;//led[0] = 0; led[1] = 0;
00235     }
00236     
00237     vl = vl      * STRAIGHT ;
00238     vs = vsOut() * SPIN     ;
00239     
00240     vl *= 0.5;
00241     vs *= 0.3;
00242         
00243     move(vl,(int)vs); 
00244   }
00245 }