main.cpp

00001 //Gyro test04
00002 // Akidukidensi AE-GYRO-SMD
00003 #include "mbed.h"
00004 #include "TextLCD0420.h"
00005 
00006 #define ON 1
00007 #define OFF 0
00008 #define Gy1_offset 0.01
00009 
00010 DigitalOut mled0(LED1);
00011 DigitalOut mled1(LED2);
00012 AnalogIn gy1_adc(p16);
00013 AnalogIn gy2_adc(p17);
00014 
00015 TextLCD lcd(p24, p25, p26, p27, p28, p29, p30,20,4); // rs, rw, e, d0, d1, d2, d3
00016 
00017 double round3(double x){
00018   double y;
00019   y=double(int((x+0.005)*100))/100;
00020   return(y);
00021 }
00022 
00023 int main() {
00024   float gy1_data;
00025   float gy1_center;
00026   float gy1_sgm=0;
00027   float gy1_wk0=0,gy1_wk1=0;
00028   float gy1_avg=0,gy1_sum;
00029   int i,gy1_lc=9,gy1_lc_old=0;
00030   
00031   lcd.cls();
00032   lcd.locate(0,0);
00033   lcd.printf("*** Gyro test04 ****\n");
00034 
00035   gy1_center=0;
00036   for(i=0;i<300;i++){
00037     gy1_center=gy1_center+round3(gy1_adc.read()*3.3);
00038     wait(0.001);
00039   }
00040   gy1_center=round3(gy1_center/300);
00041   
00042   while(1){
00043     gy1_sum=0;
00044     for(i=0;i<20;i++){
00045       gy1_sum=gy1_sum+round3(gy1_adc.read()*3.3);
00046       wait(0.005);
00047     }
00048     gy1_avg=round3(gy1_sum/20);
00049     
00050     gy1_wk1=round3((gy1_avg-gy1_center)*1000/5);
00051     if(gy1_wk1 < 0){
00052       gy1_wk1=gy1_wk1*1.1;
00053     }
00054     gy1_sgm=round3(gy1_sgm+(gy1_wk0+gy1_wk1)*0.1/2);
00055     gy1_wk0=gy1_wk1;
00056     
00057     if(gy1_sgm > 90){
00058       gy1_sgm=90;  
00059     }
00060     else if(gy1_sgm < -90){
00061       gy1_sgm=-90;
00062     }
00063     
00064     lcd.locate(0,1);
00065     lcd.printf("%3.3f %3.3f d:%3.0f",gy1_avg,gy1_center,gy1_sgm);
00066     lcd.locate(0,2);
00067     lcd.printf("--------+--------");
00068     gy1_lc=(gy1_sgm-5)/10+9;
00069     if(gy1_lc >20){
00070       gy1_lc=19;
00071     }
00072     if(gy1_lc <0){
00073       gy1_lc=0;
00074     }
00075     lcd.locate(gy1_lc_old,3);
00076     lcd.printf(" ");
00077     lcd.locate(gy1_lc,3);
00078     lcd.printf("*");
00079     gy1_lc_old=gy1_lc;
00080   }//while    
00081 }//main      
00082