oooga

Dependencies:   HMC6352 Servo TextLCD mbed

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 //歩くだけ
00002 #include "mbed.h"
00003 #include "ColorSensor.h"
00004 #include "TextLCD.h"
00005 #include "Servo.h"
00006 #include "HMC6352.h"
00007 
00008 #include "main.h"
00009 
00010 //センサの数
00011 #define COLOR_NUM 3
00012 
00013 //閾値
00014 #define R_THR 60
00015 #define G_THR 60
00016 #define B_THR 60
00017 
00018 
00019 //TextLCD lcd(p30, p29, p28, p27, p26, p25, TextLCD::LCD20x4); // rs, e, d4-d7
00020 
00021 ColorSensor color0(p20, p17, p18, p19, 10);
00022 ColorSensor color1(p16, p13, p14, p15, 10);
00023 ColorSensor color2(p12, p9, p10, p11, 10);
00024 
00025 Servo servoR(p23);
00026 Servo servoL(p24);
00027 
00028 //ColorSensor color = ColorSensor();
00029 //Serial pc(USBTX, USBRX); // tx, rx
00030 
00031 DigitalOut led[4] = {LED1,LED2,LED3,LED4};
00032 DigitalOut air[2] = {p21,p22};
00033 
00034 Timer color_t[3];
00035 
00036 
00037 double proportional = 0;
00038 uint16_t com_val = 0;
00039 
00040 
00041 #define Convert_dekaruto(a) ((a+100.0)/2.0/100.0)
00042 
00043 #define STRAIGHT 0.6;
00044 #define SPIN 0.4;
00045 
00046 //#define STRAIGHT 0.0;
00047 //#define SPIN 1.0;
00048 
00049 
00050 void move(int vl,int vs){
00051     double fut_R,fut_L,true_vs;
00052     
00053     true_vs = abs(vs)/SPIN;
00054     
00055     if(true_vs > 40){
00056         vl = 0;
00057         vs = 100*(vs/abs(vs));
00058     }
00059     
00060     fut_R = Convert_dekaruto((vl + vs));
00061     fut_L = Convert_dekaruto((vl - vs)/**1.4*/);
00062     
00063     fut_R = Convert_dekaruto(-50);
00064     fut_L = Convert_dekaruto(-50);
00065  
00066     servoR = fut_R;
00067     servoL = fut_L;
00068        
00069     //printf("R:%lf   L:%lf\n",fut_R,fut_L);
00070     //printf("R:%d   L:%d\n",(vl + vs),-(vl - vs));
00071 }
00072 
00073 void PidUpdate()
00074 {   
00075     inputPID = (((int)(compass.sample() - ((207.0) * 10.0) + 5400.0) % 3600) / 10.0);
00076     //printf("%lf\n",inputPID);      
00077 }
00078 
00079 double vsOut(){
00080     double vs;
00081     vs = ((inputPID / 360 - 0.5) * 2 * 100) * -1;
00082     vs = vs * 8;
00083     
00084     if(vs/abs(vs) < 0){
00085         //vs *= 1.3;   
00086     }
00087     
00088     if(abs(vs) > 90)vs = 90*(vs/abs(vs));
00089     if(abs(vs) < 25) vs = 25*(vs/abs(vs));
00090     
00091     return vs;
00092 }
00093 
00094 
00095 
00096 //青から赤に反応したらジャンプ
00097 //pidUpdataのせいでカラーセンサが動かない!!
00098 int main()
00099 {
00100     wait(3);
00101 
00102     unsigned R, G, B;
00103     unsigned threshold, t[3] = {0} ;
00104     double color_sum;
00105     unsigned redp[COLOR_NUM], greenp[COLOR_NUM], bluep[COLOR_NUM];
00106     //pc.baud(115200);
00107     air[0] = 0; air[1] = 1;
00108   
00109     int vl;
00110     double vs;
00111     compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
00112     pidUpdata.attach(&PidUpdate, PID_CYCLE);
00113   
00114   
00115   while(1)
00116   {
00117      
00118     color0.checkRGB(redp[0],greenp[0],bluep[0]);
00119     color1.checkRGB(redp[1],greenp[1],bluep[1]);
00120     color2.checkRGB(redp[2],greenp[2],bluep[2]);
00121     
00122     //lcd.cls();
00123     //pc.printf("R:%d G:%d B:%d\n", redp[0], greenp[0], bluep[0]);
00124     /*
00125     lcd.locate(0,0);
00126     lcd.printf("R:%d G:%d B:%d", redp[0][0], greenp[0][0], bluep[0][0]);
00127     lcd.locate(0,1);
00128     lcd.printf("R:%d G:%d B:%d", redp[1][0], greenp[1][0], bluep[1][0]);
00129      */
00130      
00131 
00132      
00133     
00134     threshold = 0;
00135     for(int i=0; i<3; i++){
00136         if(bluep[i] >= B_THR){
00137             color_t[i].reset();
00138             color_t[i].start();
00139             t[i] = 0;
00140         }else if(redp[i] >= R_THR){
00141             t[i] = color_t[i].read_ms();
00142         }else{
00143             t[i] = 0;
00144         }
00145  
00146         if((t[i] <= 500) && (t[i] != 0)){
00147             threshold++;
00148         }
00149     }
00150     
00151     
00152     
00153     if(threshold >= 1){
00154             led[0] = 1; led[1] = 1; led[2] = 1; led[3] = 1;
00155             air[0] = 1;  air[1] = 0;
00156             wait(0.8);
00157             led[0] = 0; led[1] = 0; led[2] = 0; led[3] = 0;
00158             air[0] = 0;  air[1] = 1;
00159             wait(0.5);
00160     }else{
00161             led[0] = 0; led[1] = 0; led[2] = 0; led[3] = 0;
00162     }
00163     
00164     
00165     
00166     vl = -90;
00167     //vl = 0;
00168     
00169     vl = vl      * STRAIGHT ;
00170     vs = vsOut() * SPIN     ;
00171     
00172     vl *= 0.5;
00173     vs *= 0.3;
00174         
00175     move(vl,(int)vs); 
00176   }
00177 }